Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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
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
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
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
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
00205 const planning_models::KinematicModel::JointModel* j = kmodel->getRoot();
00206 tf::Transform bt;
00207 bt.setOrigin(tf::Vector3(5.0, 5.0, 5.0));
00208 bt.setRotation(tf::Quaternion(0,0,.7071,.7071));
00209 state.getJointState(j->getName())->setJointStateValues(bt);
00210
00211 test_vals.clear();
00212
00213 state.getKinematicStateValues(test_vals);
00214
00215
00216
00217
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 }