diff --git a/Examples/Digit/src/DigitMultipleStepOptimizer.cpp b/Examples/Digit/src/DigitMultipleStepOptimizer.cpp index e8b67286..f8f5247e 100644 --- a/Examples/Digit/src/DigitMultipleStepOptimizer.cpp +++ b/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(); 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/CMakeLists.txt b/Examples/Kinova/CMakeLists.txt index 99812885..969d0d6e 100644 --- a/Examples/Kinova/CMakeLists.txt +++ b/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,7 +81,6 @@ target_link_libraries(KinovaHLP_example PUBLIC ${BOOST_LIBRARIES} pinocchio::pinocchio ompl) - target_compile_options(KinovaHLP_example PUBLIC ${PINOCCHIO_FLAGS}) @@ -74,10 +88,8 @@ target_compile_options(KinovaHLP_example PUBLIC ### 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,7 +119,6 @@ target_link_libraries(KinovaWaitr_example PUBLIC coinhsl ${BOOST_LIBRARIES} pinocchio::pinocchio) - target_compile_options(KinovaWaitr_example PUBLIC ${PINOCCHIO_FLAGS}) @@ -117,13 +126,10 @@ target_compile_options(KinovaWaitr_example PUBLIC ### 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) \ No newline at end of file diff --git a/Examples/Kinova/InverseKinematics/KinovaIKMotionExample.cpp b/Examples/Kinova/InverseKinematics/KinovaIKMotionExample.cpp new file mode 100644 index 00000000..d22e4b16 --- /dev/null +++ b/Examples/Kinova/InverseKinematics/KinovaIKMotionExample.cpp @@ -0,0 +1,104 @@ +#include "KinovaIKSolver.h" + +#include "pinocchio/parsers/urdf.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" + +using namespace RAPTOR; +using namespace Kinova; +using namespace Ipopt; + +int main() { + // Define robot model + const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; + + pinocchio::Model model; + pinocchio::urdf::buildModel(urdf_filename, model); + + // Compute forward kinematics at a random configuration first + ForwardKinematicsSolver fkSolver(&model); + + std::srand(std::time(nullptr)); + const Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq); + fkSolver.compute(0, model.nq, q); + Transform desiredTransform = fkSolver.getTransform(); + + const int N = 20; + const double step_size = 0.005; + + // Initialize Kinova optimizer + SmartPtr mynlp = new KinovaIKSolver(); + mynlp->constr_viol_tol = 1e-8; + mynlp->display_info = false; + + SmartPtr app = IpoptApplicationFactory(); + + app->Options()->SetNumericValue("tol", 1e-4); + app->Options()->SetNumericValue("constr_viol_tol", 1e-6); + app->Options()->SetNumericValue("max_wall_time", 0.2); + app->Options()->SetIntegerValue("print_level", 0); + app->Options()->SetIntegerValue("max_iter", 20); + app->Options()->SetStringValue("mu_strategy", "monotone"); + app->Options()->SetStringValue("linear_solver", "ma86"); + app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); + if (mynlp->enable_hessian) { + app->Options()->SetStringValue("hessian_approximation", "exact"); + } + else { + app->Options()->SetStringValue("hessian_approximation", "limited-memory"); + } + + // For gradient checking + // app->Options()->SetStringValue("output_file", "ipopt.out"); + // app->Options()->SetStringValue("derivative_test", "second-order"); + // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7); + // app->Options()->SetNumericValue("derivative_test_tol", 1e-4); + + // Initialize the IpoptApplication and process the options + ApplicationReturnStatus status; + status = app->Initialize(); + if( status != Solve_Succeeded ) { + throw std::runtime_error("Error during initialization of optimization!"); + } + + // Define initial guess + Eigen::VectorXd z = q; + + for (int i = 0; i < N; i++) { + try { + mynlp->reset(); + mynlp->set_parameters(z, + model, + desiredTransform); + } + catch (std::exception& e) { + std::cerr << e.what() << std::endl; + throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); + } + + // Run ipopt to solve the optimization problem + double solve_time = 0; + try { + auto start = std::chrono::high_resolution_clock::now(); + + // Ask Ipopt to solve the problem + status = app->OptimizeTNLP(mynlp); + + auto end = std::chrono::high_resolution_clock::now(); + solve_time = std::chrono::duration_cast(end - start).count(); + std::cout << "End effector configuration " << i << std::endl; + std::cout << " Solve time: " << solve_time << " microseconds." << std::endl; + std::cout << " Constraint violation: " << mynlp->final_constr_violation << std::endl; + } + catch (std::exception& e) { + throw std::runtime_error("Error solving optimization problem! Check previous error message!"); + } + + // End effector moves forward at each iteration + desiredTransform = desiredTransform * Transform(Eigen::Vector3d(0, 0, step_size)); + + // Update initial guess + z = mynlp->solution; + } + + return 0; +} diff --git a/Examples/Kinova/InverseKinematics/KinovaIKMotionPybind.cpp b/Examples/Kinova/InverseKinematics/KinovaIKMotionPybind.cpp new file mode 100644 index 00000000..5f697638 --- /dev/null +++ b/Examples/Kinova/InverseKinematics/KinovaIKMotionPybind.cpp @@ -0,0 +1,18 @@ +#include + +#include "KinovaIKMotionPybindWrapper.h" + +namespace nb = nanobind; + +using namespace RAPTOR; +using namespace Kinova; + +NB_MODULE(KinovaIKMotion_nanobind, m) { + m.doc() = "nanobind KinovaIKMotion_nanobind plugin"; + + nb::class_(m, "KinovaIKMotionPybindWrapper") + .def(nb::init()) + .def("set_desired_endeffector_transforms", &KinovaIKMotionPybindWrapper::set_desired_endeffector_transforms) + .def("set_ipopt_parameters", &KinovaIKMotionPybindWrapper::set_ipopt_parameters) + .def("solve", &KinovaIKMotionPybindWrapper::solve); +} \ No newline at end of file diff --git a/Examples/Kinova/InverseKinematics/README.md b/Examples/Kinova/InverseKinematics/README.md new file mode 100644 index 00000000..699b940f --- /dev/null +++ b/Examples/Kinova/InverseKinematics/README.md @@ -0,0 +1,7 @@ +# Kinova Inverse Kinematics Example + +`KinovaIKExample.cpp` provides the simplest example of solving an inverse kinematics problem on Kinova. +Both analytical gradient and hessian are provided to Ipopt. + +`KinovaIKMotionExample.cpp` provides an example of solving a series of inverse kinematics problems, +so that the end effector goes forward for 10 cm along its z axis. \ No newline at end of file diff --git a/Examples/Kinova/InverseKinematics/include/KinovaIKMotionPybindWrapper.h b/Examples/Kinova/InverseKinematics/include/KinovaIKMotionPybindWrapper.h new file mode 100644 index 00000000..ea65ea39 --- /dev/null +++ b/Examples/Kinova/InverseKinematics/include/KinovaIKMotionPybindWrapper.h @@ -0,0 +1,73 @@ +#ifndef KINOVA_IK_MOTION_PYBIND_WRAPPER_H + +#include +#include +#include + +#include "KinovaIKSolver.h" + +#include "pinocchio/parsers/urdf.hpp" +#include "pinocchio/algorithm/joint-configuration.hpp" + +namespace RAPTOR { +namespace Kinova { + +namespace nb = nanobind; + +class KinovaIKMotionPybindWrapper { +public: + using Model = pinocchio::Model; + using Vec3 = Eigen::Vector3d; + using VecX = Eigen::VectorXd; + using Mat3 = Eigen::Matrix3d; + using MatX = Eigen::MatrixXd; + + using nb_1d_double = nb::ndarray, nb::c_contig, nb::device::cpu>; + using nb_2d_double = nb::ndarray, nb::c_contig, nb::device::cpu>; + + // Constructor + KinovaIKMotionPybindWrapper() = default; + + KinovaIKMotionPybindWrapper(const std::string urdf_filename, + const bool display_info); + + // Destructor + ~KinovaIKMotionPybindWrapper() = default; + + // Class methods + void set_desired_endeffector_transforms(const nb_2d_double desired_endeffector_transforms_inp); + + void set_ipopt_parameters(const double tol, + const double constr_viol_tol, + const double obj_scaling_factor, + const double max_wall_time, + const int print_level, + const std::string mu_strategy, + const std::string linear_solver, + const bool gradient_check); + + nb::tuple solve(); + + // Class members + // robot model + Model model; + + // end effector transform + Transform endT; + + SmartPtr mynlp; + SmartPtr app; + + std::vector desiredTransforms; + Eigen::Matrix solutions; + + // Flags to check if the parameters are set + bool set_transform_check = false; + bool set_ipopt_parameters_check = false; + bool has_optimized = false; +}; + +}; // namespace Kinova +}; // namespace RAPTOR + +#endif // KINOVA_IK_MOTION_PYBIND_WRAPPER_H \ No newline at end of file diff --git a/Examples/Kinova/InverseKinematics/KinovaIKSolver.h b/Examples/Kinova/InverseKinematics/include/KinovaIKSolver.h similarity index 98% rename from Examples/Kinova/InverseKinematics/KinovaIKSolver.h rename to Examples/Kinova/InverseKinematics/include/KinovaIKSolver.h index 2ffcd968..4590fccd 100644 --- a/Examples/Kinova/InverseKinematics/KinovaIKSolver.h +++ b/Examples/Kinova/InverseKinematics/include/KinovaIKSolver.h @@ -31,6 +31,7 @@ class KinovaIKSolver : public Optimizer { const VecX& x0_input, const Model& model_input, const Transform& desiredTransform_input, + const Transform endT_input = Transform(), Eigen::VectorXi jtype_input = Eigen::VectorXi(0) ); diff --git a/Examples/Kinova/InverseKinematics/src/KinovaIKMotionPybindWrapper.cpp b/Examples/Kinova/InverseKinematics/src/KinovaIKMotionPybindWrapper.cpp new file mode 100644 index 00000000..85178ee8 --- /dev/null +++ b/Examples/Kinova/InverseKinematics/src/KinovaIKMotionPybindWrapper.cpp @@ -0,0 +1,140 @@ +#include "KinovaIKMotionPybindWrapper.h" + +namespace RAPTOR { +namespace Kinova { + +KinovaIKMotionPybindWrapper::KinovaIKMotionPybindWrapper(const std::string urdf_filename, + const bool display_info) { + // Define robot model + pinocchio::urdf::buildModel(urdf_filename, model); + + mynlp = new KinovaIKSolver(); + mynlp->display_info = display_info; + + app = IpoptApplicationFactory(); + app->Options()->SetStringValue("hessian_approximation", "exact"); + + endT = Transform(Vec3(M_PI, 0, 0), Vec3(0, 0, -0.061525)); +} + +void KinovaIKMotionPybindWrapper::set_desired_endeffector_transforms(const nb_2d_double desired_endeffector_transforms_inp) { + if (desired_endeffector_transforms_inp.shape(1) != 12) { + throw std::invalid_argument("Input should be of shape (N, 12)!"); + } + + desiredTransforms.clear(); + for (size_t i = 0; i < desired_endeffector_transforms_inp.shape(0); i++) { + const size_t shape_ptr[] = {3, 4}; + Transform desiredTransform; + desiredTransform.p << desired_endeffector_transforms_inp(i, 0), + desired_endeffector_transforms_inp(i, 1), + desired_endeffector_transforms_inp(i, 2); + desiredTransform.R << desired_endeffector_transforms_inp(i, 3), desired_endeffector_transforms_inp(i, 4), desired_endeffector_transforms_inp(i, 5), + desired_endeffector_transforms_inp(i, 6), desired_endeffector_transforms_inp(i, 7), desired_endeffector_transforms_inp(i, 8), + desired_endeffector_transforms_inp(i, 9), desired_endeffector_transforms_inp(i, 10), desired_endeffector_transforms_inp(i, 11); + + // test if it is a valid rotation matrix + if (fabs(desiredTransform.R.determinant() - 1) > 1e-8) { + std::cerr << desiredTransform.R << std::endl; + throw std::invalid_argument("Input rotation matrix is not valid (Determinant not equal to 1)!"); + } + const MatX testR = desiredTransform.R * desiredTransform.R.transpose(); + if (!testR.isApprox(Mat3::Identity(), 1e-8)) { + throw std::invalid_argument("Input rotation matrix is not valid (Not orthogonal)!"); + } + + desiredTransforms.push_back(desiredTransform); + } + + set_transform_check = true; + has_optimized = false; +} + +void KinovaIKMotionPybindWrapper::set_ipopt_parameters(const double tol, + const double constr_viol_tol, + const double obj_scaling_factor, + const double max_wall_time, + const int print_level, + const std::string mu_strategy, + const std::string linear_solver, + const bool gradient_check) { + app->Options()->SetNumericValue("tol", tol); + app->Options()->SetNumericValue("constr_viol_tol", constr_viol_tol); + mynlp->constr_viol_tol = constr_viol_tol; + app->Options()->SetNumericValue("obj_scaling_factor", obj_scaling_factor); + app->Options()->SetNumericValue("max_wall_time", max_wall_time); + app->Options()->SetIntegerValue("print_level", print_level); + app->Options()->SetStringValue("mu_strategy", mu_strategy); + app->Options()->SetStringValue("linear_solver", linear_solver); + app->Options()->SetStringValue("ma57_automatic_scaling", "yes"); + + if (gradient_check) { + app->Options()->SetStringValue("output_file", "ipopt.out"); + app->Options()->SetStringValue("derivative_test", "second-order"); + app->Options()->SetNumericValue("derivative_test_perturbation", 1e-8); + app->Options()->SetNumericValue("derivative_test_tol", 1e-4); + } + + set_ipopt_parameters_check = true; + has_optimized = false; +} + +nb::tuple KinovaIKMotionPybindWrapper::solve() { + if (!set_transform_check || + !set_ipopt_parameters_check) { + throw std::runtime_error("parameters not set properly!"); + } + + solutions.resize(model.nv, desiredTransforms.size()); + + // Define initial guess + VecX z = VecX::Zero(model.nv); + + // Initialize the IpoptApplication and process the options + ApplicationReturnStatus status; + status = app->Initialize(); + if( status != Solve_Succeeded ) { + throw std::runtime_error("Error during initialization of optimization!"); + } + + int pid = 0; + for (const auto& desiredTransform: desiredTransforms) { + try { + mynlp->reset(); + mynlp->set_parameters(z, + model, + desiredTransform, + endT); + } + catch (std::exception& e) { + std::cerr << e.what() << std::endl; + throw std::runtime_error("Error initializing Ipopt class! Check previous error message!"); + } + + // Run ipopt to solve the optimization problem + try { + status = app->OptimizeTNLP(mynlp); + } + catch (std::exception& e) { + std::cerr << e.what() << std::endl; + throw std::runtime_error("Error solving optimization problem! Check previous error message!"); + } + + // Update initial guess + solutions.col(pid++) = mynlp->solution; + z = mynlp->solution; + + // Update feasible flag + has_optimized &= mynlp->ifFeasible; + } + + const size_t shape_ptr[] = {model.nv, desiredTransforms.size()}; + auto result = nb::ndarray(solutions.data(), + 2, + shape_ptr, + nb::handle()); + return nb::make_tuple(result, has_optimized); +} + +}; // namespace Kinova +}; // namespace RAPTOR diff --git a/Examples/Kinova/InverseKinematics/KinovaIKSolver.cpp b/Examples/Kinova/InverseKinematics/src/KinovaIKSolver.cpp similarity index 79% rename from Examples/Kinova/InverseKinematics/KinovaIKSolver.cpp rename to Examples/Kinova/InverseKinematics/src/KinovaIKSolver.cpp index f04ca6f8..2c23f7c4 100644 --- a/Examples/Kinova/InverseKinematics/KinovaIKSolver.cpp +++ b/Examples/Kinova/InverseKinematics/src/KinovaIKSolver.cpp @@ -19,6 +19,7 @@ bool KinovaIKSolver::set_parameters( const VecX& x0_input, const Model& model_input, const Transform& desiredTransform_input, + const Transform endT_input, Eigen::VectorXi jtype_input ) { @@ -36,11 +37,17 @@ bool KinovaIKSolver::set_parameters( Utils::deg2rad( Utils::initializeEigenVectorFromArray(JOINT_LIMITS_UPPER, NUM_JOINTS)); - // Joint limits - constraintsPtrVec_.push_back(std::make_unique(trajPtr_, - JOINT_LIMITS_LOWER_VEC, - JOINT_LIMITS_UPPER_VEC)); - constraintsNameVec_.push_back("joint limits"); + // In this instance, the decision variables are the joint angles of the robot directly. + // You can see that we inherited the get_bounds_info function from the Optimizer class + // and we are modifing it (x_lb and x_ub) to directly set the bounds for the joint limits. + // As a result, the following joint limits constraints are not needed. + // It is equivalent to turn on this constraint and remove the inherited get_bounds_info function. + + // // Joint limits + // constraintsPtrVec_.push_back(std::make_unique(trajPtr_, + // JOINT_LIMITS_LOWER_VEC, + // JOINT_LIMITS_UPPER_VEC)); + // constraintsNameVec_.push_back("joint limits"); // End effector kinematics constraints constraintsPtrVec_.push_back(std::make_unique(trajPtr_, @@ -48,7 +55,7 @@ bool KinovaIKSolver::set_parameters( model_input.nq, // the last joint 0, desiredTransform_input, - Transform(), + endT_input, jtype_input)); constraintsNameVec_.push_back("kinematics constraints"); @@ -114,6 +121,12 @@ bool KinovaIKSolver::get_bounds_info( x_l[i] = JOINT_LIMITS_LOWER[i]; } + // In this instance, the decision variables are the joint angles of the robot directly. + // You can see that we inherited the get_bounds_info function from the Optimizer class + // and we are modifing it (x_lb and x_ub) to directly set the bounds for the joint limits. + // As a result, the following joint limits constraints are not needed. + // It is equivalent to turn on this constraint and remove the inherited get_bounds_info function. + // upper bounds for( Index i = 0; i < n; i++ ) { x_u[i] = JOINT_LIMITS_UPPER[i]; @@ -148,14 +161,16 @@ bool KinovaIKSolver::get_bounds_info( } // report constraints distribution - std::cout << "Dimension of each constraints and their locations: \n"; - iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - std::cout << constraintsNameVec_[c] << ": "; - std::cout << constraintsPtrVec_[c]->m << " ["; - std::cout << iter << " "; - iter += constraintsPtrVec_[c]->m; - std::cout << iter << "]\n"; + if (display_info) { + std::cout << "Dimension of each constraints and their locations: \n"; + iter = 0; + for (Index c = 0; c < constraintsPtrVec_.size(); c++) { + std::cout << constraintsNameVec_[c] << ": "; + std::cout << constraintsPtrVec_[c]->m << " ["; + std::cout << iter << " "; + iter += constraintsPtrVec_[c]->m; + std::cout << iter << "]\n"; + } } return true; diff --git a/Examples/Kinova/SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp b/Examples/Kinova/SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp index cec11403..f12c59a0 100644 --- a/Examples/Kinova/SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp +++ b/Examples/Kinova/SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp @@ -141,14 +141,16 @@ bool DataFilterOptimizer::get_bounds_info( } // report constraints distribution - std::cout << "Dimension of each constraints and their locations: \n"; - iter = 0; - for (Index c = 0; c < constraintsPtrVec_.size(); c++) { - std::cout << constraintsNameVec_[c] << ": "; - std::cout << constraintsPtrVec_[c]->m << " ["; - std::cout << iter << " "; - iter += constraintsPtrVec_[c]->m; - std::cout << iter << "]\n"; + if (display_info) { + std::cout << "Dimension of each constraints and their locations: \n"; + iter = 0; + for (Index c = 0; c < constraintsPtrVec_.size(); c++) { + std::cout << constraintsNameVec_[c] << ": "; + std::cout << constraintsPtrVec_[c]->m << " ["; + std::cout << iter << " "; + iter += constraintsPtrVec_[c]->m; + std::cout << iter << "]\n"; + } } return true; diff --git a/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.cpp b/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.cpp new file mode 100644 index 00000000..3bef67d7 --- /dev/null +++ b/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.cpp @@ -0,0 +1,139 @@ +#include "SysIDAlgFull.h" +#include "FirstOptimization.h" +#include "SecondOptimization.h" +#include + +SysIDAlgFull::SysIDAlgFull( + const std::vector& variables, + const std::vector& AlgOptions, + const std::vector& na_idx, + const std::vector& nu_idx, + const Eigen::VectorXd& lb, + const Eigen::VectorXd& ub, + const Eigen::VectorXd& lba, + const Eigen::VectorXd& uba, + const Eigen::VectorXd& X0_1, + const Eigen::VectorXd& T, + const Eigen::MatrixXd& data, + const Eigen::MatrixXd& dataFull, + const Eigen::MatrixXd& W_ip, + const Eigen::MatrixXd& Ginv, + const Eigen::MatrixXd& Aid, + const Eigen::MatrixXd& Ad, + const Eigen::MatrixXd& Kd +) + : na_idx_(na_idx), nu_idx_(nu_idx), + lb_(lb), ub_(ub), lba_(lba), uba_(uba), + X0_1_(X0_1), T_(T), data_(data), dataFull_(dataFull), + W_ip_(W_ip), Ginv_(Ginv), Aid_(Aid), Ad_(Ad), Kd_(Kd) +{ + // 初始化变量 + n_ = static_cast(variables[0]); + m_ = static_cast(variables[1]); + b_ = static_cast(variables[2]); + d_ = static_cast(variables[3]); + + // 算法选项 + k_ = AlgOptions[0]; + tol_ = AlgOptions[1]; + MS1_ = static_cast(AlgOptions[2]); + MS2_ = static_cast(AlgOptions[3]); + regroup_ = static_cast(AlgOptions[4]); + SearchAlpha_ = static_cast(AlgOptions[7]); + includeOffset_ = static_cast(AlgOptions[8]); + includeConstraints_ = static_cast(AlgOptions[10]); + constraintVariant_ = static_cast(AlgOptions[11]); + + initialize(); +} + +void SysIDAlgFull::initialize() { + // 初始化参数维度 + p_ip_ = 10 * n_; + + if (includeOffset_) { + p_full_ = p_ip_ + 4 * n_; + b_full_ = b_ + 4 * n_; + } else { + p_full_ = p_ip_ + 3 * n_; + b_full_ = b_ + 3 * n_; + } + + + + + // 其他初始化操作 +} + +void SysIDAlgFull::runAlgorithm() { + // 处理约束 + processConstraints(); + + // 参数重组 + if (regroup_) { + regroupParameters(); + } + + // 运行第一个优化问题 + runFirstOptimization(); + + // 根据需要运行第二个优化问题 + if (SearchAlpha_) { + runSecondOptimization(); + } + + // 最终结果保存在 X_, Wfull_, alphanew_ +} + +void SysIDAlgFull::processConstraints() { + // 根据 includeConstraints_ 和 constraintVariant_ 处理约束 + // 具体实现省略 +} + +void SysIDAlgFull::regroupParameters() { + // 参数重组的实现 + // 具体实现省略 +} + +void SysIDAlgFull::runFirstOptimization() { + // 构建第一个优化问题的对象 + FirstOptimization firstOpt(/* 传入必要的参数 */); + + // 设置初始点 + firstOpt.setStartingPoint(X0_1_); + + // 运行优化 + firstOpt.solve(); + + // 获取结果 + X_ = firstOpt.getSolution(); +} + +void SysIDAlgFull::runSecondOptimization() { + // 构建第二个优化问题的对象 + SecondOptimization secondOpt(/* 传入必要的参数 */); + + // 设置初始点 + Eigen::VectorXd X0_2; // 根据需要初始化 + secondOpt.setStartingPoint(X0_2); + + // 运行优化 + secondOpt.solve(); + + // 获取结果 + alphanew_ = secondOpt.getAlphaNew(); + // 更新 X_ + // 具体实现省略 +} + +Eigen::VectorXd SysIDAlgFull::getX() const { + return X_; +} + +Eigen::MatrixXd SysIDAlgFull::getWfull() const { + return Wfull_; +} + +Eigen::VectorXd SysIDAlgFull::getAlphaNew() const { + return alphanew_; +} diff --git a/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.h b/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.h new file mode 100644 index 00000000..a331a349 --- /dev/null +++ b/Examples/Kinova/SystemIdentification/IterativeSystemIdentification/SysIDAlgFull.h @@ -0,0 +1,86 @@ +#ifndef SYSIDALGFULL_H +#define SYSIDALGFULL_H + +#include +#include +#include "Utils.h" + +class SysIDAlgFull { +public: + SysIDAlgFull( + const std::vector& variables, + const std::vector& AlgOptions, + const std::vector& na_idx, + const std::vector& nu_idx, + const Eigen::VectorXd& lb, + const Eigen::VectorXd& ub, + const Eigen::VectorXd& lba, + const Eigen::VectorXd& uba, + const Eigen::VectorXd& X0_1, + const Eigen::VectorXd& T, + const Eigen::MatrixXd& data, + const Eigen::MatrixXd& dataFull, + const Eigen::MatrixXd& W_ip, + const Eigen::MatrixXd& Ginv, + const Eigen::MatrixXd& Aid, + const Eigen::MatrixXd& Ad, + const Eigen::MatrixXd& Kd + ); + + void runAlgorithm(); + + Eigen::VectorXd getX() const; + Eigen::MatrixXd getWfull() const; + Eigen::VectorXd getAlphaNew() const; + +private: + // 输入参数 + int n_; + int m_; + int b_; + int d_; + std::vector na_idx_; + std::vector nu_idx_; + Eigen::VectorXd lb_; + Eigen::VectorXd ub_; + Eigen::VectorXd lba_; + Eigen::VectorXd uba_; + Eigen::VectorXd X0_1_; + Eigen::VectorXd T_; + Eigen::MatrixXd data_; + Eigen::MatrixXd dataFull_; + Eigen::MatrixXd W_ip_; + Eigen::MatrixXd Ginv_; + Eigen::MatrixXd Aid_; + Eigen::MatrixXd Ad_; + Eigen::MatrixXd Kd_; + + // 算法选项 + double k_; + double tol_; + int MS1_; + int MS2_; + bool regroup_; + bool SearchAlpha_; + bool includeOffset_; + bool includeConstraints_; + int constraintVariant_; + + // 中间变量 + int p_ip_; + int p_full_; + int b_full_; + Eigen::VectorXd X_; + Eigen::MatrixXd Wfull_; + Eigen::VectorXd alphanew_; + + // 私有方法 + void initialize(); + void processConstraints(); + void regroupParameters(); + void runFirstOptimization(); + void runSecondOptimization(); + // 其他辅助方法 +}; + +#endif // SYSIDALGFULL_H \ No newline at end of file diff --git a/Examples/Kinova/SystemIdentification/QRDecompositionSolver.cpp b/Examples/Kinova/SystemIdentification/QRDecompositionSolver.cpp new file mode 100644 index 00000000..c62a5d5e --- /dev/null +++ b/Examples/Kinova/SystemIdentification/QRDecompositionSolver.cpp @@ -0,0 +1,132 @@ +#include "QRDecompositionSolver.h" +#include +#include +#include +#include +#include + +namespace RAPTOR { +namespace Kinova { + +QRDecompositionSolver::QRDecompositionSolver(){ + getdata(); +} + +QRDecompositionSolver::QRDecompositionSolver(const MatX& ObservationMatrix, const VecX& InParam) { + compute(ObservationMatrix, InParam); +} + +QRDecompositionSolver::~QRDecompositionSolver() {} + + +void QRDecompositionSolver::getdata(){ + // Load robot model + const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf"; + pinocchio::Model model; + pinocchio::urdf::buildModel(urdf_filename, model); + pinocchio::Data data(model); + + // Create a trajectory + int N = 1000; // number of time steps + double T = 1000.0; // total time + int degree = 5; // degree of the polynomial + trajPtr_ = std::make_shared(T, N, model.nv, Uniform, degree); + ridPtr_ = std::shared_ptr(model, trajPtr_); + + // Generate random joint p, v, and a (not accurate) + std::srand(std::time(nullptr)); + VecX z = 2 * M_PI * VecX::Random(trajPtr->varLength).array() - M_PI; + + ridPtr_.compute(z, false); + MatX ObservationMatrix = ridPtr_.Y; + VecX InParam = VecX::Zero(91); + + // compute + compute(ObservationMatrix, InParam); +} + +void RDecompositionSolver::compute(const MatX& W, const VecX& InParam){ + + + // QR decomposition with column pivoting, improve stability + Eigen::ColPivHouseholderQR qr(W); + + // Permutation matrix + Eigen::PermutationMatrix perm = qr.colsPermutation(); + + // Redefine the permutation matrix meaning, same as matlab + // perm.indices()[i] = j old matrix ith col move to new matrix jth col + // M[i] = j old j jth col is M ith col + std::vector M(perm.indices().size()); + for (int i =0; i< perm.indices().size(); ++i){ + M[perm.indices()[i]]= i; + } + + + int rankW = qr.rank(); + std::vector idx(M.begin(), M.begin() + rankW); + std::sort(idx.begin(), idx.end()); + std::vector idx_(M.begin() + rankW, M.end()); + std::sort(idx_.begin(), idx_.end()); + + int p = W.cols(); + Aid = MatX::Zero(p, rankW); + for(int i = 0; i < rankW; ++i) { + Aid(idx[i], i) = 1.0; + } + + Ad = MatX::Zero(p, p - rankW); + for(int i = 0; i < static_cast(idx_.size()); ++i) { + Ad(idx_[i], i) = 1.0; + } + + // Combine Aid and Ad + MatX A(p,p); + A << Aid, Ad; + + // QR decomposition on W * A + Eigen::HouseholderQR qr_A(W * A); + MatX R_full = qr_A.matrixQR().triangularView(); + + // Extract indep R1 (upper-left block) and dep R2 (upper-right block) + MatX R1 = R_full.topLeftCorner(rankW, rankW); + MatX R2 = R_full.topRightCorner(rankW, p - rankW); + + // kd = (R1^-1)*R2 b*(p-b) + Kd = R1.solve(R2) + + // get rid of extremely small numbers for more numerical stability + double threshold = std::sqrt(eps); + for(int i = 0; i < Kd.rows(); ++i) { + for(int j = 0; j < Kd.cols(); ++j) { + if(std::abs(Kd(i, j)) < threshold) { + Kd(i, j) = 0.0; + } + } + } + + // Get independant and dependant parameters + VecX pi_id = Aid.transpose() * InParam; + VecX pi_d = Ad.transpose() * InParam; + + // Compute base inertial parameters + beta = pi_id + Kd * pi_d; + + // compute Ginv + MatX KG_(p,p); + KG_.setZero(); + KG_.topLeftCorner(rankW, rankW) = MatX::Identity(rankW, rankW); + KG_.topRightCorner(rankW, p - rankW) = -Kd; + KG_.bottomRightCorner(p - rankW, p - rankW) = MatX::Identity(p - rankW, p - rankW); + Ginv = A * KG_; + + // Set the dimensions of independent and dependent parameters + dim_id = pi_id.size(); + dim_d = pi_d.size(); + + // Compute RegroupMatrix + RegroupMatrix = Aid + Ad * Kd.transpose() +} + +}; // namespace Kinova +}; // namespace RAPTOR diff --git a/Examples/Kinova/SystemIdentification/QRDecompositionSolver.h b/Examples/Kinova/SystemIdentification/QRDecompositionSolver.h new file mode 100644 index 00000000..32f4fefe --- /dev/null +++ b/Examples/Kinova/SystemIdentification/QRDecompositionSolver.h @@ -0,0 +1,51 @@ +#ifndef QRDECOMPOSITIONSOLVER_H +#define QRDECOMPOSITIONSOLVER_H + +#include +#include +#include +#include +#include + +#include "RegressorInverseDynamics.h" +#include "Trajectories.h" + +namespace RAPTOR { +namespace Kinova { + +class QRDecompositionSolver{ +public: + using VecX = Eigen::VectorXd; + using MatX = Eigen::MatrixXd; + + MatX Aid; + MatX Ad; + MatX Kd; + MatX Ginv; + VecX Beta; + MatX RegroupMatrix; + + std::shared_ptr trajPtr_; + std::shared_ptr ridPtr_; + // Eigen::VectorXi jtype = Eigen::VectorXi(0); + + + int dim_id; + int dim_d; + double eps = 1e-8; + + QRDecompositionSolver(); + QRDecompositionSolver(const MatX& ObservationMatrix, const VecX& InParam); + ~QRDecompositionSolver(); + +private: + void getdata(); + void compute(const MatX& W, const Eigen::VectorXd& InParam); +}; + +}; // namespace Kinova +}; // namespace RAPTOR + + + +#endif \ No newline at end of file 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(); diff --git a/Examples/Talos/src/TalosMultipleStepOptimizer.cpp b/Examples/Talos/src/TalosMultipleStepOptimizer.cpp index 688ed837..2d958982 100644 --- a/Examples/Talos/src/TalosMultipleStepOptimizer.cpp +++ b/Examples/Talos/src/TalosMultipleStepOptimizer.cpp @@ -143,7 +143,8 @@ bool TalosMultipleStepOptimizer::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]); } @@ -152,9 +153,11 @@ bool TalosMultipleStepOptimizer::get_bounds_info( const Index start_pos = m_position[stepOptVec_.size() + i]; const Index end_pos = m_position[stepOptVec_.size() + i + 1]; - std::cout << "gait " << i << " - " << i + 1 << " continuous constraint: " - << periodConsVec_[i]->m - << " [" << start_pos << " " << end_pos << "]" << std::endl; + if (display_info) { + std::cout << "gait " << i << " - " << i + 1 << " continuous constraint: " + << periodConsVec_[i]->m + << " [" << start_pos << " " << end_pos << "]" << std::endl; + } periodConsVec_[i]->compute_bounds(); diff --git a/Trajectories/src/Plain.cpp b/Trajectories/src/Plain.cpp index 91cb19f6..ebaf296b 100644 --- a/Trajectories/src/Plain.cpp +++ b/Trajectories/src/Plain.cpp @@ -25,6 +25,7 @@ Plain::Plain(const int Nact_input) { q_dd(i) = VecX::Zero(Nact); pq_pz(i) = MatX::Zero(Nact, varLength); + pq_pz(i).middleCols(i * Nact, Nact).setIdentity(); pq_d_pz(i) = MatX::Zero(Nact, varLength); pq_dd_pz(i) = MatX::Zero(Nact, varLength); @@ -60,6 +61,7 @@ Plain::Plain(const int N_input, q_dd(i) = VecX::Zero(Nact); pq_pz(i) = MatX::Zero(Nact, varLength); + pq_pz(i).middleCols(i * Nact, Nact).setIdentity(); pq_d_pz(i) = MatX::Zero(Nact, varLength); pq_dd_pz(i) = MatX::Zero(Nact, varLength); @@ -88,15 +90,14 @@ void Plain::compute(const VecX& z, // q_dd(i) = 0; if (compute_derivatives) { - pq_pz(i).resize(Nact, varLength); - pq_pz(i).block(0, i * Nact, Nact, Nact) = Eigen::MatrixXd::Identity(Nact, Nact); + // pq_pz(i).middleCols(i * Nact, Nact).setIdentity(); // We have already done that in the constructor // pq_d_pz(i).resize(Nact, varLength); // pq_dd_pz(i).resize(Nact, varLength); } if (compute_hessian) { for (int j = 0; j < Nact; j++) { - pq_pz_pz(j, i).setZero(); + // pq_pz_pz(j, i).setZero(); // We have already done that in the constructor // pq_d_pz_pz(j, i).setZero(); // pq_dd_pz_pz(j, i).setZero(); } diff --git a/Utils/include/HigherOrderDerivatives.h b/Utils/include/HigherOrderDerivatives.h index 5e6ef9d7..915c705e 100644 --- a/Utils/include/HigherOrderDerivatives.h +++ b/Utils/include/HigherOrderDerivatives.h @@ -146,7 +146,7 @@ inline double safedddacosdddx(const double x, } inline double safexSinx(const double x, - const double nearZeroThreshold = false) { + const double nearZeroThreshold = 1e-6) { if (fabs(x) < nearZeroThreshold) { // use Taylor expansion to approximate double xSquare = x * x; double xFourth = xSquare * xSquare; @@ -156,7 +156,7 @@ inline double safexSinx(const double x, } inline double safedxSinxdx(const double x, - double nearZeroThreshold = false) { + const double nearZeroThreshold = 1e-6) { if (fabs(x) < nearZeroThreshold) { // use Taylor expansion to approximate double xSquare = x * x; double xThird = x * xSquare; @@ -168,7 +168,7 @@ inline double safedxSinxdx(const double x, } inline double safeddxSinxddx(const double x, - const double nearZeroThreshold = false) { + const double nearZeroThreshold = 1e-6) { if (fabs(x) < nearZeroThreshold) { // use Taylor expansion to approximate double xSquare = x * x; double xFourth = xSquare * xSquare; @@ -182,7 +182,7 @@ inline double safeddxSinxddx(const double x, } inline double safedddxSinxdddx(const double x, - const double nearZeroThreshold = false) { + const double nearZeroThreshold = 1e-6) { if (fabs(x) < nearZeroThreshold) { // use Taylor expansion to approximate double xSquare = x * x; double xThird = x * xSquare;