From aa59d6fb9ec43df9dfcae4d2182275f98c69c13f Mon Sep 17 00:00:00 2001 From: dmronga Date: Fri, 7 Jun 2024 09:56:32 +0200 Subject: [PATCH] Remove KDL robot model, it lacks too many features compared to RBDL and pinocchio --- CMakeLists.txt | 1 - scripts/full_install.sh | 21 +- src/robot_models/CMakeLists.txt | 4 - src/robot_models/kdl/CMakeLists.txt | 28 - src/robot_models/kdl/KinematicChainKDL.cpp | 110 ---- src/robot_models/kdl/KinematicChainKDL.hpp | 76 --- src/robot_models/kdl/RobotModelKDL.cpp | 515 ------------------ src/robot_models/kdl/RobotModelKDL.hpp | 172 ------ src/robot_models/kdl/test/CMakeLists.txt | 6 - .../kdl/test/test_robot_model_kdl.cpp | 177 ------ .../kdl/wbc-robot_models-kdl.pc.in | 12 - src/robot_models/test/CMakeLists.txt | 1 - .../test/test_model_consistency.cpp | 80 +-- 13 files changed, 21 insertions(+), 1182 deletions(-) delete mode 100644 src/robot_models/kdl/CMakeLists.txt delete mode 100644 src/robot_models/kdl/KinematicChainKDL.cpp delete mode 100644 src/robot_models/kdl/KinematicChainKDL.hpp delete mode 100644 src/robot_models/kdl/RobotModelKDL.cpp delete mode 100644 src/robot_models/kdl/RobotModelKDL.hpp delete mode 100644 src/robot_models/kdl/test/CMakeLists.txt delete mode 100644 src/robot_models/kdl/test/test_robot_model_kdl.cpp delete mode 100644 src/robot_models/kdl/wbc-robot_models-kdl.pc.in diff --git a/CMakeLists.txt b/CMakeLists.txt index 9fb8f2f7..35d9ada8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,6 @@ set(API_VERSION ${PROJECT_VERSION}) list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) option(ROBOT_MODEL_RBDL "Also build the RBDL-based robot model, by default only pinocchio is built" OFF) -option(ROBOT_MODEL_KDL "Also build the KDL-based robot model, by default only pinocchio is built" OFF) option(ROBOT_MODEL_HYRODYN "Also build the HyRoDyn-based robot model, by default only pinocchio is built" OFF) option(SOLVER_PROXQP "Build the ProxQP-based solver, by default only hls and qpoases are built" OFF) option(SOLVER_EIQUADPROG "Build the Eiquadprog-based solver, by default only hls and qpoases are built" OFF) diff --git a/scripts/full_install.sh b/scripts/full_install.sh index 24e3ea1a..129c382e 100644 --- a/scripts/full_install.sh +++ b/scripts/full_install.sh @@ -20,7 +20,7 @@ make -j8 && sudo make install && cd ../.. # URDF sudo apt-get -y install liburdfdom-headers-dev liburdfdom-dev -# Clone WBC repo to have the patches for KDL and qpOASES +# Clone WBC repo to have the patches git clone https://github.com/ARC-OPT/wbc.git # RBDL @@ -31,13 +31,6 @@ mkdir build && cd build cmake .. -DRBDL_BUILD_ADDON_URDFREADER=ON make -j8 && sudo make install && cd ../.. -# KDL -git clone https://github.com/orocos/orocos_kinematics_dynamics.git -b v1.5.1 -mkdir orocos_kinematics_dynamics/orocos_kdl/build -cd orocos_kinematics_dynamics/orocos_kdl/build -cmake .. -make -j8 && sudo make install && cd ../../.. - # Pinocchio git clone --branch v2.6.8 --recurse-submodules https://github.com/stack-of-tasks/pinocchio.git cd pinocchio @@ -45,16 +38,6 @@ mkdir build && cd build cmake .. -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_UNIT_TESTS=OFF make -j8 && sudo make install && cd ../.. -# KDL parser -sudo apt-get -y install libtinyxml2-dev -git clone https://github.com/ros/kdl_parser.git -b 1.14.1 -cd kdl_parser/kdl_parser -mkdir patches && cp ../../wbc/patches/kdl_parser.patch patches -git apply patches/kdl_parser.patch -mkdir build && cd build -cmake .. -make -j8 && sudo make install && cd ../../.. - # If not done yet, setup a ssh key pair using the command `ssh-keygen` and add the # key from `~/.ssh/id_rsa.pub `to the keys in your Gitlab account. @@ -103,7 +86,7 @@ make -j8 && sudo make install && cd ../.. # WBC mkdir wbc/build && cd wbc/build -cmake .. -DROBOT_MODEL_RBDL=ON -DROBOT_MODEL_KDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DSOLVER_OSQP=ON -DCMAKE_BUILD_TYPE=RELEASE +cmake .. -DROBOT_MODEL_RBDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DSOLVER_OSQP=ON -DCMAKE_BUILD_TYPE=RELEASE make -j8 && sudo make install && cd .. sudo ldconfig diff --git a/src/robot_models/CMakeLists.txt b/src/robot_models/CMakeLists.txt index 132cd746..e9ea7a51 100644 --- a/src/robot_models/CMakeLists.txt +++ b/src/robot_models/CMakeLists.txt @@ -5,7 +5,3 @@ endif() if(ROBOT_MODEL_HYRODYN) add_subdirectory(hyrodyn) endif() -if(ROBOT_MODEL_KDL) - add_subdirectory(kdl) -endif() - diff --git a/src/robot_models/kdl/CMakeLists.txt b/src/robot_models/kdl/CMakeLists.txt deleted file mode 100644 index 1d0f7fb4..00000000 --- a/src/robot_models/kdl/CMakeLists.txt +++ /dev/null @@ -1,28 +0,0 @@ -set(TARGET_NAME wbc-robot_models-kdl) - -file(GLOB SOURCES RELATIVE ${PROJECT_SOURCE_DIR}/src/robot_models/robot_model_kdl "*.cpp") -file(GLOB HEADERS RELATIVE ${PROJECT_SOURCE_DIR}/src/robot_models/robot_model_kdl "*.hpp") - -pkg_search_module(kdl_parser REQUIRED IMPORTED_TARGET kdl_parser) - -list(APPEND PKGCONFIG_REQUIRES wbc-core) -list(APPEND PKGCONFIG_REQUIRES kdl_parser) -string (REPLACE ";" " " PKGCONFIG_REQUIRES "${PKGCONFIG_REQUIRES}") - -add_library(${TARGET_NAME} SHARED ${SOURCES} ${HEADERS}) -target_link_libraries(${TARGET_NAME} PUBLIC - wbc-core - PkgConfig::kdl_parser) - -set_target_properties(${TARGET_NAME} PROPERTIES - VERSION ${PROJECT_VERSION} - SOVERSION ${API_VERSION}) - -install(TARGETS ${TARGET_NAME} - LIBRARY DESTINATION lib) - -CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) -INSTALL(FILES ${HEADERS} DESTINATION include/wbc/robot_models/kdl) - -add_subdirectory(test) diff --git a/src/robot_models/kdl/KinematicChainKDL.cpp b/src/robot_models/kdl/KinematicChainKDL.cpp deleted file mode 100644 index e67419ad..00000000 --- a/src/robot_models/kdl/KinematicChainKDL.cpp +++ /dev/null @@ -1,110 +0,0 @@ -#include "KinematicChainKDL.hpp" -#include -#include -#include - -namespace wbc{ - -KinematicChainKDL::KinematicChainKDL(const KDL::Chain &_chain, const std::string &_root_frame, const std::string &_tip_frame) : - chain(_chain), - root_frame(_root_frame), - tip_frame(_tip_frame), - space_jacobian(KDL::Jacobian(chain.getNrOfJoints())), - body_jacobian(KDL::Jacobian(chain.getNrOfJoints())), - jacobian_dot(KDL::Jacobian(chain.getNrOfJoints())){ - - jnt_array_vel.q = KDL::JntArray(chain.getNrOfJoints()); - jnt_array_vel.qdot = KDL::JntArray(chain.getNrOfJoints()); - - jnt_array_acc.q = KDL::JntArray(chain.getNrOfJoints()); - jnt_array_acc.qdot = KDL::JntArray(chain.getNrOfJoints()); - jnt_array_acc.qdotdot = KDL::JntArray(chain.getNrOfJoints()); - for(size_t i = 0; i < chain.getNrOfSegments(); i++){ - KDL::Joint joint = chain.getSegment(i).getJoint(); - if(joint.getType() != KDL::Joint::None) - joint_names.push_back(joint.getName()); - } - - cartesian_state.frame_id = root_frame; - - has_acceleration = space_jacobian_is_up_to_date = body_jacobian_is_up_to_date = jac_dot_is_up_to_date = false; - - jac_solver = std::make_shared(chain); - jac_dot_solver = std::make_shared(chain); - fk_solver_vel = std::make_shared(chain); -} - -const base::samples::RigidBodyStateSE3 &KinematicChainKDL::rigidBodyState(){ - cartesian_state.pose.position << pose_kdl.p(0), pose_kdl.p(1), pose_kdl.p(2); - double x, y, z, w; - pose_kdl.M.GetQuaternion(x, y, z, w); - cartesian_state.pose.orientation = base::Quaterniond(w, x, y, z); - cartesian_state.twist.linear << twist_kdl.vel(0), twist_kdl.vel(1), twist_kdl.vel(2); - cartesian_state.twist.angular << twist_kdl.rot(0), twist_kdl.rot(1), twist_kdl.rot(2); - cartesian_state.acceleration.linear = acc.segment(0,3); - cartesian_state.acceleration.angular = acc.segment(3,3); - return cartesian_state; -} - -void KinematicChainKDL::update(const KDL::JntArray& q, const KDL::JntArray& qd, const KDL::JntArray& qdd, std::map joint_idx_map){ - - //// update Joints - for(size_t i = 0; i < joint_names.size(); i++){ - const std::string& name = joint_names[i]; - if(joint_idx_map.count(name) == 0){ - LOG_ERROR("Kinematic Chain %s to %s contains joint %s, but this joint is not in joint state vector", - chain.getSegment(0).getName().c_str(), chain.getSegment(chain.getNrOfSegments()-1).getName().c_str(), joint_names[i].c_str()); - throw std::invalid_argument("Invalid joint state"); - } - - uint idx = joint_idx_map[name]; - - jnt_array_vel.q(i) = jnt_array_acc.q(i) = q(idx); - jnt_array_vel.qdot(i) = jnt_array_acc.qdot(i) = qd(idx); - jnt_array_acc.qdotdot(i) = qdd(idx); - } - - space_jacobian_is_up_to_date = body_jacobian_is_up_to_date = jac_dot_is_up_to_date = fk_is_up_to_date = false; -} - -void KinematicChainKDL::calculateForwardKinematics(){ - fk_solver_vel->JntToCart(jnt_array_vel, frame_vel); - - twist_kdl = frame_vel.deriv(); - pose_kdl = frame_vel.value(); - - // Update jacobians if necessary - if(!space_jacobian_is_up_to_date) - calculateSpaceJacobian(); - if(!jac_dot_is_up_to_date) - calculateJacobianDot(); - - acc = jacobian_dot.data*jnt_array_vel.qdot.data + space_jacobian.data*jnt_array_acc.qdotdot.data; - - fk_is_up_to_date = true; -} - -void KinematicChainKDL::calculateSpaceJacobian(){ - if(jac_solver->JntToJac(jnt_array_vel.q, space_jacobian)) - throw std::runtime_error("Failed to compute Jacobian for chain " + root_frame + " -> " + tip_frame); - space_jacobian_is_up_to_date = true; -} - -void KinematicChainKDL::calculateBodyJacobian(){ - if(!fk_is_up_to_date) - calculateForwardKinematics(); - if(!space_jacobian_is_up_to_date) - calculateSpaceJacobian(); - body_jacobian = space_jacobian; - body_jacobian.changeBase(pose_kdl.M.Inverse()); - body_jacobian_is_up_to_date = true; -} - -void KinematicChainKDL::calculateJacobianDot(){ - jac_dot_solver->setRepresentation(KDL::ChainJntToJacDotSolver::HYBRID); // 0 - Hybrid represenation -> ref frame is root, ref point is tip - if(jac_dot_solver->JntToJacDot(jnt_array_vel, jacobian_dot)) - throw std::runtime_error("Failed to compute JacobianDot for chain " + root_frame + " -> " + tip_frame); - jac_dot_is_up_to_date = true; -} - -} // namespace wbc diff --git a/src/robot_models/kdl/KinematicChainKDL.hpp b/src/robot_models/kdl/KinematicChainKDL.hpp deleted file mode 100644 index 655078e0..00000000 --- a/src/robot_models/kdl/KinematicChainKDL.hpp +++ /dev/null @@ -1,76 +0,0 @@ -#ifndef KINMATICCHAINKDL_HPP -#define KINMATICCHAINKDL_HPP - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace base{ - namespace samples{ - class Joints; - } -} -namespace wbc{ - -/** - * @brief Helper class for storing information of a KDL chain in the robot model -*/ -class KinematicChainKDL{ - -protected: - base::samples::RigidBodyStateSE3 cartesian_state; - -public: - KinematicChainKDL(const KDL::Chain &chain, const std::string &root_frame, const std::string &tip_frame); - - /** - * @brief Update all joints of the kinematic chain - * @param joint_state Has to contain at least all joints that are included in the kinematic chain. Each entry has to have a valid position, velocity and acceleration - */ - void update(const KDL::JntArray& q, const KDL::JntArray& qd, const KDL::JntArray& qdd, std::map joint_idx_map); - /** Convert and return current Cartesian state*/ - const base::samples::RigidBodyStateSE3& rigidBodyState(); - - /** Compute FK (pose, twist,spatial acc) for the chain using the current joint state. Note: This will call - * calculateSpaceJacobian() and calculateJacobianDot() if space_jacobian and jacobian_dot are not up to date*/ - void calculateForwardKinematics(); - /** Compute space Jacobian using the current joint state*/ - void calculateSpaceJacobian(); - /** Compute body Jacobian using the current joint state. Note: This will call calculateSpaceJacobian() if space_jacobian is not up to date*/ - void calculateBodyJacobian(); - /** Compute derivative of space Jacobian (hybrid representation) using the current joint state*/ - void calculateJacobianDot(); - - - KDL::Frame pose_kdl; /** KDL Pose of the tip segment in root coordinate of the chain*/ - KDL::FrameVel frame_vel; /** Helper for the velocity fk*/ - KDL::Twist twist_kdl; /** KDL Pose of the tip segment in root coordinate of the chain*/ - base::Vector6d acc; /** Helper to store current frame acceleration*/ - KDL::Chain chain; /** The underlying KDL chain*/ - KDL::JntArrayVel jnt_array_vel; /** Vector of positions and velocities of all included joints*/ - KDL::JntArrayAcc jnt_array_acc; /** Vector of positions, velocities and accelerations of all included joints*/ - KDL::Jacobian space_jacobian; /** Space Jacobian of the Chain. Reference frame is root & reference point is tip*/ - KDL::Jacobian body_jacobian; /** Body Jacobian of the Chain. Reference frame is root & reference point is tip*/ - KDL::Jacobian jacobian_dot; /** Derivative of Jacobian of the Chain. Reference frame & reference point is the root frame*/ - std::vector joint_names; /** Names of the joint included in the kinematic chain*/ - std::string root_frame; /** UID of the kinematics chain root link*/ - std::string tip_frame; /** UID of the kinematics chain tip link*/ - base::Time stamp; - bool has_acceleration, space_jacobian_is_up_to_date, body_jacobian_is_up_to_date, jac_dot_is_up_to_date, fk_is_up_to_date; - std::shared_ptr jac_solver; - std::shared_ptr fk_solver_vel; - std::shared_ptr jac_dot_solver; - -}; - -} // namespace wbc -#endif diff --git a/src/robot_models/kdl/RobotModelKDL.cpp b/src/robot_models/kdl/RobotModelKDL.cpp deleted file mode 100644 index 7bfeac78..00000000 --- a/src/robot_models/kdl/RobotModelKDL.cpp +++ /dev/null @@ -1,515 +0,0 @@ -#include "RobotModelKDL.hpp" -#include "KinematicChainKDL.hpp" -#include -#include -#include "../../core/RobotModelConfig.hpp" -#include -#include -#include -#include - -namespace wbc{ - -RobotModelRegistry RobotModelKDL::reg("kdl"); - -RobotModelKDL::RobotModelKDL(){ -} - -RobotModelKDL::~RobotModelKDL(){ -} - -void RobotModelKDL::clear(){ - - RobotModel::clear(); - - full_tree = KDL::Tree(); - kdl_chain_map.clear(); -} - -bool RobotModelKDL::configure(const RobotModelConfig& cfg){ - - clear(); - - // 1. Load Robot Model - - robot_model_config = cfg; - robot_urdf = loadRobotURDF(cfg.file_or_string); - if(!robot_urdf){ - LOG_ERROR("Unable to parse urdf model") - return false; - } - URDFTools::applyJointBlacklist(robot_urdf, cfg.joint_blacklist); - base_frame = robot_urdf->getRoot()->name; - - // Joint names from URDF without floating base - std::vector joint_names_urdf = URDFTools::jointNamesFromURDF(robot_urdf); - - // Add floating base - has_floating_base = cfg.floating_base; - world_frame = base_frame; - if(cfg.floating_base){ - joint_names_floating_base = URDFTools::addFloatingBaseToURDF(robot_urdf); - world_frame = robot_urdf->getRoot()->name; - } - - // Read Joint Limits - URDFTools::jointLimitsFromURDF(robot_urdf, joint_limits); - - // Parse KDL Tree - if(!kdl_parser::treeFromUrdfModel(*robot_urdf, full_tree)){ - LOG_ERROR("Unable to load KDL Tree"); - return false; - } - - joint_names = independent_joint_names = joint_names_floating_base + joint_names_urdf; - actuated_joint_names = joint_names_urdf; - joint_state.elements.resize(joint_names.size()); - joint_state.names = joint_names; - - // 2. Verify consistency of URDF and config - - // All contact point have to be a valid link in the robot URDF - for(auto c : cfg.contact_points.names){ - if(!hasLink(c)){ - LOG_ERROR("Contact point %s is not a valid link in the robot model", c.c_str()); - return false; - } - } - - // 3. Create data structures - - q.resize(noOfJoints()); - qd.resize(noOfJoints()); - qdd.resize(noOfJoints()); - tau.resize(noOfJoints()); - zero.resize(noOfJoints()); - zero.data.setZero(); - active_contacts = cfg.contact_points; - joint_space_inertia_mat.resize(noOfJoints(), noOfJoints()); - bias_forces.resize(noOfJoints()); - selection_matrix.resize(noOfActuatedJoints(),noOfJoints()); - selection_matrix.setZero(); - for(int i = 0; i < actuated_joint_names.size(); i++) - selection_matrix(i, jointIndex(actuated_joint_names[i])) = 1.0; - - for(const auto &it : full_tree.getSegments()){ - KDL::Joint jnt = it.second.segment.getJoint(); - if(jnt.getType() != KDL::Joint::None) - joint_idx_map_kdl[jnt.getName()] = GetTreeElementQNr(it.second); - } - - // 5. Print some debug info - - LOG_DEBUG("------------------- WBC RobotModelKDL -----------------"); - LOG_DEBUG_S << "Robot Name " << robot_urdf->getName() << std::endl; - LOG_DEBUG_S << "Floating base robot: " << has_floating_base << std::endl; - LOG_DEBUG("Joint Names"); - for(auto n : jointNames()) - LOG_DEBUG_S << n << std::endl; - LOG_DEBUG("Actuated Joint Names"); - for(auto n : actuatedJointNames()) - LOG_DEBUG_S << n << std::endl; - LOG_DEBUG("Joint Limits"); - for(auto n : joint_limits.names) - LOG_DEBUG_S << n << ": Max. Pos: " << joint_limits[n].max.position << ", " - << " Min. Pos: " << joint_limits[n].min.position << ", " - << " Max. Vel: " << joint_limits[n].max.speed << ", " - << " Max. Eff: " << joint_limits[n].max.effort << std::endl; - LOG_DEBUG("------------------------------------------------------------"); - - return true; -} - -void RobotModelKDL::createChain(const std::string &root_frame, const std::string &tip_frame){ - createChain(full_tree, root_frame, tip_frame); -} - -void RobotModelKDL::createChain(const KDL::Tree& tree, const std::string &root_frame, const std::string &tip_frame){ - KDL::Chain chain; - if(!tree.getChain(root_frame, tip_frame, chain)){ - LOG_ERROR("Unable to extract kinematics chain from %s to %s from KDL tree", root_frame.c_str(), tip_frame.c_str()); - throw std::invalid_argument("Invalid robot model config"); - } - - const std::string chain_id = chainID(root_frame, tip_frame); - - KinematicChainKDLPtr kin_chain = std::make_shared(chain, root_frame, tip_frame); - kin_chain->update(q,qd,qdd,joint_idx_map_kdl); - kdl_chain_map[chain_id] = kin_chain; - - LOG_INFO_S<<"Added chain "< "< joint_state.time) - joint_state.time = floating_base_state.time; - } - - // Update actuated joints - for(size_t i = 0; i < noOfActuatedJoints(); i++){ - const std::string &name = actuated_joint_names[i]; - if(!hasJoint(name)){ - LOG_ERROR_S << "Joint " << name << " is a non-fixed joint in the KDL Tree, but it is not in the joint state vector." - << "You should either set the joint to 'fixed' in your URDF file or provide a valid joint state for it" << std::endl; - throw std::runtime_error("Incomplete Joint State"); - } - const base::JointState &state = joint_state_in[name]; - uint idx = joint_idx_map_kdl[name]; - q(idx) = state.position; - qd(idx) = state.speed; - qdd(idx) = state.acceleration; - } - - for(auto c : kdl_chain_map) - c.second->update(q,qd,qdd,joint_idx_map_kdl); -} - -void RobotModelKDL::systemState(base::VectorXd &_q, base::VectorXd &_qd, base::VectorXd &_qdd){ - _q.resize(noOfJoints()); - _qd.resize(noOfJoints()); - _qdd.resize(noOfJoints()); - - for(int i = 0; i < noOfJoints(); i++){ - const std::string& name = jointNames()[i]; - uint idx = joint_idx_map_kdl[name]; - _q[i] = q(idx); - _qd[i] = qd(idx); - _qdd[i] = qdd(idx); - } -} - -const base::samples::RigidBodyStateSE3 &RobotModelKDL::rigidBodyState(const std::string &root_frame, const std::string &tip_frame){ - - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to rigidBodyState()"); - } - - // Create chain if it does not exist - const std::string chain_id = chainID(root_frame, tip_frame); - if(kdl_chain_map.count(chain_id) == 0) - createChain(root_frame, tip_frame); - - KinematicChainKDLPtr kdl_chain = kdl_chain_map[chainID(root_frame, tip_frame)]; - kdl_chain->calculateForwardKinematics(); - rbs = kdl_chain->rigidBodyState(); - - return rbs; -} - -const base::MatrixXd& RobotModelKDL::spaceJacobian(const std::string &root_frame, const std::string &tip_frame){ - return spaceJacobianFromTree(full_tree, root_frame, tip_frame); -} - -const base::MatrixXd& RobotModelKDL::spaceJacobianFromTree(const KDL::Tree& tree, const std::string &root_frame, const std::string &tip_frame){ - - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to rigidBodyState()"); - } - - // Create chain if it does not exist - const std::string chain_id = chainID(root_frame, tip_frame); - if(kdl_chain_map.count(chain_id) == 0) - createChain(tree, root_frame, tip_frame); - - KinematicChainKDLPtr kdl_chain = kdl_chain_map[chain_id]; - kdl_chain->calculateSpaceJacobian(); - - space_jac_map[chain_id].resize(6,noOfJoints()); - space_jac_map[chain_id].setZero(); - for(uint j = 0; j < kdl_chain->joint_names.size(); j++){ - int idx = jointIndex(kdl_chain->joint_names[j]); - space_jac_map[chain_id].col(idx) = kdl_chain->space_jacobian.data.col(j); - } - return space_jac_map[chain_id]; -} - -const base::MatrixXd& RobotModelKDL::bodyJacobian(const std::string &root_frame, const std::string &tip_frame){ - - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to rigidBodyState()"); - } - - // Create chain if it does not exist - const std::string chain_id = chainID(root_frame, tip_frame); - if(kdl_chain_map.count(chain_id) == 0) - createChain(root_frame, tip_frame); - - KinematicChainKDLPtr kdl_chain = kdl_chain_map[chain_id]; - kdl_chain->calculateBodyJacobian(); - - body_jac_map[chain_id].resize(6,noOfJoints()); - body_jac_map[chain_id].setZero(); - for(uint j = 0; j < kdl_chain->joint_names.size(); j++){ - int idx = jointIndex(kdl_chain->joint_names[j]); - body_jac_map[chain_id].col(idx) = kdl_chain->body_jacobian.data.col(j); - } - return body_jac_map[chain_id]; -} - - -const base::MatrixXd &RobotModelKDL::comJacobian(){ - - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to rigidBodyState()"); - } - - com_jac = base::MatrixXd::Zero(3, noOfJoints()); - - // create a copy in which, for each segment with mass, COG frames are added - KDL::Tree tree_cog_frames = full_tree; - - double totalMass = 0; - std::map mass_map; - - std::vector COGSegmentNames; - for(auto& segment_it : full_tree.getSegments()) - { - auto segment = segment_it.second.segment; - std::string segmentName = segment.getName(); - // Skip root segment! To get consistent results with RBDL, Pinocchio, etc. we have to start with the first child of Root segment! Why? - if(segmentName == full_tree.getRootSegment()->second.segment.getName()) - continue; - std::string segmentNameCOG = segmentName + "_COG"; - - KDL::Vector segmentCOG = segment.getInertia().getCOG(); - double segmentMass = segment.getInertia().getMass(); - - if(segmentMass == 0.0) - continue; - - KDL::Frame frameCOG(KDL::Rotation(), segmentCOG); - tree_cog_frames.addSegment(KDL::Segment(segmentNameCOG, KDL::Joint(KDL::Joint::Fixed), frameCOG), segmentName); - - mass_map[segmentNameCOG] = segmentMass; - totalMass += segmentMass; - - COGSegmentNames.push_back(segmentNameCOG); - } - - // compute com jacobian as (mass) weighted average over COG frame jacobians - for(const auto& segment_name : COGSegmentNames) - com_jac += (mass_map[segment_name] / totalMass) * - spaceJacobianFromTree(tree_cog_frames, tree_cog_frames.getRootSegment()->second.segment.getName(), segment_name).topRows<3>(); - - return com_jac; -} - -const base::MatrixXd &RobotModelKDL::jacobianDot(const std::string &root_frame, const std::string &tip_frame){ - - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to jacobianDot()"); - } - - // Create chain if it does not exist - const std::string chain_id = chainID(root_frame, tip_frame); - if(kdl_chain_map.count(chain_id) == 0) - createChain(root_frame, tip_frame); - - KinematicChainKDLPtr kdl_chain = kdl_chain_map[chain_id]; - kdl_chain->calculateJacobianDot(); - - jac_dot_map[chain_id].resize(6,noOfJoints()); - jac_dot_map[chain_id].setZero(); - for(uint j = 0; j < kdl_chain->joint_names.size(); j++){ - int idx = jointIndex(kdl_chain->joint_names[j]); - jac_dot_map[chain_id].col(idx) = kdl_chain->jacobian_dot.data.col(j); - } - return jac_dot_map[chain_id]; -} - -const base::Acceleration &RobotModelKDL::spatialAccelerationBias(const std::string &root_frame, const std::string &tip_frame){ - qdot_tmp.resize(noOfJoints()); - tmp_acc.resize(6); - for(int i = 0; i < joint_names.size(); i++) - qdot_tmp[i] = qd(joint_idx_map_kdl[joint_names[i]]); - tmp_acc = jacobianDot(root_frame, tip_frame)*qdot_tmp; - spatial_acc_bias = base::Acceleration(tmp_acc.segment(0,3), tmp_acc.segment(3,3)); - return spatial_acc_bias; -} - -const base::VectorXd &RobotModelKDL::biasForces(){ - - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to jacobianDot()"); - } - - // Use ID solver with zero joint accelerations and zero external wrenches to get bias forces/torques - KDL::TreeIdSolver_RNE solver(full_tree, KDL::Vector(gravity(0), gravity(1), gravity(2))); - solver.CartToJnt(q, qd, zero, std::map(), tau); - - for(uint i = 0; i < joint_names.size(); i++){ - const std::string &name = joint_names[i]; - uint idx = joint_idx_map_kdl[name]; - bias_forces[i] = tau(idx); - } - return bias_forces; -} - -const base::MatrixXd& RobotModelKDL::jointSpaceInertiaMatrix(){ - - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to jacobianDot()"); - } - - joint_space_inertia_mat.setZero(); - - // Use ID solver with zero bias and external wrenches to compute joint space inertia matrix column by column. - // TODO: Switch to more efficient method! - KDL::TreeIdSolver_RNE solver(full_tree, KDL::Vector::Zero()); - for(uint i = 0; i < joint_names.size(); i++){ - const std::string& name = joint_names[i]; - qdd.data.setZero(); - qdd(joint_idx_map_kdl[name]) = 1; - int ret = solver.CartToJnt(q, zero, qdd, std::map(), tau); - if(ret != 0) - throw(std::runtime_error("Unable to compute Tree Inverse Dynamics in joint space inertia matrix computation. Error Code is " + std::to_string(ret))); - for(int j = 0; j < joint_names.size(); j++){ - const std::string& _name = joint_names[j]; - joint_space_inertia_mat(j, i) = tau(joint_idx_map_kdl[_name]); - } - } - return joint_space_inertia_mat; -} - - -void RobotModelKDL::recursiveCOM(const KDL::SegmentMap::const_iterator& currentSegment, const KDL::Frame& frame, double& mass, KDL::Vector& cog) -{ - double jointPosition = 0.0; // Joint position of the current joint -#ifdef KDL_USE_NEW_TREE_INTERFACE - KDL::Segment segment = currentSegment->second->segment; -#else - KDL::Segment segment = currentSegment->second.segment; -#endif - // If the segment has a real joint, get the joint position - if( segment.getJoint().getType() != KDL::Joint::None ) { - const std::string name = segment.getJoint().getName(); - if(joint_idx_map_kdl.count(name)==0){ - LOG_ERROR_S << "recursiveCOM: KDL tree contains joint " << name << " but this joint does not exist in joint idx map" << std::endl; - throw std::runtime_error("Problem with model configuration"); - } - uint idx = joint_idx_map_kdl[name]; - jointPosition = q(idx); - } - - // Transform to the frame of the current segment - KDL::Frame currentFrame = frame * segment.pose( jointPosition ); - // Get the COG of the current segemnt in the KDL tree - KDL::Vector currentCOG = segment.getInertia().getCOG(); - // Gets the current mass - double currentMass = segment.getInertia().getMass(); - - // Computes the cog - cog += currentMass * ( currentFrame * currentCOG ); - // Appends to the total mass - mass += currentMass; - - // Iterates through all the child segments - std::vector< KDL::SegmentMap::const_iterator >::const_iterator childSegment; -#ifdef KDL_USE_NEW_TREE_INTERFACE - for( childSegment = currentSegment->second->children.begin(); - childSegment != currentSegment->second->children.end(); -#else - for( childSegment = currentSegment->second.children.begin(); - childSegment != currentSegment->second.children.end(); -#endif - ++childSegment) - { - // Calls this functiion again with the child segment - recursiveCOM( *childSegment, currentFrame, mass, cog ); - } -} - -const base::samples::RigidBodyStateSE3& RobotModelKDL::centerOfMass(){ - - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to centerOfMass()"); - } - - double mass = 0.0; // to get the total mass - KDL::Frame frame = KDL::Frame::Identity(); // Transformation of the last frame - - // Computes the COG of the complete robot - KDL::Vector cog_pos; - // To get consistent results with RBDL, Pinocchio, etc. we have to start with the first child of Root link! Why? - recursiveCOM( full_tree.getRootSegment()->second.children[0], frame, mass, cog_pos ); - - // Returns the COG - com_rbs.frame_id = world_frame; - com_rbs.pose.position = base::Vector3d( cog_pos.x(), cog_pos.y(), cog_pos.z() ) / mass; - com_rbs.pose.orientation.setIdentity(); - com_rbs.time = joint_state.time; - return com_rbs; -} - -void RobotModelKDL::computeInverseDynamics(base::commands::Joints &solver_output){ - if(joint_state.time.isNull()){ - LOG_ERROR("RobotModelKDL: You have to call update() with appropriately timestamped joint data at least once before requesting kinematic information!"); - throw std::runtime_error(" Invalid call to jacobianDot()"); - } - - KDL::TreeIdSolver_RNE solver(full_tree, KDL::Vector(gravity[0], gravity[1], gravity[2])); - KDL::WrenchMap w_map; - for(auto n : contact_wrenches.names) - w_map[n] = KDL::Wrench(KDL::Vector(contact_wrenches[n].force[0], - contact_wrenches[n].force[1], - contact_wrenches[n].force[2]), - KDL::Vector(contact_wrenches[n].torque[0], - contact_wrenches[n].torque[1], - contact_wrenches[n].torque[2])); - int ret = solver.CartToJnt(q, qd, qdd, w_map, tau); - if(ret != 0) - throw(std::runtime_error("Unable to compute Tree Inverse Dynamics. Error Code is " + std::to_string(ret))); - - for(uint i = 0; i < noOfActuatedJoints(); i++){ - const std::string& name = actuated_joint_names[i]; - solver_output[name].effort = tau(joint_idx_map_kdl[name]); - } -} -} diff --git a/src/robot_models/kdl/RobotModelKDL.hpp b/src/robot_models/kdl/RobotModelKDL.hpp deleted file mode 100644 index e35668dd..00000000 --- a/src/robot_models/kdl/RobotModelKDL.hpp +++ /dev/null @@ -1,172 +0,0 @@ -#ifndef ROBOTMODELKDL_HPP -#define ROBOTMODELKDL_HPP - -#include "../../core/RobotModel.hpp" - -#include -#include -#include -#include -#include - -namespace wbc{ - -class KinematicChainKDL; - -/** - * @brief This model describes the kinemetic relationships required for velocity based wbc. It is based on a single KDL Tree. However, multiple KDL trees can be added - * and will be appropriately concatenated. This way you can describe e.g. geometric robot-object relationships or create multi-robot scenarios. - */ -class RobotModelKDL : public RobotModel{ -protected: - - static RobotModelRegistry reg; - - typedef std::shared_ptr KinematicChainKDLPtr; - typedef std::map KinematicChainKDLMap; - - KDL::JntArray q,qd,qdd,tau,zero; - base::VectorXd qdot_tmp; - base::VectorXd tmp_acc; - - KDL::Tree full_tree; /** Overall kinematic tree*/ - std::map joint_idx_map_kdl; - KinematicChainKDLMap kdl_chain_map; /** Map of KDL Chains*/ - - /** - * @brief Create a KDL chain and add it to the KDL Chain map. Throws an exception if chain cannot be extracted from KDL Tree - * @param root_frame Root frame of the chain - * @param tip_frame Tip frame of the chain - */ - void createChain(const std::string &root_frame, const std::string &tip_frame); - - /** - * @brief Create a KDL chain and add it to the KDL Chain map. Throws an exception if chain cannot be extracted from KDL Tree - * @param tree tree from which the kinematic chain is extracted - * @param root_frame Root frame of the chain - * @param tip_frame Tip frame of the chain - */ - void createChain(const KDL::Tree& tree, const std::string &root_frame, const std::string &tip_frame); - - /** Add a KDL Tree to the model. If the model is empty, the overall KDL::Tree will be replaced by the given tree. If there - * is already a KDL Tree, the new tree will be attached with the given pose to the hook frame of the overall tree. The relative poses - * of the trees can be updated online by calling update() with poses parameter appropriately set. This will also create the - * joint index map*/ - - /** Free storage and clear data structures*/ - void clear(); - - /** - * Recursively loops through all the tree segments and compute the - * COG of the complete tree. - * @param currentSegement Current Segement in the recursive loop - * @param status Current joint state - */ - void recursiveCOM( const KDL::SegmentMap::const_iterator& currentSegment, - const KDL::Frame& frame, - double& mass, KDL::Vector& cog); - - /** @brief Returns the Space Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. Size of the Jacobian will be 6 x nJoints, where nJoints is the number of joints of the whole robot. The order of the - * columns will be the same as the joint order of the robot. The columns that correspond to joints that are not part of the kinematic chain will have only zeros as entries. - * @param tree kinematic tree from which the jacobian is computed. - * @param root_frame Root frame of the chain. Has to be a valid link in the robot model. - * @param tip_frame Tip frame of the chain. Has to be a valid link in the robot model. - */ - virtual const base::MatrixXd &spaceJacobianFromTree(const KDL::Tree& tree, const std::string &root_frame, const std::string &tip_frame); - - -public: - RobotModelKDL(); - virtual ~RobotModelKDL(); - - /** - * @brief Load and configure the robot model. In this implementation, each model config constains a URDF file that will be parsed into a KDL tree. - * If the overall model is empty, the overall KDL::Tree will be replaced by the given tree. If there - * is already a KDL Tree, the new tree will be attached with the given pose to the 'hook' frame of the overall tree. The relative poses - * of the trees can be updated online by calling update() with poses parameter appropriately set. - * @param model_config The models configuration(s). These include the path to the URDF model file(s), the relative poses and hooks - * to which the models shall be attached. This way you can add multiple robot model tree and attach them to each other. - * @param base_frame Base frame of the model. If left empty, the base will be selected as the root frame of the first URDF model. - * @return True in case of success, else false - */ - virtual bool configure(const RobotModelConfig& cfg); - - /** - * @brief Update the robot model. The joint state has to contain all joints that are relevant in the model. This means: All joints that are ever required - * when requesting rigid body states, Jacobians or joint states. Note that - * @param joint_state The joint_state vector. Has to contain all robot joints. - * @param poses Optionally update links of the robot model. This can be used to update e.g. the relative position between two robots in the model. The source frame - * of the given rigid body state has to match the segment name in the KDL Tree that shall be updated - */ - virtual void update(const base::samples::Joints& joint_state, - const base::samples::RigidBodyStateSE3& floating_base_state = base::samples::RigidBodyStateSE3()); - - /** Return entire system state*/ - virtual void systemState(base::VectorXd &q, base::VectorXd &qd, base::VectorXd &qdd); - - /** - * @brief Computes and returns the relative transform between the two given frames. By convention this is the pose of the tip frame in root coordinates. - * This will create a kinematic chain between root and tip frame, if called for the first time with the given arguments. - * @param root_frame Root frame of the chain. Has to be a valid link in the robot model. - * @param tip_frame Tip frame of the chain. Has to be a valid link in the robot model. - */ - virtual const base::samples::RigidBodyStateSE3 &rigidBodyState(const std::string &root_frame, const std::string &tip_frame); - - /** @brief Returns the Space Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. Size of the Jacobian will be 6 x nJoints, where nJoints is the number of joints of the whole robot. The order of the - * columns will be the same as the joint order of the robot. The columns that correspond to joints that are not part of the kinematic chain will have only zeros as entries. - * @param root_frame Root frame of the chain. Has to be a valid link in the robot model. - * @param tip_frame Tip frame of the chain. Has to be a valid link in the robot model. - */ - virtual const base::MatrixXd &spaceJacobian(const std::string &root_frame, const std::string &tip_frame); - - /** @brief Returns the Body Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. Size of the Jacobian will be 6 x nJoints, where nJoints is the number of joints of the whole robot. The order of the - * columns will be the same as the joint order of the robot. The columns that correspond to joints that are not part of the kinematic chain will have only zeros as entries. - * @param root_frame Root frame of the chain. Has to be a valid link in the robot model. - * @param tip_frame Tip frame of the chain. Has to be a valid link in the robot model. - */ - virtual const base::MatrixXd &bodyJacobian(const std::string &root_frame, const std::string &tip_frame); - - - /** @brief Returns the CoM Jacobian for the entire robot, which maps the robot joint velocities to linear spatial velocities in robot base coordinates. - * Size of the Jacobian will be 3 x nJoints, where nJoints is the number of joints of the whole robot. The order of the - * columns will be the same as the configured joint order of the robot. - * @return A 3xN matrix, where N is the number of robot joints - */ - virtual const base::MatrixXd &comJacobian(); - - /** @brief Returns the derivative of the Jacobian for the kinematic chain between root and the tip frame as full body Jacobian. By convention reference frame & reference point - * of the Jacobian will be the root frame (corresponding to the body Jacobian). Size of the Jacobian will be 6 x nJoints, where nJoints is the number of joints of the whole robot. The order of the - * columns will be the same as the joint order of the robot. The columns that correspond to joints that are not part of the kinematic chain will have only zeros as entries. - * @param root_frame Root frame of the chain. Has to be a valid link in the robot model. - * @param tip_frame Tip frame of the chain. Has to be a valid link in the robot model. - * @return A 6xN Jacobian derivative matrix, where N is the number of robot joints - */ - virtual const base::MatrixXd &jacobianDot(const std::string &root_frame, const std::string &tip_frame); - - /** @brief Returns the spatial acceleration bias, i.e. the term Jdot*qdot - * @param root_frame Root frame of the chain. Has to be a valid link in the robot model. - * @param tip_frame Tip frame of the chain. Has to be a valid link in the robot model. - * @return A Nx1 vector, where N is the number of robot joints - */ - virtual const base::Acceleration &spatialAccelerationBias(const std::string &root_frame, const std::string &tip_frame); - - /** Compute and return the joint space mass-inertia matrix, which is nj x nj, where nj is the number of joints of the system*/ - virtual const base::MatrixXd &jointSpaceInertiaMatrix(); - - /** Compute and return the bias force vector, which is nj x 1, where nj is the number of joints of the system*/ - virtual const base::VectorXd &biasForces(); - - /** Return full tree (KDL model)*/ - KDL::Tree getTree(){return full_tree;} - - /** @brief Compute and return center of mass expressed in base frame*/ - virtual const base::samples::RigidBodyStateSE3& centerOfMass(); - - /** @brief Compute and return the inverse dynamics solution*/ - virtual void computeInverseDynamics(base::commands::Joints &solver_output); - -}; - -} - -#endif // KINEMATICMODEL_HPP diff --git a/src/robot_models/kdl/test/CMakeLists.txt b/src/robot_models/kdl/test/CMakeLists.txt deleted file mode 100644 index e5abb73f..00000000 --- a/src/robot_models/kdl/test/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -add_executable(test_robot_model_kdl test_robot_model_kdl.cpp ../../test/test_robot_model.cpp) -target_link_libraries(test_robot_model_kdl - wbc-robot_models-kdl - Boost::unit_test_framework) - -add_test(NAME test_robot_model_kdl COMMAND test_robot_model_kdl) diff --git a/src/robot_models/kdl/test/test_robot_model_kdl.cpp b/src/robot_models/kdl/test/test_robot_model_kdl.cpp deleted file mode 100644 index 3177c9f3..00000000 --- a/src/robot_models/kdl/test/test_robot_model_kdl.cpp +++ /dev/null @@ -1,177 +0,0 @@ -#define BOOST_TEST_DYN_LINK -#define BOOST_TEST_MAIN -#include -#include "../RobotModelKDL.hpp" -#include "../KinematicChainKDL.hpp" -#include "../../../core/RobotModelConfig.hpp" -#include -#include -#include -#include "../../../tools/URDFTools.hpp" -#include -#include "../../test/test_robot_model.hpp" - -using namespace std; -using namespace wbc; - -BOOST_AUTO_TEST_CASE(configure_and_update){ - - /** - * Verify that the robot model fails to configure with invalid configurations - */ - - RobotModelConfig config; - RobotModelKDL robot_model; - - std::vector joint_names = {"kuka_lbr_l_joint_1", - "kuka_lbr_l_joint_2", - "kuka_lbr_l_joint_3", - "kuka_lbr_l_joint_4", - "kuka_lbr_l_joint_5", - "kuka_lbr_l_joint_6", - "kuka_lbr_l_joint_7"}; - std::vector floating_base_names = {"floating_base_trans_x", "floating_base_trans_y", "floating_base_trans_z", - "floating_base_rot_x", "floating_base_rot_y", "floating_base_rot_z"}; - - // Valid config - config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); - BOOST_CHECK(robot_model.configure(config) == true); - for(size_t i = 0; i < robot_model.noOfJoints(); i++){ - BOOST_CHECK(joint_names[i] == robot_model.jointNames()[i]); - BOOST_CHECK(joint_names[i] == robot_model.actuatedJointNames()[i]); - BOOST_CHECK(robot_model.hasActuatedJoint(joint_names[i])); - BOOST_CHECK(robot_model.hasJoint(joint_names[i])); - BOOST_CHECK(robot_model.jointIndex(joint_names[i]) == i); - } - BOOST_CHECK(robot_model.baseFrame() == "kuka_lbr_l_link_0"); - BOOST_CHECK(robot_model.worldFrame() == "kuka_lbr_l_link_0"); - BOOST_CHECK(robot_model.hasLink("kuka_lbr_l_link_0")); - - base::samples::Joints joint_state_in = makeRandomJointState(joint_names); - BOOST_CHECK_NO_THROW(robot_model.update(joint_state_in)); - base::samples::Joints joint_state_out = robot_model.jointState(joint_names); - for(auto n : joint_names){ - BOOST_CHECK(joint_state_out[n].position = joint_state_in[n].position); - BOOST_CHECK(joint_state_out[n].speed = joint_state_in[n].speed); - BOOST_CHECK(joint_state_out[n].acceleration = joint_state_in[n].acceleration); - } - - // Invalid filename - config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urd"); - BOOST_CHECK(robot_model.configure(config) == false); - - // Empty filename - config = RobotModelConfig(""); - BOOST_CHECK(robot_model.configure(config) == false); - - // Valid config with floating base - config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.submechanism_file = "../../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; - config.floating_base = true; - BOOST_CHECK(robot_model.configure(config) == true); - for(size_t i = 0; i < 6; i++){ - BOOST_CHECK(floating_base_names[i] == robot_model.jointNames()[i]); - BOOST_CHECK(floating_base_names[i] == robot_model.independentJointNames()[i]); - BOOST_CHECK(i == robot_model.jointIndex(floating_base_names[i])); - } - for(size_t i = 0; i < robot_model.noOfActuatedJoints(); i++){ - BOOST_CHECK(joint_names[i] == robot_model.jointNames()[i+6]); - BOOST_CHECK(joint_names[i] == robot_model.actuatedJointNames()[i]); - BOOST_CHECK(joint_names[i] == robot_model.independentJointNames()[i+6]); - BOOST_CHECK(i+6 == robot_model.jointIndex(joint_names[i])); - } - - base::samples::RigidBodyStateSE3 floating_base_state_in = makeRandomFloatingBaseState(); - BOOST_CHECK_NO_THROW(robot_model.update(joint_state_in, floating_base_state_in)); - - base::samples::RigidBodyStateSE3 floating_base_state_out = robot_model.floatingBaseState(); - joint_state_out = robot_model.jointState(joint_names); - for(auto n : joint_names){ - BOOST_CHECK(joint_state_out[n].position = joint_state_in[n].position); - BOOST_CHECK(joint_state_out[n].speed = joint_state_in[n].speed); - BOOST_CHECK(joint_state_out[n].acceleration = joint_state_in[n].acceleration); - } - for(int i = 0; i < 3; i++){ - BOOST_CHECK(floating_base_state_in.pose.position[i] == floating_base_state_out.pose.position[i]); - BOOST_CHECK(floating_base_state_in.pose.orientation.coeffs()[i] == floating_base_state_out.pose.orientation.coeffs()[i]); - BOOST_CHECK(floating_base_state_in.twist.linear[i] == floating_base_state_out.twist.linear[i]); - BOOST_CHECK(floating_base_state_in.twist.angular[i] == floating_base_state_out.twist.angular[i]); - BOOST_CHECK(floating_base_state_in.acceleration.linear[i] == floating_base_state_out.acceleration.linear[i]); - BOOST_CHECK(floating_base_state_in.acceleration.angular[i] == floating_base_state_out.acceleration.angular[i]); - } - - // Config with contact points - config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.contact_points.names.push_back("kuka_lbr_l_tcp"); - config.contact_points.elements.push_back(ActiveContact(1,0.6)); - BOOST_CHECK(robot_model.configure(config) == true); - - // Config with invalid contact points - config = RobotModelConfig("../../../../../models/kuka/urdf/kuka_iiwa.urdf"); - config.contact_points.names.push_back("XYZ"); - config.contact_points.elements.push_back(ActiveContact(1,0.6)); - BOOST_CHECK(robot_model.configure(config) == false); - -} - -BOOST_AUTO_TEST_CASE(fk){ - string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; - string tip_frame = "kuka_lbr_l_tcp"; - - RobotModelPtr robot_model = make_shared(); - RobotModelConfig cfg(urdf_file); - cfg.floating_base = true; - BOOST_CHECK(robot_model->configure(cfg)); - - testSpaceJacobian(robot_model, tip_frame, false); -} - - -BOOST_AUTO_TEST_CASE(space_jacobian){ - string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; - string tip_frame = "kuka_lbr_l_tcp"; - - RobotModelPtr robot_model = make_shared(); - RobotModelConfig cfg(urdf_file); - cfg.floating_base = true; - BOOST_CHECK(robot_model->configure(cfg)); - - testSpaceJacobian(robot_model, tip_frame, false); -} - -BOOST_AUTO_TEST_CASE(body_jacobian){ - string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; - string tip_frame = "kuka_lbr_l_tcp"; - - RobotModelPtr robot_model = make_shared(); - RobotModelConfig cfg(urdf_file); - cfg.floating_base = true; - BOOST_CHECK(robot_model->configure(cfg)); - - testBodyJacobian(robot_model, tip_frame, false); -} - -BOOST_AUTO_TEST_CASE(com_jacobian){ - string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; - string tip_frame = "kuka_lbr_l_tcp"; - - RobotModelPtr robot_model = make_shared(); - RobotModelConfig cfg(urdf_file); - cfg.floating_base = false; - BOOST_CHECK(robot_model->configure(cfg)); - - testCoMJacobian(robot_model, false); -} - -BOOST_AUTO_TEST_CASE(dynamics){ - - string urdf_file = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; - - RobotModelPtr robot_model = make_shared(); - RobotModelConfig cfg(urdf_file); - cfg.floating_base = true; - BOOST_CHECK(robot_model->configure(cfg)); - - testDynamics(robot_model, false); - -} diff --git a/src/robot_models/kdl/wbc-robot_models-kdl.pc.in b/src/robot_models/kdl/wbc-robot_models-kdl.pc.in deleted file mode 100644 index dff22b9c..00000000 --- a/src/robot_models/kdl/wbc-robot_models-kdl.pc.in +++ /dev/null @@ -1,12 +0,0 @@ -prefix=@CMAKE_INSTALL_PREFIX@ -exec_prefix=@CMAKE_INSTALL_PREFIX@ -libdir=${prefix}/lib -includedir=${prefix}/include - -Name: @TARGET_NAME@ -Description: @PROJECT_DESCRIPTION@ -Version: @PROJECT_VERSION@ -Requires: @PKGCONFIG_REQUIRES@ -Libs: -L${libdir} -l@TARGET_NAME@ @PKGCONFIG_LIBS@ -Cflags: -I${includedir} @PKGCONFIG_CFLAGS@ - diff --git a/src/robot_models/test/CMakeLists.txt b/src/robot_models/test/CMakeLists.txt index 95ffdc29..11bb25f4 100644 --- a/src/robot_models/test/CMakeLists.txt +++ b/src/robot_models/test/CMakeLists.txt @@ -1,7 +1,6 @@ add_executable(test_model_consistency test_model_consistency.cpp test_robot_model.cpp) target_link_libraries(test_model_consistency wbc-robot_models-hyrodyn - wbc-robot_models-kdl wbc-robot_models-rbdl wbc-robot_models-pinocchio Boost::unit_test_framework) diff --git a/src/robot_models/test/test_model_consistency.cpp b/src/robot_models/test/test_model_consistency.cpp index 32c9ed50..a04baae8 100644 --- a/src/robot_models/test/test_model_consistency.cpp +++ b/src/robot_models/test/test_model_consistency.cpp @@ -3,7 +3,6 @@ #include #include "robot_models/hyrodyn/RobotModelHyrodyn.hpp" #include "robot_models/rbdl/RobotModelRBDL.hpp" -#include "robot_models/kdl/RobotModelKDL.hpp" #include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "test_robot_model.hpp" @@ -71,9 +70,6 @@ void compareVect(base::VectorXd vect_a, base::VectorXd vect_b, vector jo void compareRobotModels(RobotModelConfig cfg, string tip_frame, bool verbose = false){ - RobotModelKDL robot_model_kdl; - BOOST_CHECK(robot_model_kdl.configure(cfg)); - RobotModelPinocchio robot_model_pinocchio; BOOST_CHECK(robot_model_pinocchio.configure(cfg)); @@ -85,8 +81,6 @@ void compareRobotModels(RobotModelConfig cfg, string tip_frame, bool verbose = f if(verbose){ cout << "---------------- Joint order ----------------" << endl; - cout<< " .......... RobotModelKDL .........." << endl; - for(auto n : robot_model_kdl.jointNames()) cout<