small algorithm optimizations

Author: vagtsiats

bindings/python/examples/floating_frame/franka_mpc.py

@@ -18,6 +18,7 @@
 from scipy.spatial.transform import Rotation as R
 import unittest
 import os
+from ttictoc import tic, toc
 
 
 np.set_printoptions(linewidth=np.inf)
@@ -140,7 +141,7 @@ def publish_horizon(self, joint_state_msg):
 
 time.sleep(2)
 
-Ns = 20 # number of nodes
+Ns = 10 # number of nodes
 tf = 0.2 # final time
 dt = tf/Ns
 
@@ -399,8 +400,10 @@ def euler(x, xdot, dt):
 
         cartesian_task.setReference(pose_ref)
 
-        ocp.update(x0, u0)
+        tic()
         success = solver.solve(x0, u0)
+        b = toc()
+        print(b)
 
         x0 = solver.getStateSolution()
         u0 = solver.getControlSolution()

bindings/python/examples/floating_frame/franka_mpc_real.py

@@ -15,6 +15,7 @@
 import unittest
 import os
 from utils import *
+from ttictoc import tic, toc 
 
 
 np.set_printoptions(linewidth=np.inf)
@@ -258,10 +259,10 @@ def publish(self, joint_state_msg:JointState):
 
 
 print("Initing solver...")
-solver = swSQP(ocp)
+solver = pysot.swSQP(ocp)
 solver.getOptions().max_iters = 10
 solver.getOptions().verbose = False
-solver.getOptions().line_search_strategy = 2
+solver.getOptions().line_search_strategy = 0
 solver.getOptions().beta = 1e-2
 solver.getOptions().min_abs_delta_solution = 1e-3
 solver.init()
@@ -287,7 +288,7 @@ def publish(self, joint_state_msg:JointState):
 
 
 print("-"*100)
-
+# b = toc()
 msg = JointState()
 msg.name = model.getJointNames()
 # print(msg.name)
@@ -296,8 +297,10 @@ def publish(self, joint_state_msg:JointState):
         cartesian_task.setReference(node.pose_ref.copy())
 
         # x0[0] = node.state
-        ocp.update(x0, u0)
+        # tic()
         success = solver.solve(x0, u0)
+        # b = toc()
+        # print(b)
 
         x0 = solver.getStateSolution()
         u0 = solver.getControlSolution()

include/OpenSoT/solvers/swSQP.h

@@ -192,6 +192,9 @@ class swSQP{
     std::vector<Eigen::VectorXd> _x0, _u0;
     std::vector<Eigen::VectorXd> _x0_candidate, _u0_candidate;
 
+    Eigen::VectorXd _dx0; //initial delta state constraint (_dx0 = 0)
+    
+
 };
 
 }

src/oc/EulerVector.cpp

@@ -19,17 +19,16 @@ EulerVector::EulerVector(const XBot::ModelInterface& robot,
     _dt(dt)
 {
 
+    _Fx = Eigen::MatrixXd::Identity(_dX.getOutputSize(), _dX.getOutputSize());
+    _Fu = Eigen::MatrixXd::Identity(_dU.getOutputSize(), _dU.getOutputSize()) * _dt;
+
     _W.setIdentity(dX.getOutputSize(), dX.getOutputSize());
 
     update();
 }
 
 void EulerVector::_update()
 {  
-    _Fx = Eigen::MatrixXd::Identity(_dX.getOutputSize(), _dX.getOutputSize());
-
-    _Fu = Eigen::MatrixXd::Identity(_dU.getOutputSize(), _dU.getOutputSize()) * _dt;
-
     _dXnext = _Fx * _dX + _Fu * _dU + (_Xk.getValue() + _Uk.getValue()*_dt - _Xk_1.getValue());
 
     _A = _dXnext.getM();

src/solvers/swSQP.cpp

@@ -109,8 +109,7 @@ bool swSQP::solve(const std::vector<Eigen::VectorXd>& x0, const std::vector<Eige
         ddefect_dw[i] = _ocp->stage(i)->stage_ddefect_dw();
     }
 
-    Eigen::VectorXd dx0(_A[0].cols()); //initial delta state constraint (dx0 = 0)
-    dx0.setZero();
+    _dx0.setZero();
 
     for(unsigned int iter = 1; iter <= _opt.max_iters; ++iter)
     {
@@ -122,22 +121,22 @@ bool swSQP::solve(const std::vector<Eigen::VectorXd>& x0, const std::vector<Eige
         auto iter_start = std::chrono::high_resolution_clock::now();
 
         // relinarize and update qp
-        _ocp->update(_x0_candidate, _u0_candidate);
         linearize();
 
         // solve
-        if (!_qp_solver->solve(dx0))
+        if (!_qp_solver->solve(_dx0))
         {
             std::cout<< "nosolve"<< std::endl;
             return false;
         }
 
         // first update
         step(_stats.alpha);
-        _ocp->update(_x0_candidate, _u0_candidate);
+        
 
         while(_opt.line_search_strategy!=0 && _stats.alpha >= _opt.alpha_min)
         {
+            _ocp->update(_x0_candidate, _u0_candidate);
             if((this->*ls_function)())
             {
                 _stats.line_search_accepted=true;
@@ -146,10 +145,15 @@ bool swSQP::solve(const std::vector<Eigen::VectorXd>& x0, const std::vector<Eige
             _stats.alpha /= 2.;
             _stats.line_search_iters++;
             step(_stats.alpha);
-            _ocp->update(_x0_candidate, _u0_candidate);
 
 
         }
+        if (_opt.line_search_strategy==0)
+                _ocp->update(_x0_candidate, _u0_candidate);
+
+        _x0 = _x0_candidate;
+        _u0 = _u0_candidate;
+        
 
         // check break criteria on QP solution
         if (break_criteria())
@@ -160,10 +164,7 @@ bool swSQP::solve(const std::vector<Eigen::VectorXd>& x0, const std::vector<Eige
             break;
         } 
 
-        _x0 = _x0_candidate;
-        _u0 = _u0_candidate;
 
-        _ocp->update(_x0, _u0); //TODO I am not sure about this update, maybe its not needed
         _prev_cost = _ocp->cost();
         _prev_defect = _ocp->dynamics_defect();
         _prev_viol = _ocp->constraint_violation();
@@ -307,6 +308,7 @@ void swSQP::init()
 
             _qp_solver->setStageDynamics(k, A, B, b);
         }
+        _dx0.setZero(_A[0].cols()); //initial delta state constraint (_dx0 = 0)
 
         // --- Cost (always) ---
         _H.push_back(Eigen::MatrixXd(_ocp->stage(k)->stack->getStack()[0]->getA().cols(), _ocp->stage(k)->stack->getStack()[0]->getA().cols()));