coverage: improve coverage

Author: Meow404

core/test/core_test.cc

@@ -223,40 +223,6 @@ TEST_F(C3CartpoleTest, UpdateCostMatrix) {
   }
 }
 
-// Test if user can update the LCS for the C3 problem
-TEST_F(C3CartpoleTest, UpdateLCSTest) {
-  std::vector<drake::solvers::LinearEqualityConstraint*>
-      pre_dynamic_constraints = pOpt->GetDynamicConstraints();
-  vector<MatrixXd> pre_Al(N);
-  for (int i = 0; i < N; ++i) {
-    pre_Al[i] = pre_dynamic_constraints[i]->GetDenseA();
-  }
-
-  // Dynamics
-  vector<MatrixXd> An(N, MatrixXd::Identity(n, n));
-  vector<MatrixXd> Bn(N, MatrixXd::Identity(n, k));
-  vector<MatrixXd> Dn(N, MatrixXd::Identity(n, m));
-  vector<VectorXd> dn(N, VectorXd::Ones(n));
-
-  // Complimentary constraints
-  vector<MatrixXd> En(N, MatrixXd::Identity(m, n));
-  vector<MatrixXd> Fn(N, MatrixXd::Identity(m, m));
-  vector<VectorXd> cn(N, VectorXd::Ones(m));
-  vector<MatrixXd> Hn(N, MatrixXd::Identity(m, k));
-
-  LCS TestSystem(An, Bn, Dn, dn, En, Fn, Hn, cn, dt);
-
-  pOpt->UpdateLCS(TestSystem);
-
-  std::vector<drake::solvers::LinearEqualityConstraint*>
-      pst_dynamic_constraints = pOpt->GetDynamicConstraints();
-  for (int i = 0; i < N; ++i) {
-    // Linear Equality A matrix should be updated
-    MatrixXd pst_Al = pst_dynamic_constraints[i]->GetDenseA();
-    EXPECT_EQ(pre_Al[i].isApprox(pst_Al), false);
-  }
-}
-
 class C3CartpoleTestParameterizedScalingLCSTest
     : public C3CartpoleTest,
       public ::testing::WithParamInterface<std::tuple<bool, bool>> {};
@@ -368,6 +334,46 @@ class C3CartpoleTypedTest : public testing::Test, public C3CartpoleProblem {
 using projection_types = ::testing::Types<C3QP, C3MIQP, C3Plus>;
 TYPED_TEST_SUITE(C3CartpoleTypedTest, projection_types);
 
+// Test if user can update the LCS for the C3 problem
+TYPED_TEST(C3CartpoleTypedTest, UpdateLCSTest) {
+  c3::C3* pOpt = this->pOpt.get();
+  auto dt = this->dt;
+  std::vector<drake::solvers::LinearEqualityConstraint*>
+      pre_dynamic_constraints = pOpt->GetDynamicConstraints();
+  auto& N = this->N;
+  auto n = this->n;
+  auto k = this->k;
+  auto m = this->m;
+  vector<MatrixXd> pre_Al(N);
+  for (int i = 0; i < N; ++i) {
+    pre_Al[i] = pre_dynamic_constraints[i]->GetDenseA();
+  }
+
+  // Dynamics
+  vector<MatrixXd> An(N, MatrixXd::Identity(n, n));
+  vector<MatrixXd> Bn(N, MatrixXd::Identity(n, k));
+  vector<MatrixXd> Dn(N, MatrixXd::Identity(n, m));
+  vector<VectorXd> dn(N, VectorXd::Ones(n));
+
+  // Complimentary constraints
+  vector<MatrixXd> En(N, MatrixXd::Identity(m, n));
+  vector<MatrixXd> Fn(N, MatrixXd::Identity(m, m));
+  vector<VectorXd> cn(N, VectorXd::Ones(m));
+  vector<MatrixXd> Hn(N, MatrixXd::Identity(m, k));
+
+  LCS TestSystem(An, Bn, Dn, dn, En, Fn, Hn, cn, dt);
+
+  pOpt->UpdateLCS(TestSystem);
+
+  std::vector<drake::solvers::LinearEqualityConstraint*>
+      pst_dynamic_constraints = pOpt->GetDynamicConstraints();
+  for (int i = 0; i < N; ++i) {
+    // Linear Equality A matrix should be updated
+    MatrixXd pst_Al = pst_dynamic_constraints[i]->GetDenseA();
+    EXPECT_EQ(pre_Al[i].isApprox(pst_Al), false);
+  }
+}
+
 // Test the cartpole example
 // This test will take some time to complete ~30s
 TYPED_TEST(C3CartpoleTypedTest, End2EndCartpoleTest) {

examples/resources/cartpole_softwalls/c3Plus_controller_cartpole_options.yaml

@@ -0,0 +1,51 @@
+projection_type: "MIQP"
+
+solve_time_filter_alpha: 0.0
+#set to 0 to publish as fast as possible
+publish_frequency: 100
+state_prediction_joints: []
+  # - name : "CartSlider"
+  #   max_acceleration : 10
+
+lcs_factory_options:
+  #options are 'stewart_and_trinkle' or 'anitescu'
+  # contact_model : 'stewart_and_trinkle'
+  contact_model: "frictionless_spring"
+  num_friction_directions: 0
+  num_contacts: 2
+  spring_stiffness: 100
+  mu: [0, 0]
+  N: 5
+  dt: 0.01
+
+c3_options:
+  warm_start: false
+  end_on_qp_step: true
+  scale_lcs: false
+
+  num_threads: 5
+  delta_option: 0
+
+  M: 1000
+  admm_iter: 10
+
+  gamma: 1.0
+  rho_scale: 2 #matrix scaling
+  w_Q: 1
+  w_R: 1 #Penalty on all decision variables, assuming scalar
+  w_G: 1 #Penalty on all decision variables, assuming scalar
+  w_U: 1 #State Tracking Error, assuming diagonal
+  q_vector: [10, 2, 1, 1] #Penalty on efforts, assuming diagonal
+  r_vector: [1] #Penalty on matching projected variables
+  g_x: [0, 0, 0, 0]
+  g_gamma: [1, 1]
+  g_lambda_n: []
+  g_lambda_t: []
+  g_lambda: [1, 1]
+  g_u: [0] #Penalty on matching the QP variables
+  u_x: [0, 0, 0, 0]
+  u_gamma: [10000, 10000]
+  u_lambda_n: []
+  u_lambda_t: []
+  u_lambda: [1, 1]
+  u_u: [0]