feat: add updateQ function in osqp interface (autowarefoundation#214)

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
badai-nguyen · Dec 22, 2021 · 1a20fcb · 1a20fcb
1 parent b4e006b
commit 1a20fcb
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/common/osqp_interface/src/osqp_interface.cpp b/common/osqp_interface/src/osqp_interface.cpp
@@ -88,6 +88,13 @@ void OSQPInterface::updateA(const Eigen::MatrixXd & A_new)
   return;
 }
 
+void OSQPInterface::updateQ(const std::vector<double> & q_new)
+{
+  std::vector<double> q_tmp(q_new.begin(), q_new.end());
+  double * q_dyn = q_tmp.data();
+  osqp_update_lin_cost(m_work, q_dyn);
+}
+
 void OSQPInterface::updateL(const std::vector<double> & l_new)
 {
   std::vector<double> l_tmp(l_new.begin(), l_new.end());