store data in static variables before wrapping into nanobind

roahmlab · Sep 10, 2024 · e547d24 · e547d24
1 parent 686307c
commit e547d24
Show file tree

Hide file tree

Showing 5 changed files with 19 additions and 10 deletions.
diff --git a/Examples/Kinova/ArmourDiscrete/include/KinovaPybindWrapper.h b/Examples/Kinova/ArmourDiscrete/include/KinovaPybindWrapper.h
@@ -91,6 +91,10 @@ class KinovaPybindWrapper {
     SmartPtr<KinovaOptimizer> mynlp;
     SmartPtr<IpoptApplication> app;
 
+    // results
+    VecX solution;
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> trajInfo;
+
     // Flags to check if the parameters are set
     bool set_obstacles_check = false;
     bool set_ipopt_parameters_check = false;

diff --git a/Examples/Kinova/ArmourDiscrete/src/KinovaPybindWrapper.cpp b/Examples/Kinova/ArmourDiscrete/src/KinovaPybindWrapper.cpp
@@ -157,9 +157,9 @@ nb::tuple KinovaPybindWrapper::optimize() {
     }
 
     // Define initial guess
-    VecX z0 = 0.5 * (atp.q0 + qdes);
+    // VecX z0 = 0.5 * (atp.q0 + qdes);
     // VecX z0 = atp.q0;
-    // VecX z0 = qdes;
+    VecX z0 = qdes;
 
     // Initialize Kinova optimizer
     try {
@@ -211,9 +211,10 @@ nb::tuple KinovaPybindWrapper::optimize() {
     set_trajectory_parameters_check = false;
     set_target_check = false;
     has_optimized = mynlp->ifFeasible;
+    solution = mynlp->solution;
 
     const size_t shape_ptr[] = {model.nv};
-    auto result = nb::ndarray<nb::numpy, const double>(mynlp->solution.data(),
+    auto result = nb::ndarray<nb::numpy, const double>(solution.data(),
                                                        1,
                                                        shape_ptr,
                                                        nb::handle());
@@ -261,7 +262,7 @@ nb::ndarray<nb::numpy, double, nb::shape<2, -1>> KinovaPybindWrapper::analyze_so
         throw std::runtime_error("Error evaluating the solution on a finer time discretization! Check previous error message!");
     }
 
-    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> trajInfo(N_simulate, 4 * NUM_JOINTS + 1);
+    trajInfo.resize(N_simulate, 4 * NUM_JOINTS + 1);
     for (int i = 0; i < N_simulate; i++) {
         for (int j = 0; j < NUM_JOINTS; j++) {
             trajInfo(i, j) = testnlp->trajPtr_->q(i)(j);

diff --git a/Examples/Kinova/WaitrDiscrete/include/KinovaWaitrPybindWrapper.h b/Examples/Kinova/WaitrDiscrete/include/KinovaWaitrPybindWrapper.h
@@ -78,8 +78,6 @@ class KinovaWaitrPybindWrapper {
     // robot model
     Model model;
 
-    int actual_model_nq = 0;
-
     // obstacle information
     int num_obstacles = 0;
     std::vector<Vec3> boxCenters;
@@ -106,6 +104,11 @@ class KinovaWaitrPybindWrapper {
     SmartPtr<KinovaWaitrOptimizer> mynlp;
     SmartPtr<IpoptApplication> app;
 
+    // results
+    VecX solution;
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> trajInfo;
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> contactInfo;
+
     // Flags to check if the parameters are set
     bool set_obstacles_check = false;
     bool set_contact_surface_parameters_check = false;

diff --git a/Examples/Kinova/WaitrDiscrete/src/KinovaWaitrPybindWrapper.cpp b/Examples/Kinova/WaitrDiscrete/src/KinovaWaitrPybindWrapper.cpp
@@ -265,9 +265,10 @@ nb::tuple KinovaWaitrPybindWrapper::optimize() {
     set_trajectory_parameters_check = false;
     set_target_check = false;
     has_optimized = mynlp->ifFeasible;
+    solution = mynlp->solution;
 
     const size_t shape_ptr[] = {NUM_JOINTS};
-    auto result = nb::ndarray<nb::numpy, const double>(mynlp->solution.data(),
+    auto result = nb::ndarray<nb::numpy, const double>(solution.data(),
                                                        1,
                                                        shape_ptr,
                                                        nb::handle());
@@ -316,7 +317,7 @@ nb::tuple KinovaWaitrPybindWrapper::analyze_solution() {
         throw std::runtime_error("Error evaluating the solution on a finer time discretization! Check previous error message!");
     }
 
-    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> trajInfo(N_simulate, 4 * NUM_JOINTS + 1);
+    trajInfo.resize(N_simulate, 4 * NUM_JOINTS + 1);
     for (int i = 0; i < N_simulate; i++) {
         for (int j = 0; j < NUM_JOINTS; j++) {
             trajInfo(i, j) = testnlp->trajPtr_->q(i)(j);
@@ -328,7 +329,7 @@ nb::tuple KinovaWaitrPybindWrapper::analyze_solution() {
         trajInfo(i, 4 * NUM_JOINTS) = testnlp->trajPtr_->tspan(i);
     }
 
-    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> contactInfo(N_simulate, 3);
+    contactInfo.resize(N_simulate, 3);
     for (int i = 0; i < N_simulate; i++) {
         const Vec3& rotation_torque = testnlp->idPtr_->lambda(i).head(3);
         const Vec3& translation_force = testnlp->idPtr_->lambda(i).tail(3);

diff --git a/Examples/Kinova/WaypointPlanning/WaypointPlanningPybindWrapper.h b/Examples/Kinova/WaypointPlanning/WaypointPlanningPybindWrapper.h
@@ -167,7 +167,7 @@ class WaypointPlanningPybindWrapper {
             throw std::runtime_error("Failed to find a solution");
         }
 
-        ss->simplifySolution();
+        // ss->simplifySolution();
         auto sol = ss->getSolutionPath();
         sol.interpolate();