optimal condition number on the effector

Examples/Kinova/CMakeLists.txt

@@ -6,6 +6,7 @@ add_library(Kinovalib SHARED
     WaitrDiscrete/src/KinovaWaitrOptimizer.cpp
     SystemIdentification/DataFilter/src/DataFilterOptimizer.cpp
     SystemIdentification/ExcitingTrajectories/src/ConditionNumberOptimizer.cpp
+    SystemIdentification/ExcitingTrajectories/src/EndEffectorConditionalNumberOptimizer.cpp
 )
 
 target_include_directories(Kinovalib PUBLIC
@@ -159,6 +160,29 @@ target_compile_options(KinovaExciting_example PUBLIC
     ${PINOCCHIO_FLAGS})
 
 
+add_executable(KinovaExciting_example_Endeffector 
+    SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp)
+
+target_include_directories(KinovaExciting_example_Endeffector  PUBLIC
+    ystemIdentification/DataFilter/include
+    SystemIdentification/ExcitingTrajectories/include
+    SystemIdentification/IterativeSystemIdentification/include)
+
+target_link_libraries(KinovaExciting_example_Endeffector  PUBLIC
+    trajlib 
+    IDlib
+    Conslib
+    Optlib
+    Kinovalib
+    ipopt
+    coinhsl
+    ${BOOST_LIBRARIES} 
+    pinocchio::pinocchio)
+
+target_compile_options(KinovaExciting_example_Endeffector  PUBLIC
+    ${PINOCCHIO_FLAGS})
+
+
 ### Python bindings 
 nanobind_add_module(KinovaHLP_nanobind 
     NB_SHARED LTO

Examples/Kinova/SystemIdentification/ExcitingTrajectories/KinovaRegressorExampleEndeffector.cpp

@@ -0,0 +1,168 @@
+#include "EndeffectorConditionNumberOptimizer.h"
+
+using namespace RAPTOR;
+using namespace Kinova;
+using namespace Ipopt;
+
+int main(int argc, char* argv[]) {
+    const std::string regroupMatrixFileName = "../Examples/Kinova/SystemIdentification/RegroupMatrix.csv";
+    // const std::string regroupMatrixFileName = "../Examples/Kinova/SystemIdentification/RegroupMatrix_withoutmotordynamics.csv";
+
+    // Define robot model
+    const std::string urdf_filename = "../Robots/kinova-gen3/kinova.urdf";
+
+    pinocchio::Model model;
+    pinocchio::urdf::buildModel(urdf_filename, model);
+
+    model.gravity.linear()(2) = GRAVITY;
+    model.friction.setZero();
+    // model.damping.setZero();
+    // model.rotorInertia.setZero();
+
+    // Define trajectory parameters
+    const double T = 10.0;
+    const int N = 50;
+    const int degree = 5;
+    const double base_frequency = 2.0 * M_PI / T;
+
+    // Define initial guess
+    std::srand(static_cast<unsigned int>(time(0)));
+    Eigen::VectorXd z = 2 * 0.2 * Eigen::VectorXd::Random((2 * degree + 3) * model.nv).array() - 0.1;
+    z.segment((2 * degree + 1) * model.nv, model.nv) = 
+        2 * 1.0 * Eigen::VectorXd::Random(model.nv).array() - 1.0;
+    z.segment((2 * degree + 1) * model.nv + model.nv, model.nv) = 
+        2 * 0.5 * Eigen::VectorXd::Random(model.nv).array() - 0.5;
+
+    // Define limits buffer
+    Eigen::VectorXd joint_limits_buffer(model.nq);
+    joint_limits_buffer.setConstant(0.02);
+    Eigen::VectorXd velocity_limits_buffer(model.nq);
+    velocity_limits_buffer.setConstant(0.05);
+    Eigen::VectorXd torque_limits_buffer(model.nq);
+    torque_limits_buffer.setConstant(0.5);
+
+    // Initialize Kinova optimizer
+    SmartPtr<EndeffectorConditionNumberOptimizer> mynlp = new EndeffectorConditionNumberOptimizer();
+    try {
+	    mynlp->set_parameters(z,
+                              T,
+                              N,
+                              degree,
+                              base_frequency,
+                              model,
+                            //   regroupMatrixFileName,
+                              joint_limits_buffer,
+                              velocity_limits_buffer,
+                              torque_limits_buffer);
+        mynlp->constr_viol_tol = 1e-5;
+    }
+    catch (std::exception& e) {
+        std::cerr << e.what() << std::endl;
+        throw std::runtime_error("Error initializing Ipopt class! Check previous error message!");
+    }
+
+    SmartPtr<IpoptApplication> app = IpoptApplicationFactory();
+
+    app->Options()->SetNumericValue("tol", 1e-6);
+    app->Options()->SetNumericValue("constr_viol_tol", mynlp->constr_viol_tol);
+	app->Options()->SetNumericValue("max_wall_time", 60.0);
+	app->Options()->SetIntegerValue("print_level", 5);
+    app->Options()->SetStringValue("mu_strategy", "adaptive");
+    app->Options()->SetStringValue("linear_solver", "ma57");
+    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", "first-order");
+    // app->Options()->SetNumericValue("derivative_test_perturbation", 1e-7);
+    // app->Options()->SetNumericValue("derivative_test_tol", 1e-3);
+
+    // 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!");
+    }
+
+    // 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<std::chrono::milliseconds>(end - start).count();
+        std::cout << "Total solve time: " << solve_time << " milliseconds.\n";
+
+        const Eigen::VectorXd initial_position_part = mynlp->solution.segment((2 * degree + 1) * model.nv, model.nv);
+        mynlp->solution.segment((2 * degree + 1) * model.nv, model.nv) =
+            Utils::wrapToPi(initial_position_part);
+    }
+    catch (std::exception& e) {
+        std::cerr << e.what() << std::endl;
+        throw std::runtime_error("Error solving optimization problem! Check previous error message!");
+    }
+
+    // Re-evaluate the solution at a higher resolution and print the results.
+    if (mynlp->ifFeasible) {
+        std::shared_ptr<Trajectories> traj = std::make_shared<FixedFrequencyFourierCurves>(T, 
+                                                                                           2000, 
+                                                                                           model.nv, 
+                                                                                           TimeDiscretization::Uniform, 
+                                                                                           degree,
+                                                                                           base_frequency);
+        traj->compute(mynlp->solution, false);
+
+        if (argc > 1) {
+            const std::string outputfolder = "../Examples/Kinova/SystemIdentification/ExcitingTrajectories/data/T10_d5_slower/";
+            std::ofstream solution(outputfolder + "exciting-solution-" + std::string(argv[1]) + ".csv");
+            std::ofstream position(outputfolder + "exciting-position-" + std::string(argv[1]) + ".csv");
+            std::ofstream velocity(outputfolder + "exciting-velocity-" + std::string(argv[1]) + ".csv");
+            std::ofstream acceleration(outputfolder + "exciting-acceleration-" + std::string(argv[1]) + ".csv");
+
+            solution << std::setprecision(16);
+            position << std::setprecision(16);
+            velocity << std::setprecision(16);
+            acceleration << std::setprecision(16);
+
+            for (int i = 0; i < traj->N; i++) {
+                position << traj->q(i).transpose() << std::endl;
+                velocity << traj->q_d(i).transpose() << std::endl;
+                acceleration << traj->q_dd(i).transpose() << std::endl;
+            }
+
+            for (int i = 0; i < mynlp->solution.size(); i++) {
+                solution << mynlp->solution(i) << std::endl;
+            }
+            solution << base_frequency << std::endl;
+        }
+        else {
+            std::ofstream solution("exciting-solution.csv");
+            std::ofstream position("exciting-position.csv");
+            std::ofstream velocity("exciting-velocity.csv");
+            std::ofstream acceleration("exciting-acceleration.csv");
+
+            for (int i = 0; i < traj->N; i++) {
+                position << traj->q(i).transpose() << std::endl;
+                velocity << traj->q_d(i).transpose() << std::endl;
+                acceleration << traj->q_dd(i).transpose() << std::endl;
+            }
+
+            for (int i = 0; i < mynlp->solution.size(); i++) {
+                solution << mynlp->solution(i) << std::endl;
+            }
+        }
+    }
+
+
+
+    return 0;
+}

Examples/Kinova/SystemIdentification/ExcitingTrajectories/include/EndeffectorConditionNumberOptimizer.h

@@ -0,0 +1,105 @@
+#ifndef CONDITION_NUMBER_OPTIMIZER_H
+#define CONDITION_NUMBER_OPTIMIZER_H
+
+#include "KinovaConstants.h"
+
+#include "Optimizer.h"
+
+#include "RegressorInverseDynamics.h"
+#include "FixedFrequencyFourierCurves.h"
+
+#include "JointLimits.h"
+#include "VelocityLimits.h"
+#include "TorqueLimits.h"
+#include "KinovaCustomizedConstraints.h"
+
+namespace RAPTOR {
+namespace Kinova {
+
+class EndeffectorConditionNumberOptimizer : public Optimizer {
+public:
+    using Model = pinocchio::Model;
+    using Vec3 = Eigen::Vector3d;
+    using VecX = Eigen::VectorXd;
+    using MatX = Eigen::MatrixXd;
+
+    /** Default constructor */
+    EndeffectorConditionNumberOptimizer() = default;
+
+    /** Default destructor */
+    ~EndeffectorConditionNumberOptimizer() = default;
+
+    // [set_parameters]
+    bool set_parameters(
+        const VecX& x0_input,
+        const double T_input,
+        const int N_input,
+        const int degree_input,
+        const double base_frequency_input,
+        const Model& model_input, 
+        // const std::string& regroupMatrixFileName,
+        const VecX& joint_limits_buffer_input,
+        const VecX& velocity_limits_buffer_input,
+        const VecX& torque_limits_buffer_input,
+        Eigen::VectorXi jtype_input = Eigen::VectorXi(0)
+    );
+
+    /**@name Overloaded from TNLP */
+    //@{
+    /** Method to return some info about the NLP */
+    bool get_nlp_info(
+        Index&          n,
+        Index&          m,
+        Index&          nnz_jac_g,
+        Index&          nnz_h_lag,
+        IndexStyleEnum& index_style
+    ) final override;
+
+    /** Method to return the objective value */
+    bool eval_f(
+        Index         n,
+        const Number* x,
+        bool          new_x,
+        Number&       obj_value
+    ) final override;
+
+    /** Method to return the gradient of the objective */
+    bool eval_grad_f(
+        Index         n,
+        const Number* x,
+        bool          new_x,
+        Number*       grad_f
+    ) final override;
+
+    /**@name Methods to block default compiler methods.
+    *
+    * The compiler automatically generates the following three methods.
+    *  Since the default compiler implementation is generally not what
+    *  you want (for all but the most simple classes), we usually
+    *  put the declarations of these methods in the private section
+    *  and never implement them. This prevents the compiler from
+    *  implementing an incorrect "default" behavior without us
+    *  knowing. (See Scott Meyers book, "Effective C++")
+    */
+    //@{
+    EndeffectorConditionNumberOptimizer(
+       const EndeffectorConditionNumberOptimizer&
+    );
+
+    EndeffectorConditionNumberOptimizer& operator=(
+       const EndeffectorConditionNumberOptimizer&
+    );
+
+    // MatX regroupMatrix;
+
+    std::shared_ptr<Trajectories> trajPtr_;
+
+    std::shared_ptr<RegressorInverseDynamics> ridPtr_;
+
+    int joint_num;
+};
+
+}; // namespace Kinova
+}; // namespace RAPTOR
+
+#endif // CONDITION_NUMBER_OPTIMIZER_H