urdf_tests.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2018 Pilz GmbH & Co. KG
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "ros/ros.h"
#include <gtest/gtest.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

#define _USE_MATH_DEFINES
#include <math.h>

const std::string ARM_GROUP_NAME {"arm_group_name"};
const std::string ARM_GROUP_TIP_LINK_NAME {"arm_tip_link"};

static constexpr double L0 {0.2604}; // Height of foot
static constexpr double L1 {0.3500}; // Height of first connector
static constexpr double L2 {0.3070}; // Height of second connector
static constexpr double L3 {0.0840}; // Distance last joint to flange
static constexpr double delta {1.0e-10};

const std::string PARAM_MODEL {"robot_description"};

typedef std::tuple<std::string, std::vector<double>, Eigen::Vector3d> TestDataPoint;

inline std::string getName(TestDataPoint& tdp){
        return std::get<0>(tdp);
}

inline std::vector<double> getJoints(TestDataPoint& tdp){
        return std::get<1>(tdp);
}

inline Eigen::Vector3d getPos(TestDataPoint& tdp){
        return std::get<2>(tdp);
}

std::string vecToString(std::vector<double>& vec) {
        return std::string(vec.begin(), vec.end());
}

class URDFKinematicsTest : public testing::TestWithParam<std::string>
{
protected:
   virtual void SetUp();

protected:
  // ros stuff
  ros::NodeHandle ph_ {"~"};
  robot_model::RobotModelConstPtr robot_model_ {
    robot_model_loader::RobotModelLoader(GetParam()).getModel()};

  // parameters
  std::string group_name_, tip_link_name_;

  // test data
  std::vector<TestDataPoint> testDataSet_;
};

void URDFKinematicsTest::SetUp()
{
  // get necessary parameters
  ASSERT_TRUE(ph_.getParam(ARM_GROUP_NAME, group_name_));
  ASSERT_TRUE(ph_.getParam(ARM_GROUP_TIP_LINK_NAME, tip_link_name_));

  // Fill the testdata
  testDataSet_.push_back(TestDataPoint("HomePos ", {      0,       0,       0,  0,        0,  0}, Eigen::Vector3d(0,         0, L0+L1+L2+L3)));
  testDataSet_.push_back(TestDataPoint("TestPos1", {      0,  M_PI_2,       0,  0,        0,  0}, Eigen::Vector3d(L1+L2+L3,  0, L0)));
  testDataSet_.push_back(TestDataPoint("TestPos2", {      0, -M_PI_2,  M_PI_2,  0,        0,  0}, Eigen::Vector3d(-L1,       0, L0-L2-L3)));
  testDataSet_.push_back(TestDataPoint("TestPos3", {      0, -M_PI_2, -M_PI_2,  0,        0,  0}, Eigen::Vector3d(-L1,       0, L0+L2+L3)));
  testDataSet_.push_back(TestDataPoint("TestPos4", {      0,       0,  M_PI_2,  0,        0,  0}, Eigen::Vector3d(-L2-L3,    0, L0+L1)));
  testDataSet_.push_back(TestDataPoint("TestPos5", {-M_PI_2,       0, -M_PI_2,  0,        0,  0}, Eigen::Vector3d(0,    -L2-L3, L0+L1)));
  testDataSet_.push_back(TestDataPoint("TestPos6", {-M_PI_2, -M_PI_2, -M_PI_2,  0,        0,  0}, Eigen::Vector3d(0,        L1, L0+L2+L3)));
  testDataSet_.push_back(TestDataPoint("TestPos7", { M_PI_2, -M_PI_2,       0,  0,        0,  0}, Eigen::Vector3d(0, -L1-L2-L3, L0)));
  testDataSet_.push_back(TestDataPoint("TestPos8", {      0,       0,       0,  0,  -M_PI_2,  0}, Eigen::Vector3d(L3,        0, L0+L1+L2)));
  testDataSet_.push_back(TestDataPoint("TestPos9", { M_PI_2,       0,       0,  0,  -M_PI_2,  0}, Eigen::Vector3d(0,        L3, L0+L1+L2)));
  testDataSet_.push_back(TestDataPoint("TestPos10", {      0,       0,       0,  0,       0,  M_PI_2}, Eigen::Vector3d(0,    0, L0+L1+L2+L3)));
}

INSTANTIATE_TEST_CASE_P(InstantiationName, URDFKinematicsTest, ::testing::Values(PARAM_MODEL) );
TEST_P(URDFKinematicsTest, forwardKinematics)
{
  // check if the transformation is available
  robot_state::RobotState rstate(robot_model_);
  ASSERT_TRUE(rstate.knowsFrameTransform(tip_link_name_));

  // Loop over the testdata
  for(TestDataPoint & testData : testDataSet_)
  {
    std::string name = getName(testData);
    std::vector<double> joints = getJoints(testData);
    Eigen::Vector3d posExp = getPos(testData);

    // get frame transform
    rstate.setJointGroupPositions(group_name_, joints);
    //rstate.setVariablePositions(joints);
    rstate.update();
    Eigen::Affine3d transform = rstate.getFrameTransform(tip_link_name_);

    // Test the position
    double error_norm = (transform.translation() - posExp).norm();
    EXPECT_NEAR(error_norm, 0, delta)
        << "TestPosition \"" << name << "\" failed\n"
        << "\t Joints: [" << Eigen::VectorXd::Map(joints.data(), joints.size()).transpose() << "]\n"
        << "\t Expected position [" << posExp.transpose() << "]\n"
        << "\t Real position [" << transform.translation().transpose() << "]";
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "urdf_tests");
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}