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);
}