From d6cefa0076f81d96aefe2ed586cd94c879246994 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Sun, 13 Nov 2022 20:01:29 +0900 Subject: [PATCH 1/2] Added a new example to test the pose_jacobian_derivative method. --- cmake/jacobian_time_derivative/CMakeLists.txt | 8 + .../jacobian_time_derivative_test.cpp | 234 ++++++++++++++++++ 2 files changed, 242 insertions(+) create mode 100644 cmake/jacobian_time_derivative/jacobian_time_derivative_test.cpp diff --git a/cmake/jacobian_time_derivative/CMakeLists.txt b/cmake/jacobian_time_derivative/CMakeLists.txt index edaf2ad..7d1b2c4 100644 --- a/cmake/jacobian_time_derivative/CMakeLists.txt +++ b/cmake/jacobian_time_derivative/CMakeLists.txt @@ -10,3 +10,11 @@ add_executable(${PROJECT_NAME} TARGET_LINK_LIBRARIES(${PROJECT_NAME} dqrobotics ) + +add_executable(jacobian_time_derivative_test + jacobian_time_derivative_test.cpp + ) + +TARGET_LINK_LIBRARIES(jacobian_time_derivative_test + dqrobotics + ) diff --git a/cmake/jacobian_time_derivative/jacobian_time_derivative_test.cpp b/cmake/jacobian_time_derivative/jacobian_time_derivative_test.cpp new file mode 100644 index 0000000..57f5e45 --- /dev/null +++ b/cmake/jacobian_time_derivative/jacobian_time_derivative_test.cpp @@ -0,0 +1,234 @@ +/** +(C) Copyright 2022 DQ Robotics Developers +This file is part of DQ Robotics. + DQ Robotics is free software: you can redistribute it and/or modify + it under the terms of the GNU Lesser General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + DQ Robotics is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU Lesser General Public License for more details. + You should have received a copy of the GNU Lesser General Public License + along with DQ Robotics. If not, see . +Contributors: +- Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) +*/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +using namespace Eigen; +using namespace DQ_robotics; + + +/** + * @brief numerical_differentiation() returns the numerical differentiation of a + * vector of matrices using the four-point central difference method. + * @param std::vector J + * @param double T + * @return std::vector J_dot + */ +std::vector numerical_differentiation(const std::vector& J, + const double& T) +{ + int s = J.size(); + + MatrixXd J0 = J[0]; + int m = J0.rows(); + int n = J0.cols(); + std::vector J_dot; + + for(int i=0; i robot + * @param int iterations + * @param double T + * @param double threshold + * @return bool accurate_computation + */ +bool check_pose_jacobian_derivative(const std::shared_ptr& robot, + int& iterations, + double& T, + double& threshold) +{ + bool accurate_computation = true; + int njoints = robot->get_dim_configuration_space(); + VectorXd q = VectorXd::Zero(njoints); + VectorXd q_dot = VectorXd::Zero(njoints); + + std::vector J; + std::vector J_dot; + std::vector numerical_J_dot; + + double t = 0; + double w = 2*pi; + + for(int i=0;ipose_jacobian(q)); + J_dot.push_back(robot->pose_jacobian_derivative(q, q_dot)); + } + numerical_J_dot = numerical_differentiation(J, T); + + for(int i=0;i2 && i threshold) + { + //throw std::runtime_error(std::string("Wrong pose Jacobian derivative computation")); + accurate_computation = false; + } + } + + } + return accurate_computation; +} + +/** + * @brief map_return() maps a bool to string. + * @param bool value + * @return std::string + */ +std::string map_return(const bool& value) +{ + std::string msg; + if (value == true) + { + msg = "Yes!"; + }else + { + msg = "No."; + } + return msg; +} + + +int main() +{ + double T = 1e-4; + int iterations = 1500; + double threshold = 1e-10; + + //------------test the numerical differentiation implementation----// + double t = 0; + double w = 2*pi; + std::vector M; + std::vector M_dot; + std::vector Numerical_M_dot; + bool accurate_computation = true; + for(int i=0;i2 && i threshold) + { + accurate_computation = false; + } + } + } + std::cout<<"is numerical_differentiation() working? " + < + (std::make_shared(DQ_DifferentialDriveRobot(0.3, 0.01))), + iterations, T, threshold); + std::cout<<"is pose_jacobian_derivative() working for the " + "DQ_DifferentialDriveRobot()? "< + (std::make_shared(DQ_HolonomicBase())), + iterations, T, threshold); + std::cout<<"is pose_jacobian_derivative() working for the " + "DQ_HolonomicBase()? "< + (std::make_shared(KukaLw4Robot::kinematics())), + iterations, T, threshold); + std::cout<<"is pose_jacobian_derivative() working for the " + "DQ_SerialManipulatorDH()? "< franka_mdh(5,7); + franka_mdh << 0, 0, 0, 0, 0, 0, 0, + 0.333, 0, 3.16e-1, 0, 3.84e-1, 0, 0, + 0, 0, 0, 8.25e-2, -8.25e-2, 0, 8.8e-2, + 0, -M_PI_2, M_PI_2, M_PI_2, -M_PI_2, M_PI_2, M_PI_2, + 0, 0, 0, 0, 0, 0, 0; + DQ_SerialManipulatorMDH franka(franka_mdh); + DQ robot_base = 1 + E_ * 0.5 * DQ(0, 0.0413, 0, 0); + franka.set_base_frame(robot_base); + franka.set_reference_frame(robot_base); + DQ robot_effector = 1+E_*0.5*k_*1.07e-1; + franka.set_effector(robot_effector); + + bool resultmdh = check_pose_jacobian_derivative + (std::static_pointer_cast + (std::make_shared(franka)), + iterations, T, threshold); + std::cout<<"is pose_jacobian_derivative() working for the " + "DQ_SerialManipulatorMDH()? "< Date: Wed, 16 Nov 2022 18:05:07 +0900 Subject: [PATCH 2/2] [jacobian_time_derivative_test.cpp] Fixed bug in the comparison of the error and the threshold in which big negative errors were considered accurated. --- .../jacobian_time_derivative_test.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/cmake/jacobian_time_derivative/jacobian_time_derivative_test.cpp b/cmake/jacobian_time_derivative/jacobian_time_derivative_test.cpp index 57f5e45..992f344 100644 --- a/cmake/jacobian_time_derivative/jacobian_time_derivative_test.cpp +++ b/cmake/jacobian_time_derivative/jacobian_time_derivative_test.cpp @@ -43,25 +43,14 @@ using namespace DQ_robotics; * @return std::vector J_dot */ std::vector numerical_differentiation(const std::vector& J, - const double& T) + const double& T) { int s = J.size(); - - MatrixXd J0 = J[0]; - int m = J0.rows(); - int n = J0.cols(); - std::vector J_dot; - - for(int i=0; i J_dot(s, MatrixXd::Zero(J[0].rows(), J[0].cols())); for(int i=2; i& robot, //std::cout<<"Error: "<2 && i threshold) + if(std::abs(max_coeff) > threshold) { //throw std::runtime_error(std::string("Wrong pose Jacobian derivative computation")); accurate_computation = false; @@ -173,7 +162,7 @@ int main() //std::cout<<"Error: "<2 && i threshold) + if(std::abs(max_coeff) > threshold) { accurate_computation = false; }