test_robot_models.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <planning_environment/models/robot_models.h>
#include <planning_models/kinematic_state.h>
#include <ros/time.h>
#include <gtest/gtest.h>
#include <iostream>
#include <sstream>

TEST(Loading, Simple)
{
    planning_environment::RobotModels m("robot_description");

    EXPECT_TRUE(m.getKinematicModel().get() != NULL);

    //now we test that the root transform has all the expected values
    boost::shared_ptr<planning_models::KinematicModel> kmodel = m.getKinematicModel();

    const planning_models::KinematicModel::JointModel* j = kmodel->getRoot();

    //check if it's the right type - this means that yaml parsing also works
    const planning_models::KinematicModel::FloatingJointModel* pj = dynamic_cast<const planning_models::KinematicModel::FloatingJointModel*>(j);
    EXPECT_TRUE(pj != NULL);
}

TEST(SetGetOperations, Simple)
{
  planning_environment::RobotModels m("robot_description");
  boost::shared_ptr<planning_models::KinematicModel> kmodel = m.getKinematicModel();

  planning_models::KinematicState state(kmodel);

  state.setKinematicStateToDefault();

  //all positive
  std::map<std::string, double> vals; 
  state.getKinematicStateValues(vals);
  for(std::map<std::string, double>::iterator it = vals.begin();
      it != vals.end();
      it++) {
    it->second = .1;
  }
  state.setKinematicState(vals);

  std::map<std::string, double> test_vals;
  state.getKinematicStateValues(test_vals);
  for(std::map<std::string, double>::iterator it = vals.begin();
      it != vals.end();
      it++) {
    EXPECT_TRUE(test_vals.find(it->first) != test_vals.end()) << "no value set for joint " << it->first;
    EXPECT_LE(fabs(it->second-test_vals[it->first]),.00001) << "Value for " << it->first << " is " << test_vals[it->first] << " instead of " << it->second;
  }

  //all negative
  for(std::map<std::string, double>::iterator it = vals.begin();
      it != vals.end();
      it++) {
    it->second = -.1;
  }
  state.setKinematicState(vals);

  state.getKinematicStateValues(test_vals);
  for(std::map<std::string, double>::iterator it = vals.begin();
      it != vals.end();
      it++) {
    EXPECT_TRUE(test_vals.find(it->first) != test_vals.end()) << "no value set for joint " << it->first;
    EXPECT_LE(fabs(it->second-test_vals[it->first]),.00001) << "Value for " << it->first << " is " << test_vals[it->first] << " instead of " << it->second;
  }

  //using root transform
  const planning_models::KinematicModel::JointModel* j = kmodel->getRoot();
  btTransform bt;
  bt.setOrigin(btVector3(5.0, 5.0, 5.0));
  bt.setRotation(btQuaternion(0,0,.7071,.7071));
  state.getJointState(j->getName())->setJointStateValues(bt);
  
  test_vals.clear();

  state.getKinematicStateValues(test_vals);
  //for(std::map<std::string, double>::iterator it = test_vals.begin();
  //    it != test_vals.end();
  //    it++) {
  // ROS_INFO_STREAM("Var " << it->first << " val " << it->second);
  //}

  EXPECT_EQ(test_vals["floating_trans_x"],5.0);
  EXPECT_EQ(test_vals["floating_trans_y"], 5.0);
  EXPECT_LE(fabs(test_vals["floating_rot_z"]-.7071), .001); 
}

TEST(SetGetBounds,Simple)
{
  //TODO
}

TEST(ForwardKinematics, RuntimeArm)
{
  planning_environment::RobotModels m("robot_description");
  boost::shared_ptr<planning_models::KinematicModel> kmodel = m.getKinematicModel();
  
  planning_models::KinematicState state(kmodel);
  
  state.setKinematicStateToDefault();

  planning_models::KinematicState::JointStateGroup *group = state.getJointStateGroup("right_arm");
  
  std::map<std::string, double> vals;
  group->getKinematicStateValues(vals);
  for(std::map<std::string, double>::iterator it = vals.begin();
      it != vals.end();
      it++) {
    it->second = .1;
  }
  std::map<std::string, double> vals2;
  group->getKinematicStateValues(vals2);
  for(std::map<std::string, double>::iterator it = vals2.begin();
      it != vals2.end();
      it++) {
    it->second = .1;
  }
  ros::WallTime tm = ros::WallTime::now();
  const unsigned int NT = 1000000;  
  for (unsigned int i = 0 ; i < NT ; ++i) {
    if(i%2 == 0) {
      group->setKinematicState(vals);
    } else {
      group->setKinematicState(vals2);
    }
  }
  double fps = (double)NT / (ros::WallTime::now() - tm).toSec();
  ROS_INFO("Map %f forward kinematics steps per second", fps);

  EXPECT_GT(fps,5000.0);
  
  std::vector<double> jv(group->getDimension(), 0.1);
  std::vector<double> jv2(group->getDimension(), 0.2);
  tm = ros::WallTime::now();
  for (unsigned int i = 0 ; i < NT ; ++i) {
    if(i%2 == 0) {
      group->setKinematicState(jv);
    } else {
      group->setKinematicState(jv2);
    }
  }
  fps = (double)NT / (ros::WallTime::now() - tm).toSec();
  ROS_INFO("Vector %f forward kinematics steps per second", fps);
  EXPECT_GT(fps,5000.0);
}

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

---

planning_environment
Author(s): Ioan Sucan
autogenerated on Fri Jan 11 10:03:02 2013