TEST_F(LoadPlanningModelsPr2, SubgroupInit) { planning_models::KinematicModel kmodel(urdf_model_, srdf_model_); const planning_models::KinematicModel::JointModelGroup* jmg = kmodel.getJointModelGroup("arms"); ASSERT_TRUE(jmg); EXPECT_EQ(jmg->getSubgroupNames().size(), 2); EXPECT_TRUE(jmg->isSubgroup("right_arm")); const planning_models::KinematicModel::JointModelGroup* jmg2 = kmodel.getJointModelGroup("whole_body"); EXPECT_EQ(jmg2->getSubgroupNames().size(), 4); EXPECT_TRUE(jmg2->isSubgroup("arms")); EXPECT_TRUE(jmg2->isSubgroup("right_arm")); }
TEST_F(LoadPlanningModelsPr2, InitOK) { ASSERT_TRUE(urdf_ok_); ASSERT_EQ(urdf_model_->getName(), "pr2_test"); robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_)); robot_state::RobotState ks(kmodel); ks.setToRandomValues(); ks.setToDefaultValues(); robot_state::Transforms tf(kmodel->getModelFrame()); Eigen::Affine3d t1; t1.setIdentity(); t1.translation() = Eigen::Vector3d(10.0, 1.0, 0.0); tf.setTransform(t1, "some_frame_1"); Eigen::Affine3d t2(Eigen::Translation3d(10.0, 1.0, 0.0)*Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY())); tf.setTransform(t2, "some_frame_2"); Eigen::Affine3d t3; t3.setIdentity(); t3.translation() = Eigen::Vector3d(0.0, 1.0, -1.0); tf.setTransform(t3, "some_frame_3"); EXPECT_TRUE(tf.isFixedFrame("some_frame_1")); EXPECT_FALSE(tf.isFixedFrame("base_footprint")); EXPECT_TRUE(tf.isFixedFrame(kmodel->getModelFrame())); Eigen::Affine3d x; x.setIdentity(); tf.transformPose(ks, "some_frame_2", x, x); EXPECT_TRUE(t2.translation() == x.translation()); EXPECT_TRUE(t2.rotation() == x.rotation()); tf.transformPose(ks, kmodel->getModelFrame(), x, x); EXPECT_TRUE(t2.translation() == x.translation()); EXPECT_TRUE(t2.rotation() == x.rotation()); x.setIdentity(); tf.transformPose(ks, "r_wrist_roll_link", x, x); EXPECT_NEAR(x.translation().x(), 0.585315, 1e-4); EXPECT_NEAR(x.translation().y(), -0.188, 1e-4); EXPECT_NEAR(x.translation().z(), 1.24001, 1e-4); }
//TEST_F(LoadPlanningModelsPr2, robot_state::RobotState *Copy) TEST_F(LoadPlanningModelsPr2, FullTest) { robot_model::RobotModelPtr kmodel(new robot_model::RobotModel(urdf_model_, srdf_model_)); robot_state::RobotState ks(kmodel); ks.setToDefaultValues(); robot_state::RobotState ks2(kmodel); ks2.setToDefaultValues(); std::vector<shapes::ShapeConstPtr> shapes; EigenSTL::vector_Affine3d poses; shapes::Shape* shape = new shapes::Box(.1,.1,.1); shapes.push_back(shapes::ShapeConstPtr(shape)); poses.push_back(Eigen::Affine3d::Identity()); std::set<std::string> touch_links; robot_state::AttachedBody attached_body(ks.getLinkState("r_gripper_palm_link")->getLinkModel(), "box", shapes, poses, touch_links); ks.attachBody(&attached_body); std::vector<const robot_state::AttachedBody*> attached_bodies_1; ks.getAttachedBodies(attached_bodies_1); ASSERT_EQ(attached_bodies_1.size(),1); std::vector<const robot_state::AttachedBody*> attached_bodies_2; ks2 = ks; ks2.getAttachedBodies(attached_bodies_2); ASSERT_EQ(attached_bodies_2.size(),1); ks.clearAttachedBody("box"); attached_bodies_1.clear(); ks.getAttachedBodies(attached_bodies_1); ASSERT_EQ(attached_bodies_1.size(),0); ks2 = ks; attached_bodies_2.clear(); ks2.getAttachedBodies(attached_bodies_2); ASSERT_EQ(attached_bodies_2.size(),0); }
TEST_F(LoadPlanningModelsPr2, AssociatedFixedLinks) { boost::shared_ptr<planning_models::KinematicModel> kmodel(new planning_models::KinematicModel(urdf_model_, srdf_model_)); EXPECT_TRUE(kmodel->getLinkModel("r_gripper_palm_link")->getAssociatedFixedTransforms().size() > 1); }