Merge pull request #24 from roahmlab/ik_motion_dev

Solve Inverse Kinematics Motion + Nanobind bug fix

Examples/Digit/src/DigitMultipleStepOptimizer.cpp

@@ -257,7 +257,8 @@ bool DigitMultipleStepOptimizer::get_bounds_info(
     }
 
     for ( Index i = 0; i < stepOptVec_.size(); i++ ) {
-        std::cout << "gait " << i << " bounds infomation:" << std::endl;
+        if(display_info) std::cout << "gait " << i << " bounds infomation:" << std::endl;
+
         stepOptVec_[i]->get_bounds_info(n_local[i], x_l + n_position[i], x_u + n_position[i], 
                                         m_local[i], g_l + m_position[i], g_u + m_position[i]);
     }
@@ -266,15 +267,17 @@ bool DigitMultipleStepOptimizer::get_bounds_info(
         const Index start_pos = m_position[stepOptVec_.size() + i];
         const Index end_pos = m_position[stepOptVec_.size() + i + 1];
 
-        if (i == periodConsVec_.size() - 1 && periodConsVec_.size() == stepOptVec_.size()) {
-            std::cout << "gait " << i << " - " << 0 << " continuous constraint: "
-                      << periodConsVec_[i]->m 
-                      << " [" << start_pos << " " << end_pos << "]" << std::endl;
-        }
-        else {
-            std::cout << "gait " << i << " - " << i + 1 << " continuous constraint: "
-                      << periodConsVec_[i]->m 
-                      << " [" << start_pos << " " << end_pos << "]" << std::endl;
+        if (display_info) {
+            if (i == periodConsVec_.size() - 1 && periodConsVec_.size() == stepOptVec_.size()) {
+                std::cout << "gait " << i << " - " << 0 << " continuous constraint: "
+                          << periodConsVec_[i]->m 
+                          << " [" << start_pos << " " << end_pos << "]" << std::endl;
+            }
+            else {
+                std::cout << "gait " << i << " - " << i + 1 << " continuous constraint: "
+                          << periodConsVec_[i]->m 
+                          << " [" << start_pos << " " << end_pos << "]" << std::endl;
+            }
         }
 
         periodConsVec_[i]->compute_bounds();

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;

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);

Examples/Kinova/CMakeLists.txt

@@ -1,6 +1,6 @@
 # Kinova 
 add_library(Kinovalib SHARED
-    InverseKinematics/KinovaIKSolver.cpp
+    InverseKinematics/src/KinovaIKSolver.cpp
     ArmourDiscrete/src/KinovaOptimizer.cpp
     ArmourDiscrete/src/KinovaCustomizedConstraints.cpp
     WaitrDiscrete/src/KinovaWaitrOptimizer.cpp
@@ -10,7 +10,7 @@ add_library(Kinovalib SHARED
 
 target_include_directories(Kinovalib PUBLIC
     ${CMAKE_CURRENT_SOURCE_DIR}
-    ${CMAKE_CURRENT_SOURCE_DIR}/InverseKinematics
+    ${CMAKE_CURRENT_SOURCE_DIR}/InverseKinematics/include
     ${CMAKE_CURRENT_SOURCE_DIR}/ArmourDiscrete/include
     ${CMAKE_CURRENT_SOURCE_DIR}/WaitrDiscrete/include
     ${CMAKE_CURRENT_SOURCE_DIR}/SystemIdentification/DataFilter/include
@@ -33,10 +33,8 @@ target_link_libraries(Kinovalib PUBLIC
 ###
 add_executable(KinovaIK_example 
     InverseKinematics/KinovaIKExample.cpp)
-
 target_include_directories(KinovaIK_example PUBLIC
     InverseKinematics)
-
 target_link_libraries(KinovaIK_example PUBLIC
     trajlib 
     IDlib
@@ -47,17 +45,34 @@ target_link_libraries(KinovaIK_example PUBLIC
     coinhsl
     ${BOOST_LIBRARIES} 
     pinocchio::pinocchio)
-
 target_compile_options(KinovaIK_example PUBLIC
     ${PINOCCHIO_FLAGS})
 
+
+###
+add_executable(KinovaIKMotion_example 
+    InverseKinematics/KinovaIKMotionExample.cpp)
+target_include_directories(KinovaIKMotion_example PUBLIC
+    InverseKinematics/include)
+target_link_libraries(KinovaIKMotion_example PUBLIC
+    trajlib 
+    IDlib
+    Conslib
+    Optlib
+    Kinovalib
+    ipopt
+    coinhsl
+    ${BOOST_LIBRARIES} 
+    pinocchio::pinocchio)
+target_compile_options(KinovaIKMotion_example PUBLIC
+    ${PINOCCHIO_FLAGS})
+
+
 ###
 add_executable(KinovaHLP_example 
     WaypointPlanning/WaypointPlanningExample.cpp)
-
 target_include_directories(KinovaHLP_example PUBLIC
     ArmourDiscrete/include)
-
 target_link_libraries(KinovaHLP_example PUBLIC
     trajlib 
     IDlib
@@ -66,18 +81,15 @@ target_link_libraries(KinovaHLP_example PUBLIC
     ${BOOST_LIBRARIES} 
     pinocchio::pinocchio
     ompl)
-
 target_compile_options(KinovaHLP_example PUBLIC
     ${PINOCCHIO_FLAGS})
 
 
 ###
 add_executable(Kinova_example 
     ArmourDiscrete/KinovaExample.cpp)
-
 target_include_directories(Kinova_example PUBLIC
     ArmourDiscrete/include)
-
 target_link_libraries(Kinova_example PUBLIC
     trajlib 
     IDlib
@@ -88,17 +100,15 @@ target_link_libraries(Kinova_example PUBLIC
     coinhsl
     ${BOOST_LIBRARIES} 
     pinocchio::pinocchio)
-
 target_compile_options(Kinova_example PUBLIC
     ${PINOCCHIO_FLAGS})
 
+
 ###
 add_executable(KinovaWaitr_example 
     WaitrDiscrete/KinovaWaitrExample.cpp)
-
 target_include_directories(KinovaWaitr_example PUBLIC
     WaitrDiscrete/include)
-
 target_link_libraries(KinovaWaitr_example PUBLIC
     trajlib 
     IDlib
@@ -109,21 +119,17 @@ target_link_libraries(KinovaWaitr_example PUBLIC
     coinhsl
     ${BOOST_LIBRARIES} 
     pinocchio::pinocchio)
-
 target_compile_options(KinovaWaitr_example PUBLIC
     ${PINOCCHIO_FLAGS})
 
 
 ###   
 add_executable(KinovaFilter_example 
     SystemIdentification/DataFilter/KinovaAccelerationEstimator.cpp)
-
-
 target_include_directories(KinovaFilter_example PUBLIC
     systemIdentification/DataFilter/include
     SystemIdentification/ExcitingTrajectories/include
     SystemIdentification/IterativeSystemIdentification/include)
-
 target_link_libraries(KinovaFilter_example PUBLIC
     trajlib 
     IDlib
@@ -133,17 +139,17 @@ target_link_libraries(KinovaFilter_example PUBLIC
     ipopt
     coinhsl
     ${BOOST_LIBRARIES})
+target_compile_options(KinovaFilter_example PUBLIC
+    ${PINOCCHIO_FLAGS})
 
 
 ###
 add_executable(KinovaExciting_example 
     SystemIdentification/ExcitingTrajectories/KinovaRegressorExample.cpp)
-
 target_include_directories(KinovaExciting_example PUBLIC
     ystemIdentification/DataFilter/include
     SystemIdentification/ExcitingTrajectories/include
     SystemIdentification/IterativeSystemIdentification/include)
-
 target_link_libraries(KinovaExciting_example PUBLIC
     trajlib 
     IDlib
@@ -154,19 +160,36 @@ target_link_libraries(KinovaExciting_example PUBLIC
     coinhsl
     ${BOOST_LIBRARIES} 
     pinocchio::pinocchio)
-
 target_compile_options(KinovaExciting_example PUBLIC
     ${PINOCCHIO_FLAGS})
 
 
 ### Python bindings 
+nanobind_add_module(KinovaIKMotion_nanobind 
+    NB_SHARED LTO
+    InverseKinematics/KinovaIKMotionPybind.cpp
+    InverseKinematics/src/KinovaIKMotionPybindWrapper.cpp)
+target_include_directories(KinovaIKMotion_nanobind PUBLIC
+    InverseKinematics/include)
+target_link_libraries(KinovaIKMotion_nanobind PUBLIC
+    trajlib 
+    IDlib
+    Conslib
+    Optlib
+    Kinovalib
+    ${BOOST_LIBRARIES} 
+    pinocchio::pinocchio
+    ${PYTHON_LIBRARIES})
+nanobind_compile_options(KinovaIKMotion_nanobind PUBLIC
+    ${PINOCCHIO_FLAGS})
+set_property(TARGET KinovaIKMotion_nanobind PROPERTY POSITION_INDEPENDENT_CODE ON)
+
+
 nanobind_add_module(KinovaHLP_nanobind 
     NB_SHARED LTO
     WaypointPlanning/WaypointPlanningPybind.cpp)
-
 target_include_directories(KinovaHLP_nanobind PUBLIC
     ArmourDiscrete/include)
-
 target_link_libraries(KinovaHLP_nanobind PUBLIC
     trajlib 
     IDlib
@@ -177,20 +200,17 @@ target_link_libraries(KinovaHLP_nanobind PUBLIC
     pinocchio::pinocchio
     ompl
     ${PYTHON_LIBRARIES})
-
 nanobind_compile_options(KinovaHLP_nanobind PUBLIC
     ${PINOCCHIO_FLAGS})
-
 set_property(TARGET KinovaHLP_nanobind PROPERTY POSITION_INDEPENDENT_CODE ON)
 
+
 nanobind_add_module(oracle_nanobind 
     NB_SHARED LTO
     ArmourDiscrete/KinovaPybind.cpp
     ArmourDiscrete/src/KinovaPybindWrapper.cpp)
-
 target_include_directories(oracle_nanobind PUBLIC
     Kinova/ArmourDiscrete/include)
-
 target_link_libraries(oracle_nanobind PUBLIC
     trajlib 
     IDlib
@@ -202,19 +222,16 @@ target_link_libraries(oracle_nanobind PUBLIC
     ${BOOST_LIBRARIES} 
     pinocchio::pinocchio
     ${PYTHON_LIBRARIES})
-
 nanobind_compile_options(oracle_nanobind PUBLIC
     ${PINOCCHIO_FLAGS})
-
 set_property(TARGET oracle_nanobind PROPERTY POSITION_INDEPENDENT_CODE ON)
 
+
 nanobind_add_module(oracle_waitr_nanobind SHARED
     WaitrDiscrete/KinovaWaitrPybind.cpp
     WaitrDiscrete/src/KinovaWaitrPybindWrapper.cpp)
-
 target_include_directories(oracle_waitr_nanobind PUBLIC
     WaitrDiscrete/include)
-
 target_link_libraries(oracle_waitr_nanobind PUBLIC
     trajlib 
     IDlib
@@ -226,8 +243,6 @@ target_link_libraries(oracle_waitr_nanobind PUBLIC
     ${BOOST_LIBRARIES} 
     pinocchio::pinocchio
     ${PYTHON_LIBRARIES})
-
 nanobind_compile_options(oracle_waitr_nanobind PUBLIC
     ${PINOCCHIO_FLAGS})
-
 set_property(TARGET oracle_waitr_nanobind PROPERTY POSITION_INDEPENDENT_CODE ON)