diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
deleted file mode 100644
index 94594a3fc2262a4977922c7ff90292dfcf2ece62..0000000000000000000000000000000000000000
--- a/test/CMakeLists.txt
+++ /dev/null
@@ -1,38 +0,0 @@
-cmake_minimum_required(VERSION 3.10)
-project(ImpedanceController)
-
-# Set C++ standard
-set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED True)
-
-# Find required packages
-find_package(catkin REQUIRED COMPONENTS roscpp)
-find_package(pinocchio REQUIRED)
-find_package(casadi REQUIRED)
-
-# Include directories
-include_directories(
- include
- ${catkin_INCLUDE_DIRS}
-)
-
-# Add source files
-# add_executable(pin_test interpolation-SE3.cpp)
-add_executable(autodiff_test
- # casadi-crnea.cpp
- dynamics-autodiff-test.cpp
- # inverse_kinematics_test.cpp
-)
-
-# Link libraries
-# target_link_libraries(pin_test
-# ${catkin_LIBRARIES}
-# pinocchio::pinocchio
-# casadi
-# )
-
-target_link_libraries(autodiff_test
- ${catkin_LIBRARIES}
- pinocchio::pinocchio
- casadi
-)
diff --git a/test/dynamics-autodiff-test.cpp b/test/dynamics-autodiff-test.cpp
deleted file mode 100644
index 6b018e45eff73afdba21ed64051967ace59b337b..0000000000000000000000000000000000000000
--- a/test/dynamics-autodiff-test.cpp
+++ /dev/null
@@ -1,243 +0,0 @@
-#include "pinocchio/autodiff/casadi.hpp"
-
-#include "pinocchio/algorithm/aba.hpp" // joint acceleration, Minverse
-#include "pinocchio/algorithm/crba.hpp" // joint-space inertia (upper-triangle)
-#include "pinocchio/algorithm/jacobian.hpp" // Jacobian
-#include "pinocchio/algorithm/joint-configuration.hpp"
-#include "pinocchio/algorithm/kinematics.hpp"
-#include "pinocchio/algorithm/rnea.hpp" // Coriolis, gravity, joint torque
-
-#include "pinocchio/parsers/urdf.hpp"
-
-#include <iostream>
-
-int main(int argc, char ** argv)
-{
- using namespace pinocchio;
-
- //* typedef for CasADi Auto-Diff *//
- typedef double Scalar;
- typedef ::casadi::SX ADScalar;
-
- typedef ModelTpl<Scalar> Model;
- typedef Model::Data Data;
-
- typedef ModelTpl<ADScalar> ADModel;
- typedef ADModel::Data ADData;
-
- typedef ADModel::ConfigVectorType ConfigVectorAD;
- typedef ADModel::TangentVectorType TangentVectorAD;
-
- //* load Franka Research 3 URDF file *//
- const std::string urdf_filename = std::string("../model/fr3.urdf");
-
- Model model;
- pinocchio::urdf::buildModel(urdf_filename, model);
- // std::cout << "model name: " << model.name << std::endl;
- Data data(model);
-
- // set end-effector's ID (joint 8)
- const std::string ee_name = "fr_joint8";
- JointIndex ee_joint_id = model.getJointId(ee_name);
- if (ee_joint_id == 0)
- {
- std::cerr << "Invalid joint ID for end-effector: " << ee_name << std::endl;
- return -1;
- }
- else
- {
- std::cout << "End-effector ID: " << ee_joint_id << std::endl; // 8
- }
-
- //* Pick up random configuration, velocity and acceleration vectors. *//
- Eigen::VectorXd q = randomConfiguration(model);
- Eigen::VectorXd v(Eigen::VectorXd::Random(model.nv));
- Eigen::VectorXd a(Eigen::VectorXd::Random(model.nv));
-
- std::cout << "q: " << q.transpose() << std::endl;
- std::cout << "v: " << v.transpose() << std::endl;
- std::cout << "a: " << a.transpose() << std::endl << std::endl;
-
- //* Create CasADi model and data from Pinocchio model *//
- ADModel ad_model = model.cast<ADScalar>();
- ADData ad_data(ad_model);
-
- //* Create symbolic CasADi vectors & Matices *//
- // joint configuration
- ::casadi::SX cs_q = ::casadi::SX::sym("q", model.nq);
- ConfigVectorAD q_ad(model.nq);
- q_ad = Eigen::Map<ConfigVectorAD>(static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
-
- // joint velocity
- ::casadi::SX cs_v = ::casadi::SX::sym("v", model.nv);
- TangentVectorAD v_ad(model.nv);
- v_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
-
- // joint acceleration
- ::casadi::SX cs_a = ::casadi::SX::sym("a", model.nv);
- TangentVectorAD a_ad(model.nv);
- a_ad = Eigen::Map<TangentVectorAD>(static_cast<std::vector<ADScalar>>(cs_a).data(), model.nv, 1);
-
- // Upper-triangular Mass Matrix
- ::casadi::SX cs_M = ::casadi::SX::zeros(model.nv, model.nv);
- // ::casadi::SX cs_M_dot = ::casadi::SX::zeros(model.nv, model.nv);
- ::casadi::SX cs_Minv = ::casadi::SX::zeros(model.nv, model.nv);
- ::casadi::SX cs_Minv_dot = ::casadi::SX::zeros(model.nv, model.nv);
-
- // end-effector Jacobian Matrix
- ::casadi::SX cs_J = ::casadi::SX::zeros(6, model.nv);
- ::casadi::SX cs_J_dot = ::casadi::SX::zeros(6, model.nv);
-
- //* Create CasADi "symbolic" function *//
- /* forward kinematics */
- // "Numerical" forward kinematics
- // ! joint : universe, joint1, joint2, ... joint7 => selecting joint8 would cause segmentation
- // error pinocchio::forwardKinematics(model, data, q); for (JointIndex joint_id = 0; joint_id <
- // (JointIndex) model.njoints; ++joint_id)
- // {
- // std::cout << std::setw(24) << std::left
- // << model.names[joint_id] << ": "
- // << std::fixed << std::setprecision(2)
- // << data.oMi[joint_id].translation().transpose()
- // << std::endl;
- // }
-
- // "Symbolic" forward kinematics
- // pinocchio::forwardKinematics(ad_model, ad_data, q_ad);
- // for (JointIndex joint_id = 0; joint_id < (JointIndex) ad_model.njoints; ++joint_id)
- // {
- // std::cout << std::setw(24) << std::left
- // << ad_model.names[joint_id] << ": "
- // << std::fixed << std::setprecision(2)
- // << ad_data.oMi[joint_id].translation().transpose()
- // << std::endl;
- // }
-
- /* CRBA algorithm : compute Mass Matrix */
- pinocchio::crba(ad_model, ad_data, q_ad);
- for (int i = 0; i < model.nv; ++i)
- {
- for (int j = 0; j < model.nv; ++j)
- {
- cs_M(i, j) = ad_data.M(i, j); // copy symbolic M(q) into CasADi matrix
- }
- }
- // std::cout << "Symbolic mass matrix:\n" << cs_M << std::endl;
-
- // derivative of M w.r.t time
- // for (int i = 0; i < model.nv; ++i)
- // {
- // for (int j = 0; j < model.nv; ++j)
- // {
- // ::casadi::SX dMij_dq = jacobian(cs_M(i,j), cs_q); // extract derivative of M[i, j] w.r.t
- // all q cs_M_dot(i, j) = mtimes(dMij_dq, cs_v); // multiply by v and sum over nq
- // }
- // }
-
- /* compute derivative of Inverse Mass Matrix */
- pinocchio::computeMinverse(ad_model, ad_data, q_ad);
- for (int i = 0; i < model.nv; ++i)
- {
- for (int j = 0; j < model.nv; ++j)
- {
- cs_Minv(i, j) = ad_data.Minv(i, j); // copy symbolic M(q) into CasADi matrix
- }
- }
-
- for (int i = 0; i < model.nv; ++i)
- {
- for (int j = 0; j < model.nv; ++j)
- {
- ::casadi::SX dMinv_dq = jacobian(cs_Minv(i, j), cs_q);
- cs_Minv_dot(i, j) = mtimes(dMinv_dq, cs_v);
- }
- }
-
- /* compute derivative of (end-effector) Jacobian matrix */
- // !!!!! Segmentation fault
- // Eigen::Matrix<ADScalar, 6, Eigen::Dynamic> J(6, model.nv);
- // Data::Matrix6x J(6, model.nv);
- // J.setZero();
- // pinocchio::computeJointJacobian(model, data, q, ee_joint_id, J);
- // pinocchio::computeJointJacobian(ad_model, ad_data, q_ad, ee_joint_id, J);
- // pinocchio::computeJointJacobian(ad_model, ad_data, q_ad, ee_joint_id, ad_data.J);
- // std::cout << "Jacobian J:\n" << J << std::endl;
-
- // compute "Numerical" Jacobian
- // pinocchio::computeJointJacobians(model, data, q);
- // std::cout << "Jacobian J:\n" << data.J << std::endl;
-
- // compute "Symbolic" Jacobian
- pinocchio::computeJointJacobians(ad_model, ad_data, q_ad);
- for (int i = 0; i < 6; ++i)
- {
- for (int j = 0; j < model.nv; ++j)
- {
- cs_J(i, j) = ad_data.J(i, j); // copy symbolic J(q) into CasADi matrix
- }
- }
- // std::cout << "Symbolic mass matrix:\n" << cs_J << std::endl;
-
- // Jacobian derivative
- for (int i = 0; i < 6; ++i)
- {
- for (int j = 0; j < model.nv; ++j)
- {
- ::casadi::SX dJij_dq =
- jacobian(cs_J(i, j), cs_q); // extract derivative of J[i, j] w.r.t all q
- cs_J_dot(i, j) = mtimes(dJij_dq, cs_v); // multiply by v and sum over nq
- }
- }
-
- /* Inverse Dynamics -> joint torque */
- pinocchio::rnea(ad_model, ad_data, q_ad, v_ad, a_ad);
- ::casadi::SX tau_ad(model.nv, 1);
- for (Eigen::DenseIndex k = 0; k < model.nv; ++k)
- {
- tau_ad(k) = ad_data.tau[k];
- }
-
- //* Build (symbolic) CasADi function for evaluation *//
- ::casadi::Function eval_rnea("eval_rnea", ::casadi::SXVector{ cs_q, cs_v, cs_a },
- ::casadi::SXVector{ tau_ad });
-
- ::casadi::Function eval_Minv_dot("eval_Minv_dot", ::casadi::SXVector{ cs_q, cs_v },
- ::casadi::SXVector{ cs_Minv_dot });
- // std::cout << cs_Minv_dot << std::endl;
-
- ::casadi::Function eval_Jdot("eval_Jdot", ::casadi::SXVector{ cs_q, cs_v },
- ::casadi::SXVector{ cs_J_dot });
- // std::cout << cs_J_dot << std::endl;
-
- //* Evaluate CasADi expression with real value *//
- std::vector<double> q_vec((size_t)model.nq);
- Eigen::Map<Eigen::VectorXd>(q_vec.data(), model.nq, 1) = q;
-
- std::vector<double> v_vec((size_t)model.nv);
- Eigen::Map<Eigen::VectorXd>(v_vec.data(), model.nv, 1) = v;
-
- std::vector<double> a_vec((size_t)model.nv);
- Eigen::Map<Eigen::VectorXd>(a_vec.data(), model.nv, 1) = a;
-
- // Inverse dynamics
- ::casadi::DM tau_casadi_res = eval_rnea(::casadi::DMVector{ q_vec, v_vec, a_vec })[0];
- Data::TangentVectorType tau_casadi_vec = Eigen::Map<Data::TangentVectorType>(
- static_cast<std::vector<double>>(tau_casadi_res).data(), model.nv, 1);
-
- pinocchio::rnea(model, data, q, v, a);
- std::cout << "pinocchio double:\n"
- << "\ttau = " << data.tau.transpose() << std::endl;
- std::cout << "pinocchio CasADi:\n"
- << "\ttau = " << tau_casadi_vec.transpose() << std::endl;
-
- // Minv_dot
- ::casadi::DM Minv_dot_res = eval_Minv_dot(::casadi::DMVector{ q_vec, v_vec })[0];
- std::cout << "(CasADi) Minv_dot:" << Minv_dot_res << std::endl << std::endl;
-
- // J_dot
- ::casadi::DM Jdot_res = eval_Jdot(::casadi::DMVector{ q_vec, v_vec })[0];
- std::cout << "(CasADi) Jdot:" << Jdot_res << std::endl << std::endl;
-
- pinocchio::computeJointJacobiansTimeVariation(model, data, q, v);
- std::cout << "(pinocchio) Jdot:\n" << data.dJ << std::endl << std::endl;
-}
\ No newline at end of file