TEST_F(LoadPlanningModelsPr2, MultidofInit) { boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model()); // with no world multidof we should get a fixed joint planning_models::KinematicModel kin_model0(urdf_model_, srdfModel); EXPECT_TRUE(kin_model0.getRoot()->getVariableCount() == 0); static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>" "<robot name=\"pr2_test\">" "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>" "</robot>"; srdfModel->initString(*urdf_model_, SMODEL1); planning_models::KinematicModel kin_model1(urdf_model_, srdfModel); ASSERT_TRUE(kin_model1.getRoot() != NULL); EXPECT_EQ(kin_model1.getModelFrame(), "base_footprint"); static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>" "<robot name=\"pr2_test\">" "<virtual_joint name=\"world_joint\" child_link=\"base_footprint\" parent_frame=\"odom_combined\" type=\"floating\"/>" "</robot>"; srdfModel->initString(*urdf_model_, SMODEL2); planning_models::KinematicModel kin_model2(urdf_model_, srdfModel); ASSERT_TRUE(kin_model2.getRoot() != NULL); EXPECT_EQ(kin_model2.getModelFrame(), "odom_combined"); }
TEST(Loading, SimpleRobot) { static const std::string MODEL0 = "<?xml version=\"1.0\" ?>" "<robot name=\"myrobot\">" " <link name=\"base_link\">" " <collision name=\"base_collision\">" " <origin rpy=\"0 0 0\" xyz=\"0 0 0.165\"/>" " <geometry name=\"base_collision_geom\">" " <box size=\"0.65 0.65 0.23\"/>" " </geometry>" " </collision>" " </link>" "</robot>"; static const std::string SMODEL0 = "<?xml version=\"1.0\" ?>" "<robot name=\"myrobot\">" "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"floating\"/>" "</robot>"; boost::shared_ptr<urdf::ModelInterface> urdfModel = urdf::parseURDF(MODEL0); boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model()); srdfModel->initString(*urdfModel, SMODEL0); EXPECT_TRUE(srdfModel->getVirtualJoints().size() == 1); planning_models::KinematicModelPtr model(new planning_models::KinematicModel(urdfModel, srdfModel)); planning_models::KinematicState state(model); state.setToDefaultValues(); //make sure that this copy constructor works planning_models::KinematicState new_state(state); //(0,0,0,0) isn't a valid quaternion, so the w should be 1 std::map<std::string, double> state_values; new_state.getStateValues(state_values); EXPECT_EQ(state_values["base_joint/rot_w"], 1.0); EXPECT_EQ(std::string("myrobot"), model->getName()); EXPECT_EQ((unsigned int)7, new_state.getVariableCount()); const std::vector<planning_models::KinematicModel::LinkModel*>& links = model->getLinkModels(); EXPECT_EQ((unsigned int)1, links.size()); const std::vector<planning_models::KinematicModel::JointModel*>& joints = model->getJointModels(); EXPECT_EQ((unsigned int)1, joints.size()); const std::vector<std::string>& pgroups = model->getJointModelGroupNames(); EXPECT_EQ((unsigned int)0, pgroups.size()); }
TEST(FK, OneRobot) { static const std::string MODEL2 = "<?xml version=\"1.0\" ?>" "<robot name=\"one_robot\">" "<link name=\"base_link\">" " <inertial>" " <mass value=\"2.81\"/>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>" " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" " </inertial>" " <collision name=\"my_collision\">" " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </collision>" " <visual>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </visual>" "</link>" "<joint name=\"joint_a\" type=\"continuous\">" " <axis xyz=\"0 0 1\"/>" " <parent link=\"base_link\"/>" " <child link=\"link_a\"/>" " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>" "</joint>" "<link name=\"link_a\">" " <inertial>" " <mass value=\"1.0\"/>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>" " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" " </inertial>" " <collision>" " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </collision>" " <visual>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </visual>" "</link>" "<joint name=\"joint_b\" type=\"fixed\">" " <parent link=\"link_a\"/>" " <child link=\"link_b\"/>" " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>" "</joint>" "<link name=\"link_b\">" " <inertial>" " <mass value=\"1.0\"/>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>" " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" " </inertial>" " <collision>" " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </collision>" " <visual>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </visual>" "</link>" " <joint name=\"joint_c\" type=\"prismatic\">" " <axis xyz=\"1 0 0\"/>" " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>" " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" soft_upper_limit=\"0.089\"/>" " <parent link=\"link_b\"/>" " <child link=\"link_c\"/>" " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>" " </joint>" "<link name=\"link_c\">" " <inertial>" " <mass value=\"1.0\"/>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>" " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" " </inertial>" " <collision>" " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </collision>" " <visual>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </visual>" "</link>" "</robot>"; static const std::string MODEL2_INFO = "Model one_robot in frame odom_combined, of dimension 5\n" "Joint values bounds: \n" " base_joint/x [DBL_MIN, DBL_MAX]\n" " base_joint/y [DBL_MIN, DBL_MAX]\n" " base_joint/theta [-3.14159, 3.14159]\n" " joint_a [-3.14159, 3.14159]\n" " joint_c [0.00000, 0.08900]\n" "\n" "Available groups: \n" " base_from_base_to_tip (of dimension 4):\n" " joints:\n" " base_joint\n" " joint_a\n" " links:\n" " base_link\n" " link_a\n" " link_b\n" " roots:\n" " base_joint\n" " base_from_joints (of dimension 5):\n" " joints:\n" " base_joint\n" " joint_a\n" " joint_c\n" " links:\n" " base_link\n" " link_a\n" " link_c\n" " roots:\n" " base_joint\n" " base_with_subgroups (of dimension 5):\n" " joints:\n" " base_joint\n" " joint_a\n" " joint_c\n" " links:\n" " base_link\n" " link_a\n" " link_b\n" " link_c\n" " roots:\n" " base_joint"; static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>" "<robot name=\"one_robot\">" "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>" "<group name=\"base_from_joints\">" "<joint name=\"base_joint\"/>" "<joint name=\"joint_a\"/>" "<joint name=\"joint_c\"/>" "</group>" "<group name=\"base_with_subgroups\">" "<group name=\"base_from_base_to_tip\"/>" "<joint name=\"joint_c\"/>" "</group>" "<group name=\"base_from_base_to_tip\">" "<chain base_link=\"base_link\" tip_link=\"link_b\"/>" "<joint name=\"base_joint\"/>" "</group>" "<group name=\"base_with_bad_subgroups\">" "<group name=\"error\"/>" "</group>" "</robot>"; boost::shared_ptr<urdf::ModelInterface> urdfModel = urdf::parseURDF(MODEL2); boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model()); srdfModel->initString(*urdfModel, SMODEL2); planning_models::KinematicModelPtr model(new planning_models::KinematicModel(urdfModel, srdfModel)); //testing that the two planning groups are the same const planning_models::KinematicModel::JointModelGroup* g_one = model->getJointModelGroup("base_from_joints"); const planning_models::KinematicModel::JointModelGroup* g_two = model->getJointModelGroup("base_from_base_to_tip"); const planning_models::KinematicModel::JointModelGroup* g_three = model->getJointModelGroup("base_with_subgroups"); const planning_models::KinematicModel::JointModelGroup* g_four = model->getJointModelGroup("base_with_bad_subgroups"); ASSERT_TRUE(g_one != NULL); ASSERT_TRUE(g_two != NULL); ASSERT_TRUE(g_three != NULL); ASSERT_TRUE(g_four == NULL); //joint_b is a fixed joint, so no one should have it ASSERT_EQ(g_one->getJointModelNames().size(), 3); ASSERT_EQ(g_two->getJointModelNames().size(), 2); ASSERT_EQ(g_three->getJointModelNames().size(), 3); //only the links in between the joints, and the children of the leafs ASSERT_EQ(g_one->getLinkModelNames().size(), 3); //g_two only has three links ASSERT_EQ(g_two->getLinkModelNames().size(), 3); ASSERT_EQ(g_three->getLinkModelNames().size(), 4); std::vector<std::string> jmn = g_one->getJointModelNames(); std::sort(jmn.begin(), jmn.end()); EXPECT_EQ(jmn[0],"base_joint"); EXPECT_EQ(jmn[1],"joint_a"); EXPECT_EQ(jmn[2],"joint_c"); jmn = g_two->getJointModelNames(); std::sort(jmn.begin(), jmn.end()); EXPECT_EQ(jmn[0],"base_joint"); EXPECT_EQ(jmn[1],"joint_a"); jmn = g_three->getJointModelNames(); std::sort(jmn.begin(), jmn.end()); EXPECT_EQ(jmn[0],"base_joint"); EXPECT_EQ(jmn[1],"joint_a"); EXPECT_EQ(jmn[2],"joint_c"); //but they should have the same links to be updated ASSERT_EQ(g_one->getUpdatedLinkModels().size(), 4); ASSERT_EQ(g_two->getUpdatedLinkModels().size(), 4); ASSERT_EQ(g_three->getUpdatedLinkModels().size(), 4); EXPECT_EQ(g_one->getUpdatedLinkModels()[0]->getName(),"base_link"); EXPECT_EQ(g_one->getUpdatedLinkModels()[1]->getName(),"link_a"); EXPECT_EQ(g_one->getUpdatedLinkModels()[2]->getName(),"link_b"); EXPECT_EQ(g_one->getUpdatedLinkModels()[3]->getName(),"link_c"); EXPECT_EQ(g_two->getUpdatedLinkModels()[0]->getName(),"base_link"); EXPECT_EQ(g_two->getUpdatedLinkModels()[1]->getName(),"link_a"); EXPECT_EQ(g_two->getUpdatedLinkModels()[2]->getName(),"link_b"); EXPECT_EQ(g_two->getUpdatedLinkModels()[3]->getName(),"link_c"); EXPECT_EQ(g_three->getUpdatedLinkModels()[0]->getName(),"base_link"); EXPECT_EQ(g_three->getUpdatedLinkModels()[1]->getName(),"link_a"); EXPECT_EQ(g_three->getUpdatedLinkModels()[2]->getName(),"link_b"); EXPECT_EQ(g_three->getUpdatedLinkModels()[3]->getName(),"link_c"); //bracketing so the state gets destroyed before we bring down the model planning_models::KinematicState state(model); EXPECT_EQ((unsigned int)5, state.getVariableCount()); state.setToDefaultValues(); std::map<std::string, double> joint_values; joint_values["base_joint/x"]=1.0; joint_values["base_joint/y"]=1.0; joint_values["base_joint/theta"]=0.5; joint_values["joint_a"] = -0.5; joint_values["joint_c"] = 0.1; state.getJointStateGroup("base_from_joints")->setStateValues(joint_values); //double param[5] = { 1, 1, 0.5, -0.5, 0.1 }; //model->getGroup("base")->computeTransforms(param); std::stringstream ss0; model->printModelInfo(ss0); EXPECT_TRUE(sameStringIgnoringWS(MODEL2_INFO, ss0.str())) << MODEL2_INFO << "\n" << ss0.str(); std::stringstream ss1; state.printTransforms(ss1); //make sure it works the same way for the whole robot state.setStateValues(joint_values); std::stringstream ss2; state.printTransforms(ss2); EXPECT_EQ(ss1.str(), ss2.str()); EXPECT_NEAR(1.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5); EXPECT_NEAR(1.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5); EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("base_link")->getGlobalLinkTransform().rotation()).x(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("base_link")->getGlobalLinkTransform().rotation()).y(), 1e-5); EXPECT_NEAR(0.247404, Eigen::Quaterniond(state.getLinkState("base_link")->getGlobalLinkTransform().rotation()).z(), 1e-5); EXPECT_NEAR(0.968912, Eigen::Quaterniond(state.getLinkState("base_link")->getGlobalLinkTransform().rotation()).w(), 1e-5); EXPECT_NEAR(1.0, state.getLinkState("link_a")->getGlobalLinkTransform().translation().x(), 1e-5); EXPECT_NEAR(1.0, state.getLinkState("link_a")->getGlobalLinkTransform().translation().y(), 1e-5); EXPECT_NEAR(0.0, state.getLinkState("link_a")->getGlobalLinkTransform().translation().z(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("link_a")->getGlobalLinkTransform().rotation()).x(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("link_a")->getGlobalLinkTransform().rotation()).y(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("link_a")->getGlobalLinkTransform().rotation()).z(), 1e-5); EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getLinkState("link_a")->getGlobalLinkTransform().rotation()).w(), 1e-5); EXPECT_NEAR(1.0, state.getLinkState("link_b")->getGlobalLinkTransform().translation().x(), 1e-5); EXPECT_NEAR(1.5, state.getLinkState("link_b")->getGlobalLinkTransform().translation().y(), 1e-5); EXPECT_NEAR(0.0, state.getLinkState("link_b")->getGlobalLinkTransform().translation().z(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("link_b")->getGlobalLinkTransform().rotation()).x(), 1e-5); EXPECT_NEAR(-0.2084598, Eigen::Quaterniond(state.getLinkState("link_b")->getGlobalLinkTransform().rotation()).y(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("link_b")->getGlobalLinkTransform().rotation()).z(), 1e-5); EXPECT_NEAR(0.97803091, Eigen::Quaterniond(state.getLinkState("link_b")->getGlobalLinkTransform().rotation()).w(), 1e-5); EXPECT_NEAR(1.1, state.getLinkState("link_c")->getGlobalLinkTransform().translation().x(), 1e-5); EXPECT_NEAR(1.4, state.getLinkState("link_c")->getGlobalLinkTransform().translation().y(), 1e-5); EXPECT_NEAR(0.0, state.getLinkState("link_c")->getGlobalLinkTransform().translation().z(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("link_c")->getGlobalLinkTransform().rotation()).x(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("link_c")->getGlobalLinkTransform().rotation()).y(), 1e-5); EXPECT_NEAR(0.0, Eigen::Quaterniond(state.getLinkState("link_c")->getGlobalLinkTransform().rotation()).z(), 1e-5); EXPECT_NEAR(1.0, Eigen::Quaterniond(state.getLinkState("link_c")->getGlobalLinkTransform().rotation()).w(), 1e-5); //bonus bounds lookup test std::vector<std::string> jn; jn.push_back("base_joint"); EXPECT_TRUE(state.satisfiesBounds(jn)); jn.push_back("monkey"); EXPECT_FALSE(state.satisfiesBounds(jn)); std::map<std::string, double> upd_a; upd_a["joint_a"] = 0.2; state.setStateValues(upd_a); EXPECT_TRUE(state.satisfiesBounds("joint_a")); EXPECT_NEAR(state.getJointState("joint_a")->getVariableValues()[0], 0.2, 1e-3); state.enforceBounds(); EXPECT_NEAR(state.getJointState("joint_a")->getVariableValues()[0], 0.2, 1e-3); upd_a["joint_a"] = 3.2; state.setStateValues(upd_a); EXPECT_TRUE(state.satisfiesBounds("joint_a")); EXPECT_NEAR(state.getJointState("joint_a")->getVariableValues()[0], 3.2, 1e-3); state.enforceBounds(); EXPECT_NEAR(state.getJointState("joint_a")->getVariableValues()[0], -3.083185, 1e-3); EXPECT_TRUE(state.satisfiesBounds("joint_a")); }
TEST(LoadingAndFK, SimpleRobot) { static const std::string MODEL1 = "<?xml version=\"1.0\" ?>" "<robot name=\"myrobot\">" "<link name=\"base_link\">" " <inertial>" " <mass value=\"2.81\"/>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0.099 .0\"/>" " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>" " </inertial>" " <collision name=\"my_collision\">" " <origin rpy=\"0 0 -1\" xyz=\"-0.1 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </collision>" " <visual>" " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>" " <geometry>" " <box size=\"1 2 1\" />" " </geometry>" " </visual>" "</link>" "</robot>"; static const std::string MODEL1_INFO = "Model myrobot in frame odom_combined, of dimension 3\n" "Joint values bounds:\n" " base_joint/x [DBL_MIN, DBL_MAX]\n" " base_joint/y [DBL_MIN, DBL_MAX]\n" " base_joint/theta [-3.14159, 3.14159]\n" "Available groups: \n" " base (of dimension 3):\n" " joints:\n" " base_joint\n" " links:\n" " base_link\n" " roots:\n" " base_joint"; static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>" "<robot name=\"myrobot\">" "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>" "<group name=\"base\">" "<joint name=\"base_joint\"/>" "</group>" "</robot>"; boost::shared_ptr<urdf::ModelInterface> urdfModel = urdf::parseURDF(MODEL1); boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model()); srdfModel->initString(*urdfModel, SMODEL1); planning_models::KinematicModelPtr model(new planning_models::KinematicModel(urdfModel, srdfModel)); planning_models::KinematicState state(model); EXPECT_EQ((unsigned int)3, state.getVariableCount()); state.setToDefaultValues(); const std::vector<planning_models::KinematicState::JointState*>& joint_states = state.getJointStateVector(); EXPECT_EQ((unsigned int)1, joint_states.size()); EXPECT_EQ((unsigned int)3, joint_states[0]->getVariableValues().size()); std::stringstream ssi; model->printModelInfo(ssi); EXPECT_TRUE(sameStringIgnoringWS(MODEL1_INFO, ssi.str())) << ssi.str(); std::map<std::string, double> joint_values; joint_values["base_joint/x"] = 10.0; joint_values["base_joint/y"] = 8.0; //testing incomplete state std::vector<std::string> missing_states; state.setStateValues(joint_values, missing_states); ASSERT_EQ(missing_states.size(), 1); EXPECT_EQ(missing_states[0], std::string("base_joint/theta")); joint_values["base_joint/theta"] = 0.0; state.setStateValues(joint_values, missing_states); ASSERT_EQ(missing_states.size(), 0); EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5); EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5); EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5); //making sure that values get copied planning_models::KinematicState new_state(state); EXPECT_NEAR(10.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5); EXPECT_NEAR(8.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5); EXPECT_NEAR(0.0, new_state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5); const std::map<std::string, unsigned int>& ind_map = model->getJointVariablesIndexMap(); std::vector<double> jv(state.getVariableCount(), 0.0); jv[ind_map.at("base_joint/x")] = 10.0; jv[ind_map.at("base_joint/y")] = 8.0; jv[ind_map.at("base_joint/theta")] = 0.0; state.setStateValues(jv); EXPECT_NEAR(10.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().x(), 1e-5); EXPECT_NEAR(8.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().y(), 1e-5); EXPECT_NEAR(0.0, state.getLinkState("base_link")->getGlobalLinkTransform().translation().z(), 1e-5); }
TEST_F(LoadPlanningModelsPr2, GroupInit) { static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>" "<robot name=\"pr2_test\">" "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>" "<group name=\"left_arm_base_tip\">" "<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>" "</group>" "<group name=\"left_arm_joints\">" "<joint name=\"l_monkey_pan_joint\"/>" "<joint name=\"l_monkey_fles_joint\"/>" "</group>" "</robot>"; boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model()); srdfModel->initString(*urdf_model_, SMODEL1); planning_models::KinematicModel kin_model1(urdf_model_, srdfModel); const planning_models::KinematicModel::JointModelGroup* left_arm_base_tip_group = kin_model1.getJointModelGroup("left_arm_base_tip"); ASSERT_TRUE(left_arm_base_tip_group == NULL); const planning_models::KinematicModel::JointModelGroup* left_arm_joints_group = kin_model1.getJointModelGroup("left_arm_joints"); ASSERT_TRUE(left_arm_joints_group == NULL); static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>" "<robot name=\"pr2_test\">" "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>" "<group name=\"left_arm_base_tip\">" "<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>" "</group>" "<group name=\"left_arm_joints\">" "<joint name=\"l_shoulder_pan_joint\"/>" "<joint name=\"l_shoulder_lift_joint\"/>" "<joint name=\"l_upper_arm_roll_joint\"/>" "<joint name=\"l_elbow_flex_joint\"/>" "<joint name=\"l_forearm_roll_joint\"/>" "<joint name=\"l_wrist_flex_joint\"/>" "<joint name=\"l_wrist_roll_joint\"/>" "</group>" "</robot>"; srdfModel->initString(*urdf_model_, SMODEL2); planning_models::KinematicModelPtr kin_model2(new planning_models::KinematicModel(urdf_model_, srdfModel)); left_arm_base_tip_group = kin_model2->getJointModelGroup("left_arm_base_tip"); ASSERT_TRUE(left_arm_base_tip_group != NULL); left_arm_joints_group = kin_model2->getJointModelGroup("left_arm_joints"); ASSERT_TRUE(left_arm_joints_group != NULL); EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 7); EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7); EXPECT_EQ(left_arm_joints_group->getActiveDOFNames().size(), left_arm_joints_group->getVariableCount()); EXPECT_EQ(left_arm_joints_group->getVariableCount(), 7); EXPECT_EQ(kin_model2->getActiveDOFNames().size(), kin_model2->getVariableCount()); bool found_shoulder_pan_link = false; bool found_wrist_roll_link = false; for(unsigned int i = 0; i < left_arm_base_tip_group->getLinkModels().size(); i++) { if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_shoulder_pan_link") { EXPECT_TRUE(found_shoulder_pan_link == false); found_shoulder_pan_link = true; } if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_wrist_roll_link") { EXPECT_TRUE(found_wrist_roll_link == false); found_wrist_roll_link = true; } EXPECT_TRUE(left_arm_base_tip_group->getLinkModels()[i]->getName() != "torso_lift_link"); } EXPECT_TRUE(found_shoulder_pan_link); EXPECT_TRUE(found_wrist_roll_link); planning_models::KinematicState ks(kin_model2); ks.setToDefaultValues(); std::map<std::string, double> jv; jv["base_joint.x"] = 0.433; jv["base_joint.theta"] = -0.5; ks.setStateValues(jv); moveit_msgs::RobotState robot_state; planning_models::kinematicStateToRobotState(ks, robot_state); planning_models::KinematicState ks2(kin_model2); robotStateToKinematicState(robot_state, ks2); std::vector<double> v1; ks.getStateValues(v1); std::vector<double> v2; ks2.getStateValues(v2); EXPECT_TRUE(v1.size() == v2.size()); for (unsigned int i = 0; i < v1.size(); ++i) EXPECT_NEAR(v1[i], v2[i], 1e-5); geometry_msgs::Quaternion q; q.x = q.y = q.z = q.w = 0.0; Eigen::Quaterniond tq; EXPECT_FALSE(planning_models::quatFromMsg(q, tq)); EXPECT_TRUE(tq.w() == 1.0); std::vector<double> state_double_vector; ks.getStateValues(state_double_vector); ASSERT_TRUE(ks.setStateValues(state_double_vector)); }
TEST_F(LoadPlanningModelsPr2, GroupInit) { static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>" "<robot name=\"pr2\">" "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>" "<group name=\"left_arm_base_tip\">" "<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>" "</group>" "<group name=\"left_arm_joints\">" "<joint name=\"l_monkey_pan_joint\"/>" "<joint name=\"l_monkey_fles_joint\"/>" "</group>" "</robot>"; boost::shared_ptr<srdf::Model> srdfModel(new srdf::Model()); srdfModel->initString(*urdf_model, SMODEL1); moveit::core::RobotModel robot_model1(urdf_model, srdfModel); const moveit::core::JointModelGroup* left_arm_base_tip_group = robot_model1.getJointModelGroup("left_arm_base_tip"); ASSERT_TRUE(left_arm_base_tip_group == NULL); const moveit::core::JointModelGroup* left_arm_joints_group = robot_model1.getJointModelGroup("left_arm_joints"); ASSERT_TRUE(left_arm_joints_group == NULL); static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>" "<robot name=\"pr2\">" "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" parent_frame=\"base_footprint\" type=\"planar\"/>" "<group name=\"left_arm_base_tip\">" "<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>" "</group>" "<group name=\"left_arm_joints\">" "<joint name=\"l_shoulder_pan_joint\"/>" "<joint name=\"l_shoulder_lift_joint\"/>" "<joint name=\"l_upper_arm_roll_joint\"/>" "<joint name=\"l_elbow_flex_joint\"/>" "<joint name=\"l_forearm_roll_joint\"/>" "<joint name=\"l_wrist_flex_joint\"/>" "<joint name=\"l_wrist_roll_joint\"/>" "</group>" "</robot>"; srdfModel->initString(*urdf_model, SMODEL2); moveit::core::RobotModelPtr robot_model2(new moveit::core::RobotModel(urdf_model, srdfModel)); left_arm_base_tip_group = robot_model2->getJointModelGroup("left_arm_base_tip"); ASSERT_TRUE(left_arm_base_tip_group != NULL); left_arm_joints_group = robot_model2->getJointModelGroup("left_arm_joints"); ASSERT_TRUE(left_arm_joints_group != NULL); EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 9); EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7); EXPECT_EQ(left_arm_joints_group->getVariableNames().size(), left_arm_joints_group->getVariableCount()); EXPECT_EQ(left_arm_joints_group->getVariableCount(), 7); EXPECT_EQ(robot_model2->getVariableNames().size(), robot_model2->getVariableCount()); bool found_shoulder_pan_link = false; bool found_wrist_roll_link = false; for(unsigned int i = 0; i < left_arm_base_tip_group->getLinkModels().size(); i++) { if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_shoulder_pan_link") { EXPECT_TRUE(found_shoulder_pan_link == false); found_shoulder_pan_link = true; } if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_wrist_roll_link") { EXPECT_TRUE(found_wrist_roll_link == false); found_wrist_roll_link = true; } EXPECT_TRUE(left_arm_base_tip_group->getLinkModels()[i]->getName() != "torso_lift_link"); } EXPECT_TRUE(found_shoulder_pan_link); EXPECT_TRUE(found_wrist_roll_link); moveit::core::RobotState ks(robot_model2); ks.setToDefaultValues(); std::map<std::string, double> jv; jv["base_joint/x"] = 0.433; jv["base_joint/theta"] = -0.5; ks.setVariablePositions(jv); moveit_msgs::RobotState robot_state; moveit::core::robotStateToRobotStateMsg(ks, robot_state); moveit::core::RobotState ks2(robot_model2); moveit::core::robotStateMsgToRobotState(robot_state, ks2); const double *v1 = ks.getVariablePositions(); const double *v2 = ks2.getVariablePositions(); for (std::size_t i = 0; i < ks.getVariableCount(); ++i) EXPECT_NEAR(v1[i], v2[i], 1e-5); }