From e547d242010d064aaede205550b3241eba9538bf Mon Sep 17 00:00:00 2001 From: Cfather Date: Tue, 10 Sep 2024 17:35:01 +0000 Subject: [PATCH] store data in static variables before wrapping into nanobind --- .../Kinova/ArmourDiscrete/include/KinovaPybindWrapper.h | 4 ++++ .../Kinova/ArmourDiscrete/src/KinovaPybindWrapper.cpp | 9 +++++---- .../WaitrDiscrete/include/KinovaWaitrPybindWrapper.h | 7 +++++-- .../WaitrDiscrete/src/KinovaWaitrPybindWrapper.cpp | 7 ++++--- .../WaypointPlanning/WaypointPlanningPybindWrapper.h | 2 +- 5 files changed, 19 insertions(+), 10 deletions(-) diff --git a/Examples/Kinova/ArmourDiscrete/include/KinovaPybindWrapper.h b/Examples/Kinova/ArmourDiscrete/include/KinovaPybindWrapper.h index 991300d3..9bd5a8e5 100644 --- a/Examples/Kinova/ArmourDiscrete/include/KinovaPybindWrapper.h +++ b/Examples/Kinova/ArmourDiscrete/include/KinovaPybindWrapper.h @@ -91,6 +91,10 @@ class KinovaPybindWrapper { SmartPtr mynlp; SmartPtr app; + // results + VecX solution; + Eigen::Matrix 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 index 71662457..fd7f4598 100644 --- 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(mynlp->solution.data(), + auto result = nb::ndarray(solution.data(), 1, shape_ptr, nb::handle()); @@ -261,7 +262,7 @@ nb::ndarray> KinovaPybindWrapper::analyze_so throw std::runtime_error("Error evaluating the solution on a finer time discretization! Check previous error message!"); } - Eigen::Matrix 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 index cf9656a2..1af02565 100644 --- 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 boxCenters; @@ -106,6 +104,11 @@ class KinovaWaitrPybindWrapper { SmartPtr mynlp; SmartPtr app; + // results + VecX solution; + Eigen::Matrix trajInfo; + Eigen::Matrix 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 index f0ec0b50..d4dccda6 100644 --- 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(mynlp->solution.data(), + auto result = nb::ndarray(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 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 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 index 88835aaf..826563a5 100644 --- 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();