ikfast_kinematics_unit.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <gtest/gtest.h>
#include <fstream>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include "kinematics_test_utils.h"
#include "tesseract_kinematics/ikfast/impl/ikfast_inv_kin.hpp"
#include <tesseract_kinematics/kdl/kdl_fwd_kin_chain.h>
 
using namespace tesseract_kinematics::test_suite;
using namespace tesseract_kinematics;
 
TEST(TesseractKinematicsUnit, IKFastInvKin)  // NOLINT
{
  // Inverse target pose and seed
  Eigen::Isometry3d pose;
  pose.setIdentity();
  pose.translation()[0] = 1;
  pose.translation()[1] = 0;
  pose.translation()[2] = 1.306;
 
  Eigen::VectorXd seed = Eigen::VectorXd::Zero(6);
 
  // Setup test
  tesseract_common::GeneralResourceLocator locator;
  auto scene_graph = getSceneGraphABB(locator);
  std::string base_link_name = "base_link";
  std::string tip_link_name = "tool0";
  std::vector<std::string> joint_names{ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6" };
 
  KDLFwdKinChain fwd_kin(*scene_graph, base_link_name, tip_link_name);
 
  auto inv_kin = std::make_shared<IKFastInvKin>(base_link_name, tip_link_name, joint_names);
 
  EXPECT_EQ(inv_kin->getSolverName(), IKFAST_INV_KIN_CHAIN_SOLVER_NAME);
  EXPECT_EQ(inv_kin->numJoints(), 6);
  EXPECT_EQ(inv_kin->getBaseLinkName(), base_link_name);
  EXPECT_EQ(inv_kin->getWorkingFrame(), base_link_name);
  EXPECT_EQ(inv_kin->getTipLinkNames().size(), 1);
  EXPECT_EQ(inv_kin->getTipLinkNames()[0], tip_link_name);
  EXPECT_EQ(inv_kin->getJointNames(), joint_names);
 
  runInvKinTest(*inv_kin, fwd_kin, pose, tip_link_name, seed);
 
  // Check cloned
  InverseKinematics::Ptr inv_kin2 = inv_kin->clone();
  EXPECT_TRUE(inv_kin2 != nullptr);
  EXPECT_EQ(inv_kin2->getSolverName(), IKFAST_INV_KIN_CHAIN_SOLVER_NAME);
  EXPECT_EQ(inv_kin2->numJoints(), 6);
  EXPECT_EQ(inv_kin2->getBaseLinkName(), base_link_name);
  EXPECT_EQ(inv_kin2->getWorkingFrame(), base_link_name);
  EXPECT_EQ(inv_kin2->getTipLinkNames().size(), 1);
  EXPECT_EQ(inv_kin2->getTipLinkNames()[0], tip_link_name);
  EXPECT_EQ(inv_kin2->getJointNames(), joint_names);
 
  runInvKinTest(*inv_kin2, fwd_kin, pose, tip_link_name, seed);
}
 
int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
 
  return RUN_ALL_TESTS();
}