planning_environment: planning_environment: test_robot

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include <planning_environment/models/robot_models.h>
00038 #include <planning_models/kinematic_state.h>
00039 #include <ros/time.h>
00040 #include <gtest/gtest.h>
00041 #include <iostream>
00042 #include <sstream>
00043 #include <fstream>
00044 #include <ros/package.h>
00045 
00046 static const std::string rel_path = "/test_urdf/robot.xml";
00047 
00048 static const std::string FLOATING_JOINT_XML = 
00049   "<value>"
00050   "<array>"
00051   "<data>"
00052   "<value>"
00053   "<struct>"
00054   "<member>"
00055   "<name>name</name>"
00056   "<value><string>base_joint</string></value>"
00057   "</member>"
00058   "<member>"
00059   "<name>parent_frame_id</name>"
00060   "<value><string>base_footprint</string></value>"
00061   "</member>"
00062   "<member>"
00063   "<name>child_frame_id</name>"
00064   "<value><string>base_footprint</string></value>"
00065   "</member>"
00066   "<member>"
00067   "<name>type</name>"
00068   "<value><string>Floating</string></value>"
00069   "</member>"
00070   "</struct>"
00071   "</value>"
00072   "</data>"
00073   "</array>"
00074   "</value>";
00075 
00076 static const std::string RIGHT_ARM_GROUP_XML = 
00077   "<value>"
00078   "<array>"
00079   "<data>"
00080   "<value>"
00081   "<struct>"
00082   "<member>"
00083   "<name>name</name>"
00084   "<value><string>right_arm</string></value>"
00085   "</member>"
00086   "<member>"
00087   "<name>base_link</name>"
00088   "<value><string>torso_lift_link</string></value>"
00089   "</member>"
00090   "<member>"
00091   "<name>tip_link</name>"
00092   "<value><string>r_wrist_roll_link</string></value>"
00093   "</member>"
00094   "</struct>"
00095   "</value>"
00096   "</data>"
00097   "</array>"
00098   "</value>";
00099 
00100 class TestRobotModels : public testing::Test 
00101 {
00102 protected:
00103   
00104   virtual void SetUp() {
00105 
00106     full_path_ = ros::package::getPath("planning_models")+rel_path;
00107     
00108     std::string com = "rosparam set robot_description -t "+full_path_;
00109 
00110     int ok = system(com.c_str());
00111     
00112     if(ok != 0) {
00113       ROS_WARN_STREAM("Setting parameter system call not ok");
00114     }
00115   }
00116   
00117 protected:
00118 
00119   ros::NodeHandle nh_;
00120   std::string full_path_;
00121 };
00122 
00123 TEST_F(TestRobotModels, Loading)
00124 {
00125   int offset1=0;
00126 
00127   //this will be used by other tests unless another joint is pushed
00128   XmlRpc::XmlRpcValue floating_multi_dof_joint(FLOATING_JOINT_XML, &offset1);
00129 
00130   ASSERT_TRUE(floating_multi_dof_joint.valid());
00131   ASSERT_EQ(floating_multi_dof_joint.getType(),XmlRpc::XmlRpcValue::TypeArray); 
00132 
00133   nh_.setParam("robot_description_planning/multi_dof_joints", floating_multi_dof_joint);
00134 
00135   //and these groups
00136   offset1 = 0;
00137   XmlRpc::XmlRpcValue planning_groups(RIGHT_ARM_GROUP_XML, &offset1);
00138 
00139   ASSERT_TRUE(planning_groups.valid());
00140   ASSERT_EQ(planning_groups.getType(),XmlRpc::XmlRpcValue::TypeArray); 
00141 
00142   nh_.setParam("robot_description_planning/groups", planning_groups);
00143   
00144   planning_environment::RobotModels m("robot_description");
00145 
00146   ASSERT_TRUE(m.getKinematicModel() != NULL);
00147 
00148   //now we test that the root transform has all the expected values
00149   const planning_models::KinematicModel* kmodel = m.getKinematicModel();
00150 
00151   const planning_models::KinematicModel::JointModel* j = kmodel->getRoot();
00152     
00153   ASSERT_TRUE(j != NULL);
00154   
00155   //check if it's the right type - this means that yaml parsing also works
00156   const planning_models::KinematicModel::FloatingJointModel* pj = dynamic_cast<const planning_models::KinematicModel::FloatingJointModel*>(j);
00157   EXPECT_TRUE(pj != NULL);
00158 }
00159 
00160 TEST_F(TestRobotModels, SetGetOperations)
00161 {
00162   planning_environment::RobotModels m("robot_description");
00163   const planning_models::KinematicModel* kmodel = m.getKinematicModel();
00164 
00165   planning_models::KinematicState state(kmodel);
00166 
00167   state.setKinematicStateToDefault();
00168 
00169   //all positive
00170   std::map<std::string, double> vals; 
00171   state.getKinematicStateValues(vals);
00172   for(std::map<std::string, double>::iterator it = vals.begin();
00173       it != vals.end();
00174       it++) {
00175     it->second = .1;
00176   }
00177   state.setKinematicState(vals);
00178 
00179   std::map<std::string, double> test_vals;
00180   state.getKinematicStateValues(test_vals);
00181   for(std::map<std::string, double>::iterator it = vals.begin();
00182       it != vals.end();
00183       it++) {
00184     EXPECT_TRUE(test_vals.find(it->first) != test_vals.end()) << "no value set for joint " << it->first;
00185     EXPECT_LE(fabs(it->second-test_vals[it->first]),.00001) << "Value for " << it->first << " is " << test_vals[it->first] << " instead of " << it->second;
00186   }
00187 
00188   //all negative
00189   for(std::map<std::string, double>::iterator it = vals.begin();
00190       it != vals.end();
00191       it++) {
00192     it->second = -.1;
00193   }
00194   state.setKinematicState(vals);
00195 
00196   state.getKinematicStateValues(test_vals);
00197   for(std::map<std::string, double>::iterator it = vals.begin();
00198       it != vals.end();
00199       it++) {
00200     EXPECT_TRUE(test_vals.find(it->first) != test_vals.end()) << "no value set for joint " << it->first;
00201     EXPECT_LE(fabs(it->second-test_vals[it->first]),.00001) << "Value for " << it->first << " is " << test_vals[it->first] << " instead of " << it->second;
00202   }
00203 
00204   //using root transform
00205   const planning_models::KinematicModel::JointModel* j = kmodel->getRoot();
00206   btTransform bt;
00207   bt.setOrigin(btVector3(5.0, 5.0, 5.0));
00208   bt.setRotation(btQuaternion(0,0,.7071,.7071));
00209   state.getJointState(j->getName())->setJointStateValues(bt);
00210   
00211   test_vals.clear();
00212 
00213   state.getKinematicStateValues(test_vals);
00214   //for(std::map<std::string, double>::iterator it = test_vals.begin();
00215   //    it != test_vals.end();
00216   //    it++) {
00217   // ROS_INFO_STREAM("Var " << it->first << " val " << it->second);
00218   //}
00219 
00220   EXPECT_EQ(test_vals["floating_trans_x"],5.0);
00221   EXPECT_EQ(test_vals["floating_trans_y"], 5.0);
00222   EXPECT_LE(fabs(test_vals["floating_rot_z"]-.7071), .001); 
00223 
00224 }
00225 
00226 TEST_F(TestRobotModels, SetGetBounds)
00227 {
00228   planning_environment::RobotModels m("robot_description");
00229   const planning_models::KinematicModel* kmodel = m.getKinematicModel();
00230 
00231   planning_models::KinematicState state(kmodel);
00232 
00233   state.setKinematicStateToDefault();
00234 
00235   std::map<std::string, double> test_vals;
00236   test_vals["r_shoulder_pan_joint"] =  4.0;
00237   test_vals["r_wrist_roll_link"] = 4.0;
00238   state.setKinematicState(test_vals);
00239   EXPECT_FALSE(state.isJointWithinBounds("r_shoulder_pan_joint"));
00240   EXPECT_TRUE(state.isJointWithinBounds("r_wrist_roll_joint"));
00241 }
00242 
00243 TEST_F(TestRobotModels, ForwardKinematics)
00244 {
00245   planning_environment::RobotModels m("robot_description");
00246   const planning_models::KinematicModel* kmodel = m.getKinematicModel();
00247   
00248   planning_models::KinematicState state(kmodel);
00249   
00250   state.setKinematicStateToDefault();
00251 
00252   planning_models::KinematicState::JointStateGroup *group = state.getJointStateGroup("right_arm");
00253   
00254   ASSERT_TRUE(group != NULL);
00255 
00256   std::map<std::string, double> vals;
00257   group->getKinematicStateValues(vals);
00258   for(std::map<std::string, double>::iterator it = vals.begin();
00259       it != vals.end();
00260       it++) {
00261     it->second = .1;
00262   }
00263   std::map<std::string, double> vals2;
00264   group->getKinematicStateValues(vals2);
00265   for(std::map<std::string, double>::iterator it = vals2.begin();
00266       it != vals2.end();
00267       it++) {
00268     it->second = .1;
00269   }
00270   ros::WallTime tm = ros::WallTime::now();
00271   const unsigned int NT = 100000;  
00272   for (unsigned int i = 0 ; i < NT ; ++i) {
00273     if(i%2 == 0) {
00274       group->setKinematicState(vals);
00275     } else {
00276       group->setKinematicState(vals2);
00277     }
00278   }
00279   double fps = (double)NT / (ros::WallTime::now() - tm).toSec();
00280   ROS_INFO("Map %f forward kinematics steps per second", fps);
00281 
00282   EXPECT_GT(fps,5000.0);
00283   
00284   std::vector<double> jv(group->getDimension(), 0.1);
00285   std::vector<double> jv2(group->getDimension(), 0.2);
00286   tm = ros::WallTime::now();
00287   for (unsigned int i = 0 ; i < NT ; ++i) {
00288     if(i%2 == 0) {
00289       group->setKinematicState(jv);
00290     } else {
00291       group->setKinematicState(jv2);
00292     }
00293   }
00294   fps = (double)NT / (ros::WallTime::now() - tm).toSec();
00295   ROS_INFO("Vector %f forward kinematics steps per second", fps);
00296   EXPECT_GT(fps,5000.0);
00297 }
00298 
00299 int main(int argc, char **argv)
00300 {
00301   testing::InitGoogleTest(&argc, argv);
00302   ros::init(argc, argv, "test_robot_models");
00303     
00304   return RUN_ALL_TESTS();
00305 }
test_robot_models.cpp
