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>
47 #include <ros/package.h>
48 
49 class LoadPlanningModelsPr2 : public testing::Test
50 {
51 protected:
52  void SetUp() override
53  {
54  std::string robot_name = "pr2";
57  robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_model_);
58  };
59 
60  void TearDown() override
61  {
62  }
63 
64 protected:
65  urdf::ModelInterfaceSharedPtr urdf_model_;
67  moveit::core::RobotModelPtr robot_model_;
68 };
69 
71 {
72  ASSERT_EQ(urdf_model_->getName(), "pr2");
73  ASSERT_EQ(srdf_model_->getName(), "pr2");
74 }
75 
77 {
78  srdf::ModelSharedPtr srdf_model(new srdf::Model());
79 
80  // with no world multidof we should get a fixed joint
81  moveit::core::RobotModel robot_model0(urdf_model_, srdf_model);
82  EXPECT_TRUE(robot_model0.getRootJoint()->getVariableCount() == 0);
83 
84  static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>"
85  "<robot name=\"pr2\">"
86  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
87  "parent_frame=\"base_footprint\" type=\"planar\"/>"
88  "</robot>";
89  srdf_model->initString(*urdf_model_, SMODEL1);
90 
91  moveit::core::RobotModel robot_model1(urdf_model_, srdf_model);
92  ASSERT_TRUE(robot_model1.getRootJoint() != nullptr);
93  EXPECT_EQ(robot_model1.getModelFrame(), "base_footprint");
94 
95  static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>"
96  "<robot name=\"pr2\">"
97  "<virtual_joint name=\"world_joint\" child_link=\"base_footprint\" "
98  "parent_frame=\"odom_combined\" type=\"floating\"/>"
99  "</robot>";
100  srdf_model->initString(*urdf_model_, SMODEL2);
101 
102  moveit::core::RobotModel robot_model2(urdf_model_, srdf_model);
103  ASSERT_TRUE(robot_model2.getRootJoint() != nullptr);
104  EXPECT_EQ(robot_model2.getModelFrame(), "odom_combined");
105 }
106 
108 {
109  static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>"
110  "<robot name=\"pr2\">"
111  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
112  "parent_frame=\"base_footprint\" type=\"planar\"/>"
113  "<group name=\"left_arm_base_tip\">"
114  "<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>"
115  "</group>"
116  "<group name=\"left_arm_joints\">"
117  "<joint name=\"l_monkey_pan_joint\"/>"
118  "<joint name=\"l_monkey_fles_joint\"/>"
119  "</group>"
120  "</robot>";
121 
122  srdf::ModelSharedPtr srdf_model(new srdf::Model());
123  srdf_model->initString(*urdf_model_, SMODEL1);
124  moveit::core::RobotModel robot_model1(urdf_model_, srdf_model);
125 
126  const moveit::core::JointModelGroup* left_arm_base_tip_group = robot_model1.getJointModelGroup("left_arm_base_tip");
127  ASSERT_TRUE(left_arm_base_tip_group == nullptr);
128 
129  const moveit::core::JointModelGroup* left_arm_joints_group = robot_model1.getJointModelGroup("left_arm_joints");
130  ASSERT_TRUE(left_arm_joints_group == nullptr);
131 
132  static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>"
133  "<robot name=\"pr2\">"
134  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
135  "parent_frame=\"base_footprint\" type=\"planar\"/>"
136  "<group name=\"left_arm_base_tip\">"
137  "<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>"
138  "</group>"
139  "<group name=\"left_arm_joints\">"
140  "<joint name=\"l_shoulder_pan_joint\"/>"
141  "<joint name=\"l_shoulder_lift_joint\"/>"
142  "<joint name=\"l_upper_arm_roll_joint\"/>"
143  "<joint name=\"l_elbow_flex_joint\"/>"
144  "<joint name=\"l_forearm_roll_joint\"/>"
145  "<joint name=\"l_wrist_flex_joint\"/>"
146  "<joint name=\"l_wrist_roll_joint\"/>"
147  "</group>"
148  "</robot>";
149  srdf_model->initString(*urdf_model_, SMODEL2);
150 
151  moveit::core::RobotModelPtr robot_model2(new moveit::core::RobotModel(urdf_model_, srdf_model));
152 
153  left_arm_base_tip_group = robot_model2->getJointModelGroup("left_arm_base_tip");
154  ASSERT_TRUE(left_arm_base_tip_group != nullptr);
155 
156  left_arm_joints_group = robot_model2->getJointModelGroup("left_arm_joints");
157  ASSERT_TRUE(left_arm_joints_group != nullptr);
158 
159  EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 9u);
160  EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7u);
161 
162  EXPECT_EQ(left_arm_joints_group->getVariableNames().size(), left_arm_joints_group->getVariableCount());
163  EXPECT_EQ(left_arm_joints_group->getVariableCount(), 7u);
164 
165  EXPECT_EQ(robot_model2->getVariableNames().size(), robot_model2->getVariableCount());
166 
167  bool found_shoulder_pan_link = false;
168  bool found_wrist_roll_link = false;
169  for (unsigned int i = 0; i < left_arm_base_tip_group->getLinkModels().size(); i++)
170  {
171  if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_shoulder_pan_link")
172  {
173  EXPECT_TRUE(!found_shoulder_pan_link);
174  found_shoulder_pan_link = true;
175  }
176  if (left_arm_base_tip_group->getLinkModels()[i]->getName() == "l_wrist_roll_link")
177  {
178  EXPECT_TRUE(!found_wrist_roll_link);
179  found_wrist_roll_link = true;
180  }
181  EXPECT_TRUE(left_arm_base_tip_group->getLinkModels()[i]->getName() != "torso_lift_link");
182  }
183  EXPECT_TRUE(found_shoulder_pan_link);
184  EXPECT_TRUE(found_wrist_roll_link);
185 
186  moveit::core::RobotState ks(robot_model2);
187  ks.setToDefaultValues();
188  std::map<std::string, double> jv;
189  jv["base_joint/x"] = 0.433;
190  jv["base_joint/theta"] = -0.5;
191  ks.setVariablePositions(jv);
192  moveit_msgs::RobotState robot_state;
194 
195  moveit::core::RobotState ks2(robot_model2);
196  moveit::core::robotStateMsgToRobotState(robot_state, ks2);
197 
198  const double* v1 = ks.getVariablePositions();
199  const double* v2 = ks2.getVariablePositions();
200  for (std::size_t i = 0; i < ks.getVariableCount(); ++i)
201  EXPECT_NEAR(v1[i], v2[i], 1e-5);
202 }
203 
205 {
207  const moveit::core::JointModelGroup* jmg = robot_model.getJointModelGroup("arms");
208  ASSERT_TRUE(jmg);
209  EXPECT_EQ(jmg->getSubgroupNames().size(), 2u);
210  EXPECT_TRUE(jmg->isSubgroup("right_arm"));
211 
212  const moveit::core::JointModelGroup* jmg2 = robot_model.getJointModelGroup("whole_body");
213  EXPECT_EQ(jmg2->getSubgroupNames().size(), 5u);
214  EXPECT_TRUE(jmg2->isSubgroup("arms"));
215  EXPECT_TRUE(jmg2->isSubgroup("right_arm"));
216 }
217 
218 TEST_F(LoadPlanningModelsPr2, AssociatedFixedLinks)
219 {
220  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
221  EXPECT_TRUE(model->getLinkModel("r_gripper_palm_link")->getAssociatedFixedTransforms().size() > 1);
222 }
223 
225 {
226  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
227 
228  moveit::core::RobotState ks(robot_model);
229  ks.setToDefaultValues();
230 
231  moveit::core::RobotState ks2(robot_model);
232  ks2.setToDefaultValues();
233 
234  std::vector<shapes::ShapeConstPtr> shapes;
236  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
237  shapes.push_back(shapes::ShapeConstPtr(shape));
238  poses.push_back(Eigen::Isometry3d::Identity());
239  std::set<std::string> touch_links;
240 
241  trajectory_msgs::JointTrajectory empty_state;
243  robot_model->getLinkModel("r_gripper_palm_link"), "box", shapes, poses, touch_links, empty_state);
244  ks.attachBody(attached_body);
245 
246  std::vector<const moveit::core::AttachedBody*> attached_bodies_1;
247  ks.getAttachedBodies(attached_bodies_1);
248  ASSERT_EQ(attached_bodies_1.size(), 1u);
249 
250  std::vector<const moveit::core::AttachedBody*> attached_bodies_2;
251  ks2 = ks;
252  ks2.getAttachedBodies(attached_bodies_2);
253  ASSERT_EQ(attached_bodies_2.size(), 1u);
254 
255  ks.clearAttachedBody("box");
256  attached_bodies_1.clear();
257  ks.getAttachedBodies(attached_bodies_1);
258  ASSERT_EQ(attached_bodies_1.size(), 0u);
259 
260  ks2 = ks;
261  attached_bodies_2.clear();
262  ks2.getAttachedBodies(attached_bodies_2);
263  ASSERT_EQ(attached_bodies_2.size(), 0u);
264 }
265 
266 int main(int argc, char** argv)
267 {
268  testing::InitGoogleTest(&argc, argv);
269  return RUN_ALL_TESTS();
270 }
urdf::ModelInterfaceSharedPtr urdf_model_
Core components of MoveIt!
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 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...
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:180
#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...
bool isSubgroup(const std::string &group) const
Check if the joints of group group are a subset of the joints in this group.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:67
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
void getAttachedBodies(std::vector< const AttachedBody *> &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
#define EXPECT_EQ(a, b)
moveit::core::RobotModelPtr robot_model_
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:72
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
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...
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.
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
std::shared_ptr< Model > ModelSharedPtr
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:143
TEST_F(LoadPlanningModelsPr2, InitOK)
robot_model::RobotModelPtr robot_model_
const std::vector< std::string > & getSubgroupNames() const
Get the names of the groups that are subsets of this one (in terms of joints set) ...
int main(int argc, char **argv)
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string &robot_name)
Loads a URDF Model Interface from moveit_resources.
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:122
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
srdf::ModelSharedPtr loadSRDFModel(const std::string &robot_name)
Loads an SRDF Model from moveit_resources.
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
srdf::ModelSharedPtr srdf_model_
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
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 Tue Sep 8 2020 04:12:45