diff --git a/orocos_kdl/src/chainexternalwrenchestimator.cpp b/orocos_kdl/src/chainexternalwrenchestimator.cpp new file mode 100644 index 000000000..c20f5c53c --- /dev/null +++ b/orocos_kdl/src/chainexternalwrenchestimator.cpp @@ -0,0 +1,214 @@ +// Copyright (C) 2021 Djordje Vukcevic + +// Version: 1.0 +// Author: Djordje Vukcevic +// URL: http://www.orocos.org/kdl + +// This library 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 2.1 of the License, or (at your option) any later version. + +// This library 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 this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#include "chainexternalwrenchestimator.hpp" + +namespace KDL { + +ChainExternalWrenchEstimator::ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps, const int maxiter) : + CHAIN(chain), + DT_SEC(1.0 / sample_frequency), FILTER_CONST(filter_constant), + svd_eps(eps), + svd_maxiter(maxiter), + nj(CHAIN.getNrOfJoints()), ns(CHAIN.getNrOfSegments()), + jnt_mass_matrix(nj), previous_jnt_mass_matrix(nj), jnt_mass_matrix_dot(nj), + initial_jnt_momentum(nj), estimated_momentum_integral(nj), filtered_estimated_ext_torque(nj), + gravity_torque(nj), coriolis_torque(nj), total_torque(nj), estimated_ext_torque(nj), + jacobian_end_eff(nj), + jacobian_end_eff_transpose(Eigen::MatrixXd::Zero(nj, 6)), jacobian_end_eff_transpose_inv(Eigen::MatrixXd::Zero(6, nj)), + U(Eigen::MatrixXd::Zero(nj, 6)), V(Eigen::MatrixXd::Zero(6, 6)), + S(Eigen::VectorXd::Zero(6)), S_inv(Eigen::VectorXd::Zero(6)), tmp(Eigen::VectorXd::Zero(6)), + ESTIMATION_GAIN(Eigen::VectorXd::Constant(nj, estimation_gain)), + dynparam_solver(CHAIN, gravity), + jacobian_solver(CHAIN), + fk_pos_solver(CHAIN) +{ +} + +void ChainExternalWrenchEstimator::updateInternalDataStructures() +{ + nj = CHAIN.getNrOfJoints(); + ns = CHAIN.getNrOfSegments(); + jnt_mass_matrix.resize(nj); + previous_jnt_mass_matrix.resize(nj); + jnt_mass_matrix_dot.resize(nj); + initial_jnt_momentum.resize(nj); + estimated_momentum_integral.resize(nj); + filtered_estimated_ext_torque.resize(nj); + gravity_torque.resize(nj); + coriolis_torque.resize(nj); + total_torque.resize(nj); + estimated_ext_torque.resize(nj); + jacobian_end_eff.resize(nj); + jacobian_end_eff_transpose.conservativeResizeLike(MatrixXd::Zero(nj, 6)); + jacobian_end_eff_transpose_inv.conservativeResizeLike(MatrixXd::Zero(6, nj)); + U.conservativeResizeLike(MatrixXd::Zero(nj, 6)); + V.conservativeResizeLike(MatrixXd::Zero(6, 6)); + S.conservativeResizeLike(VectorXd::Zero(6)); + S_inv.conservativeResizeLike(VectorXd::Zero(6)); + tmp.conservativeResizeLike(VectorXd::Zero(6)); + ESTIMATION_GAIN.conservativeResizeLike(Eigen::VectorXd::Constant(nj, ESTIMATION_GAIN(0))); + dynparam_solver.updateInternalDataStructures(); + jacobian_solver.updateInternalDataStructures(); + fk_pos_solver.updateInternalDataStructures(); +} + +// Calculates robot's initial momentum in the joint space. If this method is not called by the user, zero values will be taken for the initial momentum. +int ChainExternalWrenchEstimator::setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity) +{ + // Check sizes first + if (joint_position.rows() != nj || joint_velocity.rows() != nj) + return (error = E_SIZE_MISMATCH); + + // Calculate robot's inertia and momentum in the joint space + if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix)) + return (error = E_DYNPARAMSOLVERMASS_FAILED); + + initial_jnt_momentum.data = jnt_mass_matrix.data * joint_velocity.data; + + // Reset data because of the new momentum offset + SetToZero(estimated_momentum_integral); + SetToZero(filtered_estimated_ext_torque); + + return (error = E_NOERROR); +} + +// Sets singular-value eps parameter for the SVD calculation +void ChainExternalWrenchEstimator::setSVDEps(const double eps_in) +{ + svd_eps = eps_in; +} + +// Sets maximum iteration parameter for the SVD calculation +void ChainExternalWrenchEstimator::setSVDMaxIter(const int maxiter_in) +{ + svd_maxiter = maxiter_in; +} + +// This method calculates the external wrench that is applied on the robot's end-effector. +int ChainExternalWrenchEstimator::JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench) +{ + /** + * ========================================================================== + * First-order momentum observer, an implementation based on: + * S. Haddadin, A. De Luca and A. Albu-Schäffer, + * "Robot Collisions: A Survey on Detection, Isolation, and Identification," + * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017. + * ========================================================================== + */ + + // Check sizes first + if (nj != CHAIN.getNrOfJoints() || ns != CHAIN.getNrOfSegments()) + return (error = E_NOT_UP_TO_DATE); + if (joint_position.rows() != nj || joint_velocity.rows() != nj || joint_torque.rows() != nj) + return (error = E_SIZE_MISMATCH); + + /** + * ================================================================================================================ + * Part I: Estimation of the torques felt in joints as a result of the external wrench being applied on the robot + * ================================================================================================================ + */ + + // Calculate decomposed robot's dynamics + if (E_NOERROR != dynparam_solver.JntToMass(joint_position, jnt_mass_matrix)) + return (error = E_DYNPARAMSOLVERMASS_FAILED); + + if (E_NOERROR != dynparam_solver.JntToCoriolis(joint_position, joint_velocity, coriolis_torque)) + return (error = E_DYNPARAMSOLVERCORIOLIS_FAILED); + + if (E_NOERROR != dynparam_solver.JntToGravity(joint_position, gravity_torque)) + return (error = E_DYNPARAMSOLVERGRAVITY_FAILED); + + // Calculate the change of robot's inertia in the joint space + jnt_mass_matrix_dot.data = (jnt_mass_matrix.data - previous_jnt_mass_matrix.data) / DT_SEC; + + // Save data for the next iteration + previous_jnt_mass_matrix.data = jnt_mass_matrix.data; + + // Calculate total torque exerted on the joint + total_torque.data = joint_torque.data - gravity_torque.data - coriolis_torque.data + jnt_mass_matrix_dot.data * joint_velocity.data; + + // Accumulate main integral + estimated_momentum_integral.data += (total_torque.data + filtered_estimated_ext_torque.data) * DT_SEC; + + // Estimate external joint torque + estimated_ext_torque.data = ESTIMATION_GAIN.asDiagonal() * (jnt_mass_matrix.data * joint_velocity.data - estimated_momentum_integral.data - initial_jnt_momentum.data); + + // First order low-pass filter: filter out the noise from the estimated signal + // This filter can be turned off by setting FILTER_CONST value to 0 + filtered_estimated_ext_torque.data = FILTER_CONST * filtered_estimated_ext_torque.data + (1.0 - FILTER_CONST) * estimated_ext_torque.data; + + /** + * ================================================================================================================== + * Part II: Propagate above-estimated joint torques to Cartesian wrench using a pseudo-inverse of Jacobian-Transpose + * ================================================================================================================== + */ + + // Compute robot's end-effector frame, expressed in the base frame + Frame end_eff_frame; + if (E_NOERROR != fk_pos_solver.JntToCart(joint_position, end_eff_frame)) + return (error = E_FKSOLVERPOS_FAILED); + + // Compute robot's jacobian for the end-effector frame, expressed in the base frame + if (E_NOERROR != jacobian_solver.JntToJac(joint_position, jacobian_end_eff)) + return (error = E_JACSOLVER_FAILED); + + // Transform the jacobian from the base frame to the end-effector frame. + // This part can be commented out if the user wants estimated wrench to be expressed w.r.t. base frame + jacobian_end_eff.changeBase(end_eff_frame.M.Inverse()); // Jacobian is now expressed w.r.t. end-effector frame + + // SVD of "Jac^T" with maximum iterations "maxiter": Jac^T = U * S^-1 * V^T + jacobian_end_eff_transpose = jacobian_end_eff.data.transpose(); + if (E_NOERROR != svd_eigen_HH(jacobian_end_eff_transpose, U, S, V, tmp, svd_maxiter)) + return (error = E_SVD_FAILED); + + // Invert singular values: S^-1 + for (int i = 0; i < S.size(); ++i) + S_inv(i) = (std::fabs(S(i)) < svd_eps) ? 0.0 : 1.0 / S(i); + + // Compose the inverse: (Jac^T)^-1 = V * S^-1 * U^T + jacobian_end_eff_transpose_inv = V * S_inv.asDiagonal() * U.adjoint(); + + // Compute end-effector's Cartesian wrench from the estimated joint torques: (Jac^T)^-1 * ext_tau + Vector6d estimated_wrench = jacobian_end_eff_transpose_inv * filtered_estimated_ext_torque.data; + for (int i = 0; i < 6; i++) + external_wrench(i) = estimated_wrench(i); + + return (error = E_NOERROR); +} + +// Getter for the torques felt in the robot's joints due to the external wrench being applied on the robot +void ChainExternalWrenchEstimator::getEstimatedJntTorque(JntArray &external_joint_torque) +{ + assert(external_joint_torque.rows() == filtered_estimated_ext_torque.rows()); + external_joint_torque = filtered_estimated_ext_torque; +} + +const char* ChainExternalWrenchEstimator::strError(const int error) const +{ + if (E_FKSOLVERPOS_FAILED == error) return "Internally-used Forward Position Kinematics (Recursive) solver failed"; + else if (E_JACSOLVER_FAILED == error) return "Internally-used Jacobian solver failed"; + else if (E_DYNPARAMSOLVERMASS_FAILED == error) return "Internally-used Dynamics Parameters (Mass) solver failed"; + else if (E_DYNPARAMSOLVERCORIOLIS_FAILED == error) return "Internally-used Dynamics Parameters (Coriolis) solver failed"; + else if (E_DYNPARAMSOLVERGRAVITY_FAILED == error) return "Internally-used Dynamics Parameters (Gravity) solver failed"; + else return SolverI::strError(error); +} + +} // namespace diff --git a/orocos_kdl/src/chainexternalwrenchestimator.hpp b/orocos_kdl/src/chainexternalwrenchestimator.hpp new file mode 100644 index 000000000..c60b882d4 --- /dev/null +++ b/orocos_kdl/src/chainexternalwrenchestimator.hpp @@ -0,0 +1,128 @@ +// Copyright (C) 2021 Djordje Vukcevic + +// Version: 1.0 +// Author: Djordje Vukcevic +// URL: http://www.orocos.org/kdl + +// This library 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 2.1 of the License, or (at your option) any later version. + +// This library 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 this library; if not, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + +#ifndef KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP +#define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP + +#include +#include "utilities/svd_eigen_HH.hpp" +#include "chaindynparam.hpp" +#include "chainjnttojacsolver.hpp" +#include "chainfksolverpos_recursive.hpp" +#include + +namespace KDL { + + /** + * \brief First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector. + * + * Implementation based on: + * S. Haddadin, A. De Luca and A. Albu-Schäffer, + * "Robot Collisions: A Survey on Detection, Isolation, and Identification," + * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017. + * + * Note: This component assumes that the external wrench is applied on the end-effector (last) link of the robot's chain. + */ + class ChainExternalWrenchEstimator : public SolverI + { + typedef Eigen::Matrix Vector6d; + + public: + + static const int E_FKSOLVERPOS_FAILED = -100; //! Internally-used Forward Position Kinematics (Recursive) solver failed + static const int E_JACSOLVER_FAILED = -101; //! Internally-used Jacobian solver failed + static const int E_DYNPARAMSOLVERMASS_FAILED = -102; //! Internally-used Dynamics Parameters (Mass) solver failed + static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103; //! Internally-used Dynamics Parameters (Coriolis) solver failed + static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104; //! Internally-used Dynamics Parameters (Gravity) solver failed + + /** + * Constructor for the estimator, it will allocate all the necessary memory + * \param chain The kinematic chain of the robot, an internal copy will be made. + * \param gravity The gravity-acceleration vector to use during the calculation. + * \param sample_frequency Frequency at which users updates it estimation loop (in Hz). + * \param estimation_gain Parameter used to control the estimator's convergence + * \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter. + * This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out. + * The filter can be turned off by setting this value to 0. + * \param eps If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001 + * \param maxiter Maximum iterations for the SVD computations. Default: 150. + */ + ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps = 0.00001, const int maxiter = 150); + ~ChainExternalWrenchEstimator(){}; + + /** + * Calculates robot's initial momentum in the joint space. + * Bassically, sets the offset for future estimation (momentum calculation). + * If this method is not called by the user, zero values will be taken for the initial momentum. + */ + int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity); + + // Sets singular-value eps parameter for the SVD calculation + void setSVDEps(const double eps_in); + + // Sets maximum iteration parameter for the SVD calculation + void setSVDMaxIter(const int maxiter_in); + + /** + * This method calculates the external wrench that is applied on the robot's end-effector. + * Input parameters: + * \param joint_position The current (measured) joint positions. + * \param joint_velocity The current (measured) joint velocities. + * \param joint_torque The joint space torques. + * Depending on the user's choice, this array can represent commanded or measured joint torques. + * A particular choice depends on the available sensors in robot's joint. + * For more details see the above-referenced article. + * + * Output parameters: + * \param external_wrench The estimated external wrench applied on the robot's end-effector. + * The wrench will be expressed w.r.t. end-effector's frame. + * + * @return error/success code + */ + int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench); + + // Returns the torques felt in the robot's joints as a result of the external wrench being applied on the robot. + void getEstimatedJntTorque(JntArray &external_joint_torque); + + /// @copydoc KDL::SolverI::updateInternalDataStructures() + virtual void updateInternalDataStructures(); + + /// @copydoc KDL::SolverI::strError() + virtual const char* strError(const int error) const; + + private: + const Chain &CHAIN; + const double DT_SEC, FILTER_CONST; + double svd_eps; + int svd_maxiter; + unsigned int nj, ns; + JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix, jnt_mass_matrix_dot; + JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque, + gravity_torque, coriolis_torque, total_torque, estimated_ext_torque; + Jacobian jacobian_end_eff; + Eigen::MatrixXd jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, U, V; + Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN; + ChainDynParam dynparam_solver; + ChainJntToJacSolver jacobian_solver; + ChainFkSolverPos_recursive fk_pos_solver; + }; +} + +#endif diff --git a/orocos_kdl/tests/solvertest.cpp b/orocos_kdl/tests/solvertest.cpp index 57b275463..24bde309b 100644 --- a/orocos_kdl/tests/solvertest.cpp +++ b/orocos_kdl/tests/solvertest.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #include @@ -247,6 +248,7 @@ void SolverTest::UpdateChainTest() ChainIdSolver_RNE idsolver1(chain2, Vector::Zero()); unsigned int nr_of_constraints = 4; ChainHdSolver_Vereshchagin hdsolver(chain2,Twist::Zero(),4); + ChainExternalWrenchEstimator extwrench_estimator(chain2,Vector::Zero(), 100.0, 30.0, 0.5); JntArray q_in(chain2.getNrOfJoints()); JntArray q_in2(chain2.getNrOfJoints()); @@ -266,6 +268,7 @@ void SolverTest::UpdateChainTest() FrameVel T2; Wrenches wrenches(chain2.getNrOfSegments()); JntSpaceInertiaMatrix m(chain2.getNrOfJoints()); + Wrench wrench_out; Jacobian alpha(nr_of_constraints - 1); JntArray beta(nr_of_constraints - 1); @@ -291,6 +294,7 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToGravity(q_in, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, dynparam.JntToMass(q_in, m)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_NOT_UP_TO_DATE, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); jacsolver1.updateInternalDataStructures(); jacdotsolver1.updateInternalDataStructures(); @@ -304,6 +308,7 @@ void SolverTest::UpdateChainTest() idsolver1.updateInternalDataStructures(); hdsolver.updateInternalDataStructures(); dynparam.updateInternalDataStructures(); + extwrench_estimator.updateInternalDataStructures(); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolverpos.JntToCart(q_in, T, chain2.getNrOfSegments())); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH,fksolvervel.JntToCart(q_in3, T2, chain2.getNrOfSegments())); @@ -322,6 +327,7 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); q_in.resize(chain2.getNrOfJoints()); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, jacsolver1.JntToJac(q_in, jac, chain2.getNrOfSegments())); @@ -339,10 +345,12 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToGravity(q_in, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToMass(q_in, m)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); q_in2.resize(chain2.getNrOfJoints()); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); + CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); wrenches.resize(chain2.getNrOfSegments()); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, idsolver1.CartToJnt(q_in,q_in2,q_out,wrenches,q_out2)); CPPUNIT_ASSERT_EQUAL((int)SolverI::E_SIZE_MISMATCH, hdsolver.CartToJnt(q_in,q_in2,q_out,alpha,beta,wrenches, ff_tau, constraint_tau)); @@ -379,6 +387,7 @@ void SolverTest::UpdateChainTest() CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToCoriolis(q_in, q_in2, q_out)); CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToGravity(q_in, q_out)); CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= dynparam.JntToMass(q_in, m)); + CPPUNIT_ASSERT((int)SolverI::E_NOERROR <= extwrench_estimator.JntToExtWrench(q_in,q_in2,ff_tau,wrench_out)); } void SolverTest::FkPosAndJacTest() { @@ -1550,3 +1559,271 @@ void SolverTest::FdAndVereshchaginSolversConsistencyTest() return; } + +void SolverTest::ExternalWrenchEstimatorTest() +{ + /** + * Closed-loop test for the external wrench estimator class: + * Simple controlled behaviour of the robot subjected to an external force is simulated. + * The external wrench estimator is called in each iteration of the control loop + * so to converge to final wrench value. + * In the end, estimated wrench is compared to the ground-truth values of the simulated wrench. + */ + std::cout << "KDL External Wrench Estimator Test" << std::endl; + + /** + * This EPS has a slightly different purpose than the EPSes of the other solver-tests. While other EPSes are taking care of the differences that + * originate from e.g. floating-number imprecisions, different compilers (or same compiler but different flags) used between different machines (OS), etc. + * The EPS specified bellow is there to cover those imperfections as well but, it's also there to + * take into account the noise in estimated signals (the differences between estimated and ground-truth wrenches), caused by other computations in this test + * (ones coming from the implemented controller and the dynamics simulator) not just those coming from the estimator itself. + */ + double eps_wrench = 0.5, eps_torque = 0.3; + int ret; + unsigned int nj = kukaLWR.getNrOfJoints(); + unsigned int ns = kukaLWR.getNrOfSegments(); + CPPUNIT_ASSERT(Equal(nj, ns)); // Current implementation of the Vereshchagin solver can only work with chains that have equal number of joints and segments + + // Initialize state and control variables + JntArray q(nj); // Current joint position + JntArray qd(nj); // Current joint velocity + JntArray qdd(nj); // Resultant joint accelerations + JntArrayVel jnt_position_velocity(nj); // variable necessary for the FK vel solver + JntArray jnt_array_zero(nj); // Zero joint input for RNE + JntArray command_torque(nj); // Control torque to actuate the robot + JntArray constraint_tau(nj); // It will result in zero in Vereshchagin for this test + JntArray gravity_torque(nj); // Gravity torque computed by RNE + JntArray ext_torque_reference(nj); // Ground-truth joint torques due to the external force applied on the end-effector + JntArray ext_torque_estimated(nj); // Estimated joint torques + Wrenches f_ext_base(ns); // External Wrenches acting on the end-effector, expressed in base-link coordinates + Wrenches f_ext_zero(ns); // Zero Wrenches + Wrench f_tool_estimated; // External Wrenches estimated by the momentum-observer + Frame end_effector_pose; + Frame desired_end_eff_pose; + Jacobian jacobian_end_eff(nj); + FrameVel end_eff_twist; + FrameVel desired_end_eff_twist; + Eigen::Matrix end_eff_force; // variable necessary for the control + Eigen::Matrix end_eff_pos_error; // variable necessary for the control + Eigen::Matrix end_eff_vel_error; // variable necessary for the control + + // Arm root acceleration (robot's base mounted on an even surface) + Vector linearAcc(0.0, 0.0, -9.81); Vector angularAcc(0.0, 0.0, 0.0); + + // Initialize kinematics solvers + ChainFkSolverPos_recursive fksolverpos(kukaLWR); + ChainFkSolverVel_recursive fksolvervel(kukaLWR); + ChainJntToJacSolver jacobian_solver(kukaLWR); + + // RNE ID solver for control purposes + KDL::ChainIdSolver_RNE IdSolver(kukaLWR, linearAcc); + + // Vereshchagin Hybrid Dynamics solver for simulation purposes + int numberOfConstraints = 6; + Jacobian alpha(numberOfConstraints); // Constraint Unit forces at the end-effector + JntArray beta(numberOfConstraints); // Acceleration energy at the end-effector + KDL::SetToZero(alpha); // Set to zero to deactivate all constraints + KDL::SetToZero(beta); // Set to zero to deactivate all constraints + Twist vereshchagin_root_Acc(-linearAcc, angularAcc); // Note: Vereshchagin solver takes root acc. with opposite sign comparead to the above FD and RNE solvers + ChainHdSolver_Vereshchagin constraintSolver(kukaLWR, vereshchagin_root_Acc, numberOfConstraints); + + // External Wrench Estimator + double sample_frequency = 1000.0; // Hz + double estimation_gain = 45.0; + double filter_constant = 0.5; + ChainExternalWrenchEstimator extwrench_estimator(kukaLWR, linearAcc, sample_frequency, estimation_gain, filter_constant); + + // Prepare test cases + std::vector jnt_pos; + std::vector wrench_reference; + + // Intialize random generator + std::random_device rd; //Will be used to obtain a seed for the random number engine + std::mt19937 gen(rd()); //Standard mersenne_twister_engine seeded with rd() + std::uniform_real_distribution<> dis_force(-15.0, 15.0); + std::uniform_real_distribution<> dis_moment(-0.9, 0.9); + std::uniform_real_distribution<> dis_jnt_vel(-0.5, 0.5); + + // Set first test case + q(0) = 1.0; + q(1) = 0.0; + q(2) = 0.0; + q(3) = 4.71; + q(4) = 0.0; + q(5) = 1.57; + q(6) = 5.48; + jnt_pos.push_back(q); + wrench_reference.push_back(Wrench(Vector(dis_force(gen), dis_force(gen), dis_force(gen)), Vector(0.0, 0.0, 0.0))); // Ground-truth external wrench acting on the end-effector expressed in local end-effector's frame + + // Set second test case + q(0) = 2.96; + q(1) = 1.02; + q(2) = 6.15; + q(3) = 1.61; + q(4) = 0.22; + q(5) = 0.17; + q(6) = 0.01; + jnt_pos.push_back(q); + wrench_reference.push_back(Wrench(Vector(0.0, 0.0, 0.0), Vector(dis_moment(gen), dis_moment(gen), 0.0))); // expressed in local end-effector's frame + + // Set third test case + q(0) = 1.12; + q(1) = 0.66; + q(2) = 6.15; + q(3) = 4.09; + q(4) = 1.64; + q(5) = 0.12; + q(6) = 0.01; + jnt_pos.push_back(q); + wrench_reference.push_back(Wrench(Vector(dis_force(gen), dis_force(gen), dis_force(gen)), Vector(dis_moment(gen), 0.0, dis_moment(gen)))); // expressed in local end-effector's frame + + // ########################################################################################## + // Control and simulation + // ########################################################################################## + + // Control gains for a simple PD controller + double k_p = 1500.0; // Proportional + double k_d = 300.0; // Derivative + + // Time required to complete the task + double simulationTime = 0.4; // in seconds + double timeDelta = 1.0 / sample_frequency; // unit of seconds + + // Iterate over test cases + for (unsigned int i = 0; i < jnt_pos.size(); i++) + { + // Re-set control and simulation variables + q = jnt_pos[i]; + qd(0) = dis_jnt_vel(gen); + qd(1) = dis_jnt_vel(gen); + qd(2) = dis_jnt_vel(gen); + qd(3) = dis_jnt_vel(gen); + qd(4) = dis_jnt_vel(gen); + qd(5) = dis_jnt_vel(gen); + qd(6) = dis_jnt_vel(gen); + + end_eff_force.setZero(); + end_eff_pos_error.setZero(); + end_eff_vel_error.setZero(); + f_ext_base = f_ext_zero; + + // Initialize the estimator + extwrench_estimator.updateInternalDataStructures(); + extwrench_estimator.setInitialMomentum(q, qd); // sets the offset for future estimation (momentum calculation) + + // Set the desired Cartesian state + fksolverpos.JntToCart(q, end_effector_pose); + desired_end_eff_pose.p(0) = end_effector_pose.p(0) + 0.02; + desired_end_eff_pose.p(1) = end_effector_pose.p(1) + 0.02; + desired_end_eff_pose.p(2) = end_effector_pose.p(2) + 0.02; + desired_end_eff_twist.p.v(0) = 0.0; + desired_end_eff_twist.p.v(1) = 0.0; + desired_end_eff_twist.p.v(2) = 0.0; + + for (double t = 0.0; t <= simulationTime; t = t + timeDelta) + { + ret = jacobian_solver.JntToJac(q, jacobian_end_eff); + if (ret < 0) + { + std::cout << "Jacobian solver ERROR: " << ret << std::endl; + break; + } + + ret = fksolverpos.JntToCart(q, end_effector_pose); + if (ret < 0) + { + std::cout << "FK pos solver ERROR: " << ret << std::endl; + break; + } + + jnt_position_velocity.q = q; + jnt_position_velocity.qdot = qd; + ret = fksolvervel.JntToCart(jnt_position_velocity, end_eff_twist); + if (ret < 0) + { + std::cout << "FK vel solver ERROR: " << ret << std::endl; + break; + } + + end_eff_pos_error(0) = end_effector_pose.p(0) - desired_end_eff_pose.p(0); + end_eff_pos_error(1) = end_effector_pose.p(1) - desired_end_eff_pose.p(1); + end_eff_pos_error(2) = end_effector_pose.p(2) - desired_end_eff_pose.p(2); + + end_eff_vel_error(0) = end_eff_twist.p.v(0) - desired_end_eff_twist.p.v(0); + end_eff_vel_error(1) = end_eff_twist.p.v(1) - desired_end_eff_twist.p.v(1); + end_eff_vel_error(2) = end_eff_twist.p.v(2) - desired_end_eff_twist.p.v(2); + + end_eff_force = -end_eff_pos_error * k_p - end_eff_vel_error * k_d; + + // Compute gravity joint torques (hide external wrench from this dynamics calculation) + ret = IdSolver.CartToJnt(q, jnt_array_zero, jnt_array_zero, f_ext_zero, gravity_torque); + if (ret < 0) + { + std::cout << "KDL RNE solver ERROR: " << ret << std::endl; + break; + } + + // Compute joint control commands + command_torque.data = jacobian_end_eff.data.transpose() * end_eff_force; + command_torque.data += gravity_torque.data; + + // Start simulating the external force + if (t > 0.2) f_ext_base[ns - 1] = end_effector_pose.M * wrench_reference[i]; + + // Compute resultant joint accelerations that simulate robot's behaviour, given the command torques (add external wrench in this dynamics calculation) + ret = constraintSolver.CartToJnt(q, qd, qdd, alpha, beta, f_ext_base, command_torque, constraint_tau); + if (ret < 0) + { + std::cout << "KDL Vereshchagin solver ERROR: " << ret << std::endl; + break; + } + + // State integration: integrate from model accelerations to next joint state (positions and velocities) + qd.data = qd.data + qdd.data * timeDelta; // Euler Forward + q.data = q.data + qd.data * timeDelta; // Symplectic Euler + + // Saturate integrated joint position for full circle crossing + for (unsigned int j = 0; j < nj; j++) + { + q(j) = std::fmod(q(j), 360 * deg2rad); + if (q(j) < 0.0) q(j) += 360 * deg2rad; + } + + // Estimate external wrench + extwrench_estimator.JntToExtWrench(q, qd, command_torque, f_tool_estimated); + } + + // Inverse Force Kinematics + Eigen::Matrix wrench; + wrench(0) = f_ext_base[ns - 1](0); + wrench(1) = f_ext_base[ns - 1](1); + wrench(2) = f_ext_base[ns - 1](2); + wrench(3) = f_ext_base[ns - 1](3); + wrench(4) = f_ext_base[ns - 1](4); + wrench(5) = f_ext_base[ns - 1](5); + ext_torque_reference.data = jacobian_end_eff.data.transpose() * wrench; + + // Get estimated joint torque + extwrench_estimator.getEstimatedJntTorque(ext_torque_estimated); + + // ################################################################################## + // Final comparison + // ################################################################################## + CPPUNIT_ASSERT(Equal(f_tool_estimated(0), wrench_reference[i](0), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(1), wrench_reference[i](1), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(2), wrench_reference[i](2), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(3), wrench_reference[i](3), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(4), wrench_reference[i](4), eps_wrench)); + CPPUNIT_ASSERT(Equal(f_tool_estimated(5), wrench_reference[i](5), eps_wrench)); + + CPPUNIT_ASSERT(Equal(ext_torque_estimated(0), ext_torque_reference(0), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(1), ext_torque_reference(1), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(2), ext_torque_reference(2), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(3), ext_torque_reference(3), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(4), ext_torque_reference(4), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(5), ext_torque_reference(5), eps_torque)); + CPPUNIT_ASSERT(Equal(ext_torque_estimated(6), ext_torque_reference(6), eps_torque)); + } + + return; +} diff --git a/orocos_kdl/tests/solvertest.hpp b/orocos_kdl/tests/solvertest.hpp index 699c0cba0..8da18319f 100644 --- a/orocos_kdl/tests/solvertest.hpp +++ b/orocos_kdl/tests/solvertest.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include @@ -33,6 +34,7 @@ class SolverTest : public CppUnit::TestFixture CPPUNIT_TEST(FkVelAndIkVelTest ); CPPUNIT_TEST(FkPosAndIkPosTest ); CPPUNIT_TEST(VereshchaginTest ); + CPPUNIT_TEST(ExternalWrenchEstimatorTest ); CPPUNIT_TEST(IkSingularValueTest ); CPPUNIT_TEST(IkVelSolverWDLSTest ); CPPUNIT_TEST(FkPosVectTest ); @@ -53,6 +55,7 @@ class SolverTest : public CppUnit::TestFixture void FkVelAndIkVelTest(); void FkPosAndIkPosTest(); void VereshchaginTest(); + void ExternalWrenchEstimatorTest(); void IkSingularValueTest() ; void IkVelSolverWDLSTest(); void FkPosVectTest();