test_kinematic_complex.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
40 #include <urdf_parser/urdf_parser.h>
41 #include <fstream>
42 #include <gtest/gtest.h>
43 #include <boost/filesystem/path.hpp>
46 #include <moveit_resources/config.h>
47 
48 class LoadPlanningModelsPr2 : public testing::Test
49 {
50 protected:
51  void SetUp() override
52  {
53  boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR);
54 
55  srdf_model.reset(new srdf::Model());
56  std::string xml_string;
57  std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in);
58  if (xml_file.is_open())
59  {
60  while (xml_file.good())
61  {
62  std::string line;
63  std::getline(xml_file, line);
64  xml_string += (line + "\n");
65  }
66  xml_file.close();
67  urdf_model = urdf::parseURDF(xml_string);
68  }
69  srdf_model->initFile(*urdf_model, (res_path / "pr2_description/srdf/robot.xml").string());
71  };
72 
73  void TearDown() override
74  {
75  }
76 
77 protected:
78  urdf::ModelInterfaceSharedPtr urdf_model;
80  moveit::core::RobotModelConstPtr robot_model;
81 };
82 
84 {
85  ASSERT_EQ(urdf_model->getName(), "pr2");
86  ASSERT_EQ(srdf_model->getName(), "pr2");
87 }
88 
90 {
91  srdf::ModelSharedPtr srdfModel(new srdf::Model());
92 
93  // with no world multidof we should get a fixed joint
94  moveit::core::RobotModel robot_model0(urdf_model, srdfModel);
95  EXPECT_TRUE(robot_model0.getRootJoint()->getVariableCount() == 0);
96 
97  static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>"
98  "<robot name=\"pr2\">"
99  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
100  "parent_frame=\"base_footprint\" type=\"planar\"/>"
101  "</robot>";
102  srdfModel->initString(*urdf_model, SMODEL1);
103 
104  moveit::core::RobotModel robot_model1(urdf_model, srdfModel);
105  ASSERT_TRUE(robot_model1.getRootJoint() != nullptr);
106  EXPECT_EQ(robot_model1.getModelFrame(), "/base_footprint");
107 
108  static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>"
109  "<robot name=\"pr2\">"
110  "<virtual_joint name=\"world_joint\" child_link=\"base_footprint\" "
111  "parent_frame=\"odom_combined\" type=\"floating\"/>"
112  "</robot>";
113  srdfModel->initString(*urdf_model, SMODEL2);
114 
115  moveit::core::RobotModel robot_model2(urdf_model, srdfModel);
116  ASSERT_TRUE(robot_model2.getRootJoint() != nullptr);
117  EXPECT_EQ(robot_model2.getModelFrame(), "/odom_combined");
118 }
119 
121 {
122  static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>"
123  "<robot name=\"pr2\">"
124  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
125  "parent_frame=\"base_footprint\" type=\"planar\"/>"
126  "<group name=\"left_arm_base_tip\">"
127  "<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>"
128  "</group>"
129  "<group name=\"left_arm_joints\">"
130  "<joint name=\"l_monkey_pan_joint\"/>"
131  "<joint name=\"l_monkey_fles_joint\"/>"
132  "</group>"
133  "</robot>";
134 
135  srdf::ModelSharedPtr srdfModel(new srdf::Model());
136  srdfModel->initString(*urdf_model, SMODEL1);
137  moveit::core::RobotModel robot_model1(urdf_model, srdfModel);
138 
139  const moveit::core::JointModelGroup* left_arm_base_tip_group = robot_model1.getJointModelGroup("left_arm_base_tip");
140  ASSERT_TRUE(left_arm_base_tip_group == nullptr);
141 
142  const moveit::core::JointModelGroup* left_arm_joints_group = robot_model1.getJointModelGroup("left_arm_joints");
143  ASSERT_TRUE(left_arm_joints_group == nullptr);
144 
145  static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>"
146  "<robot name=\"pr2\">"
147  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
148  "parent_frame=\"base_footprint\" type=\"planar\"/>"
149  "<group name=\"left_arm_base_tip\">"
150  "<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>"
151  "</group>"
152  "<group name=\"left_arm_joints\">"
153  "<joint name=\"l_shoulder_pan_joint\"/>"
154  "<joint name=\"l_shoulder_lift_joint\"/>"
155  "<joint name=\"l_upper_arm_roll_joint\"/>"
156  "<joint name=\"l_elbow_flex_joint\"/>"
157  "<joint name=\"l_forearm_roll_joint\"/>"
158  "<joint name=\"l_wrist_flex_joint\"/>"
159  "<joint name=\"l_wrist_roll_joint\"/>"
160  "</group>"
161  "</robot>";
162  srdfModel->initString(*urdf_model, SMODEL2);
163 
164  moveit::core::RobotModelPtr robot_model2(new moveit::core::RobotModel(urdf_model, srdfModel));
165 
166  left_arm_base_tip_group = robot_model2->getJointModelGroup("left_arm_base_tip");
167  ASSERT_TRUE(left_arm_base_tip_group != nullptr);
168 
169  left_arm_joints_group = robot_model2->getJointModelGroup("left_arm_joints");
170  ASSERT_TRUE(left_arm_joints_group != nullptr);
171 
172  EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 9);
173  EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7);
174 
175  EXPECT_EQ(left_arm_joints_group->getVariableNames().size(), left_arm_joints_group->getVariableCount());
176  EXPECT_EQ(left_arm_joints_group->getVariableCount(), 7);
177 
178  EXPECT_EQ(robot_model2->getVariableNames().size(), robot_model2->getVariableCount());
179 
180  bool found_shoulder_pan_link = false;
181  bool found_wrist_roll_link = false;
182  for (unsigned int i = 0; i < left_arm_base_tip_group->getLinkModels().size(); i++)
183  {
184  if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_shoulder_pan_link")
185  {
186  EXPECT_TRUE(!found_shoulder_pan_link);
187  found_shoulder_pan_link = true;
188  }
189  if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_wrist_roll_link")
190  {
191  EXPECT_TRUE(!found_wrist_roll_link);
192  found_wrist_roll_link = true;
193  }
194  EXPECT_TRUE(left_arm_base_tip_group->getLinkModels()[i]->getName() != "torso_lift_link");
195  }
196  EXPECT_TRUE(found_shoulder_pan_link);
197  EXPECT_TRUE(found_wrist_roll_link);
198 
199  moveit::core::RobotState ks(robot_model2);
200  ks.setToDefaultValues();
201  std::map<std::string, double> jv;
202  jv["base_joint/x"] = 0.433;
203  jv["base_joint/theta"] = -0.5;
204  ks.setVariablePositions(jv);
205  moveit_msgs::RobotState robot_state;
207 
208  moveit::core::RobotState ks2(robot_model2);
209  moveit::core::robotStateMsgToRobotState(robot_state, ks2);
210 
211  const double* v1 = ks.getVariablePositions();
212  const double* v2 = ks2.getVariablePositions();
213  for (std::size_t i = 0; i < ks.getVariableCount(); ++i)
214  EXPECT_NEAR(v1[i], v2[i], 1e-5);
215 }
216 
218 {
220  const moveit::core::JointModelGroup* jmg = robot_model.getJointModelGroup("arms");
221  ASSERT_TRUE(jmg);
222  EXPECT_EQ(jmg->getSubgroupNames().size(), 2);
223  EXPECT_TRUE(jmg->isSubgroup("right_arm"));
224 
225  const moveit::core::JointModelGroup* jmg2 = robot_model.getJointModelGroup("whole_body");
226  EXPECT_EQ(jmg2->getSubgroupNames().size(), 5);
227  EXPECT_TRUE(jmg2->isSubgroup("arms"));
228  EXPECT_TRUE(jmg2->isSubgroup("right_arm"));
229 }
230 
231 TEST_F(LoadPlanningModelsPr2, AssociatedFixedLinks)
232 {
233  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdf_model, srdf_model));
234  EXPECT_TRUE(model->getLinkModel("r_gripper_palm_link")->getAssociatedFixedTransforms().size() > 1);
235 }
236 
238 {
239  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model, srdf_model));
240 
241  moveit::core::RobotState ks(robot_model);
242  ks.setToDefaultValues();
243 
244  moveit::core::RobotState ks2(robot_model);
245  ks2.setToDefaultValues();
246 
247  std::vector<shapes::ShapeConstPtr> shapes;
249  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
250  shapes.push_back(shapes::ShapeConstPtr(shape));
251  poses.push_back(Eigen::Affine3d::Identity());
252  std::set<std::string> touch_links;
253 
254  trajectory_msgs::JointTrajectory empty_state;
256  robot_model->getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state);
257  ks.attachBody(attached_body);
258 
259  std::vector<const moveit::core::AttachedBody*> attached_bodies_1;
260  ks.getAttachedBodies(attached_bodies_1);
261  ASSERT_EQ(attached_bodies_1.size(), 1);
262 
263  std::vector<const moveit::core::AttachedBody*> attached_bodies_2;
264  ks2 = ks;
265  ks2.getAttachedBodies(attached_bodies_2);
266  ASSERT_EQ(attached_bodies_2.size(), 1);
267 
268  ks.clearAttachedBody("box");
269  attached_bodies_1.clear();
270  ks.getAttachedBodies(attached_bodies_1);
271  ASSERT_EQ(attached_bodies_1.size(), 0);
272 
273  ks2 = ks;
274  attached_bodies_2.clear();
275  ks2.getAttachedBodies(attached_bodies_2);
276  ASSERT_EQ(attached_bodies_2.size(), 0);
277 }
278 
279 int main(int argc, char** argv)
280 {
281  testing::InitGoogleTest(&argc, argv);
282  return RUN_ALL_TESTS();
283 }
bool isSubgroup(const std::string &group) const
Check if the joints of group group are a subset of the joints in this group.
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
const std::string & getModelFrame() const
Get the frame in which the transforms for this model are computed (when using a RobotState). This frame depends on the root joint. As such, the frame is either extracted from SRDF, or it is assumed to be the name of the root link.
Definition: robot_model.h:86
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:196
Core components of MoveIt!
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
const std::vector< std::string > & getSubgroupNames() const
Get the names of the groups that are subsets of this one (in terms of joints set) ...
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void attachBody(AttachedBody *attached_body)
Add an attached body to this state. Ownership of the memory for the attached body is assumed by the s...
const JointModel * getRootJoint() const
Get the root joint. There will be one root joint unless the model is empty. This is either extracted ...
Definition: robot_model.cpp:73
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully. If you change these values externally you need to make sure you trigger a forced update for the state by calling update(true).
Definition: robot_state.h:181
#define EXPECT_NEAR(a, b, prec)
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state...
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
#define EXPECT_EQ(a, b)
urdf::ModelInterfaceSharedPtr urdf_model
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state...
def xml_string(rootXml, addHeader=True)
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt! robot state to a robot state message.
TEST_F(LoadPlanningModelsPr2, InitOK)
int main(int argc, char **argv)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
moveit::core::RobotModelConstPtr robot_model
Definition: test.cpp:77
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:144
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
Definition: attached_body.h:56
#define EXPECT_TRUE(args)
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)...
std::shared_ptr< const Shape > ShapeConstPtr


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jul 10 2019 04:03:05