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 
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 (const moveit::core::LinkModel* link_model : left_arm_base_tip_group->getLinkModels())
170  {
171  if (link_model->getName() == "l_shoulder_pan_link")
172  {
173  EXPECT_TRUE(!found_shoulder_pan_link);
174  found_shoulder_pan_link = true;
175  }
176  if (link_model->getName() == "l_wrist_roll_link")
177  {
178  EXPECT_TRUE(!found_wrist_roll_link);
179  found_wrist_roll_link = true;
180  }
181  EXPECT_TRUE(link_model->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);
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 {
206  moveit::core::RobotModel robot_model(urdf_model_, srdf_model_);
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 
229  ks.setToDefaultValues();
230 
232  ks2.setToDefaultValues();
233 
234  std::vector<shapes::ShapeConstPtr> shapes;
235  EigenSTL::vector_Isometry3d poses;
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 }
moveit::core::LinkModel
A link from the robot. Contains the constant transform applied to the link and its geometry.
Definition: link_model.h:71
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:81
moveit::core::RobotState::setVariablePositions
void setVariablePositions(const double *position)
It is assumed positions is an array containing the new positions for all variables in this state....
Definition: robot_state.cpp:414
shapes
moveit::core::JointModel::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
Definition: joint_model.h:261
main
int main(int argc, char **argv)
Definition: test_kinematic_complex.cpp:266
moveit::core::RobotState::getVariableCount
std::size_t getVariableCount() const
Get the number of variables that make up this state.
Definition: robot_state.h:176
moveit::core::JointModelGroup::getSubgroupNames
const std::vector< std::string > & getSubgroupNames() const
Get the names of the groups that are subsets of this one (in terms of joints set)
Definition: joint_model_group.h:484
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
moveit::core::RobotModel
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:132
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
LoadPlanningModelsPr2
Definition: test_constraint_samplers.cpp:56
srdf::ModelSharedPtr
std::shared_ptr< Model > ModelSharedPtr
moveit::core::RobotModel::getRootJoint
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:138
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
shapes::Shape
moveit::core::RobotModel::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
Definition: robot_model.cpp:543
moveit::core::RobotState::attachBody
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...
Definition: robot_state.cpp:986
ASSERT_EQ
#define ASSERT_EQ(val1, val2)
conversions.h
EXPECT_TRUE
#define EXPECT_TRUE(condition)
moveit::core::loadModelInterface
urdf::ModelInterfaceSharedPtr loadModelInterface(const std::string &robot_name)
Loads a URDF Model Interface from moveit_resources.
Definition: robot_model_test_utils.cpp:124
robot_model.h
moveit::core::JointModelGroup::getLinkModels
const std::vector< const LinkModel * > & getLinkModels() const
Get the links that are part of this joint group.
Definition: joint_model_group.h:278
moveit::core::AttachedBody
Object defining bodies that can be attached to robot links.
Definition: attached_body.h:121
moveit::core::JointModelGroup::getVariableNames
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...
Definition: joint_model_group.h:254
profiler.h
EXPECT_EQ
#define EXPECT_EQ(expected, actual)
LoadPlanningModelsPr2::SetUp
void SetUp() override
Definition: test_constraint_samplers.cpp:105
shapes::Box
moveit::core::robotStateMsgToRobotState
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.
Definition: conversions.cpp:481
RUN_ALL_TESTS
int RUN_ALL_TESTS() GTEST_MUST_USE_RESULT_
moveit::core::RobotState::setToDefaultValues
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Definition: robot_state.cpp:405
LoadPlanningModelsPr2::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_constraint_samplers.cpp:136
shapes.h
ASSERT_TRUE
#define ASSERT_TRUE(condition)
moveit::core::RobotState::getAttachedBodies
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
Definition: robot_state.cpp:1006
moveit::core::JointModelGroup::getJointModels
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
Definition: joint_model_group.h:209
package.h
moveit::core::loadSRDFModel
srdf::ModelSharedPtr loadSRDFModel(const std::string &robot_name)
Loads an SRDF Model from moveit_resources.
Definition: robot_model_test_utils.cpp:145
testing::InitGoogleTest
GTEST_API_ void InitGoogleTest(int *argc, char **argv)
moveit::core::RobotState::getVariablePositions
double * getVariablePositions()
Get a raw pointer to the positions of the variables stored in this state. Use carefully....
Definition: robot_state.h:213
robot_model_test_utils.h
LoadPlanningModelsPr2::TearDown
void TearDown() override
Definition: test_constraint_samplers.cpp:131
LoadPlanningModelsPr2::urdf_model_
urdf::ModelInterfaceSharedPtr urdf_model_
Definition: test_kinematic_complex.cpp:97
moveit::core::RobotState::clearAttachedBody
bool clearAttachedBody(const std::string &id)
Remove the attached body named id. Return false if the object was not found (and thus not removed)....
Definition: robot_state.cpp:1078
LoadPlanningModelsPr2::srdf_model_
srdf::ModelSharedPtr srdf_model_
Definition: test_kinematic_complex.cpp:98
testing::Test
shapes::ShapeConstPtr
std::shared_ptr< const Shape > ShapeConstPtr
gtest.h
moveit::core::JointModelGroup::getVariableCount
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
Definition: joint_model_group.h:475
srdf::Model
TEST_F
TEST_F(LoadPlanningModelsPr2, InitOK)
Definition: test_kinematic_complex.cpp:70
moveit::core::robotStateToRobotStateMsg
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.
Definition: conversions.cpp:489
robot_state.h
moveit::core::JointModelGroup::isSubgroup
bool isSubgroup(const std::string &group) const
Check if the joints of group group are a subset of the joints in this group.
Definition: joint_model_group.h:493


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Nov 24 2020 03:26:40