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 constexpr double EPSILON = 1e-2;
50 constexpr double M_TAU = 2 * M_PI;
51 
53 {
54 protected:
55  void SetUp() override
56  {
57  std::string robot_name = "pr2";
60  robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model_, srdf_model_);
61  };
62 
63  void TearDown() override
64  {
65  }
66 
67 protected:
68  urdf::ModelInterfaceSharedPtr urdf_model_;
70  moveit::core::RobotModelPtr robot_model_;
71 };
72 
74 {
75  ASSERT_EQ(urdf_model_->getName(), "pr2");
76  ASSERT_EQ(srdf_model_->getName(), "pr2");
77 }
78 
80 {
81  srdf::ModelSharedPtr srdf_model(new srdf::Model());
82 
83  // with no world multidof we should get a fixed joint
84  moveit::core::RobotModel robot_model0(urdf_model_, srdf_model);
85  EXPECT_TRUE(robot_model0.getRootJoint()->getVariableCount() == 0);
86 
87  static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>"
88  "<robot name=\"pr2\">"
89  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
90  "parent_frame=\"base_footprint\" type=\"planar\"/>"
91  "</robot>";
92  srdf_model->initString(*urdf_model_, SMODEL1);
93 
94  moveit::core::RobotModel robot_model1(urdf_model_, srdf_model);
95  ASSERT_TRUE(robot_model1.getRootJoint() != nullptr);
96  EXPECT_EQ(robot_model1.getModelFrame(), "base_footprint");
97 
98  static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>"
99  "<robot name=\"pr2\">"
100  "<virtual_joint name=\"world_joint\" child_link=\"base_footprint\" "
101  "parent_frame=\"odom_combined\" type=\"floating\"/>"
102  "</robot>";
103  srdf_model->initString(*urdf_model_, SMODEL2);
104 
105  moveit::core::RobotModel robot_model2(urdf_model_, srdf_model);
106  ASSERT_TRUE(robot_model2.getRootJoint() != nullptr);
107  EXPECT_EQ(robot_model2.getModelFrame(), "odom_combined");
108 }
109 
111 {
112  static const std::string SMODEL1 = "<?xml version=\"1.0\" ?>"
113  "<robot name=\"pr2\">"
114  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
115  "parent_frame=\"base_footprint\" type=\"planar\"/>"
116  "<group name=\"left_arm_base_tip\">"
117  "<chain base_link=\"monkey_base\" tip_link=\"monkey_tip\"/>"
118  "</group>"
119  "<group name=\"left_arm_joints\">"
120  "<joint name=\"l_monkey_pan_joint\"/>"
121  "<joint name=\"l_monkey_fles_joint\"/>"
122  "</group>"
123  "</robot>";
124 
125  srdf::ModelSharedPtr srdf_model(new srdf::Model());
126  srdf_model->initString(*urdf_model_, SMODEL1);
127  moveit::core::RobotModel robot_model1(urdf_model_, srdf_model);
128 
129  const moveit::core::JointModelGroup* left_arm_base_tip_group = robot_model1.getJointModelGroup("left_arm_base_tip");
130  ASSERT_TRUE(left_arm_base_tip_group == nullptr);
131 
132  const moveit::core::JointModelGroup* left_arm_joints_group = robot_model1.getJointModelGroup("left_arm_joints");
133  ASSERT_TRUE(left_arm_joints_group == nullptr);
134 
135  static const std::string SMODEL2 = "<?xml version=\"1.0\" ?>"
136  "<robot name=\"pr2\">"
137  "<virtual_joint name=\"base_joint\" child_link=\"base_footprint\" "
138  "parent_frame=\"base_footprint\" type=\"planar\"/>"
139  "<group name=\"left_arm_base_tip\">"
140  "<chain base_link=\"torso_lift_link\" tip_link=\"l_wrist_roll_link\"/>"
141  "</group>"
142  "<group name=\"left_arm_joints\">"
143  "<joint name=\"l_shoulder_pan_joint\"/>"
144  "<joint name=\"l_shoulder_lift_joint\"/>"
145  "<joint name=\"l_upper_arm_roll_joint\"/>"
146  "<joint name=\"l_elbow_flex_joint\"/>"
147  "<joint name=\"l_forearm_roll_joint\"/>"
148  "<joint name=\"l_wrist_flex_joint\"/>"
149  "<joint name=\"l_wrist_roll_joint\"/>"
150  "</group>"
151  "</robot>";
152  srdf_model->initString(*urdf_model_, SMODEL2);
153 
154  moveit::core::RobotModelPtr robot_model2(new moveit::core::RobotModel(urdf_model_, srdf_model));
155 
156  left_arm_base_tip_group = robot_model2->getJointModelGroup("left_arm_base_tip");
157  ASSERT_TRUE(left_arm_base_tip_group != nullptr);
158 
159  left_arm_joints_group = robot_model2->getJointModelGroup("left_arm_joints");
160  ASSERT_TRUE(left_arm_joints_group != nullptr);
161 
162  EXPECT_EQ(left_arm_base_tip_group->getJointModels().size(), 9u);
163  EXPECT_EQ(left_arm_joints_group->getJointModels().size(), 7u);
164 
165  EXPECT_EQ(left_arm_joints_group->getVariableNames().size(), left_arm_joints_group->getVariableCount());
166  EXPECT_EQ(left_arm_joints_group->getVariableCount(), 7u);
167 
168  EXPECT_EQ(robot_model2->getVariableNames().size(), robot_model2->getVariableCount());
169 
170  bool found_shoulder_pan_link = false;
171  bool found_wrist_roll_link = false;
172  for (const moveit::core::LinkModel* link_model : left_arm_base_tip_group->getLinkModels())
173  {
174  if (link_model->getName() == "l_shoulder_pan_link")
175  {
176  EXPECT_TRUE(!found_shoulder_pan_link);
177  found_shoulder_pan_link = true;
178  }
179  if (link_model->getName() == "l_wrist_roll_link")
180  {
181  EXPECT_TRUE(!found_wrist_roll_link);
182  found_wrist_roll_link = true;
183  }
184  EXPECT_TRUE(link_model->getName() != "torso_lift_link");
185  }
186  EXPECT_TRUE(found_shoulder_pan_link);
187  EXPECT_TRUE(found_wrist_roll_link);
188 
189  moveit::core::RobotState ks(robot_model2);
190  ks.setToDefaultValues();
191  std::map<std::string, double> jv;
192  jv["base_joint/x"] = 0.433;
193  jv["base_joint/theta"] = -0.5;
194  ks.setVariablePositions(jv);
195  moveit_msgs::RobotState robot_state;
197 
198  moveit::core::RobotState ks2(robot_model2);
200 
201  const double* v1 = ks.getVariablePositions();
202  const double* v2 = ks2.getVariablePositions();
203  for (std::size_t i = 0; i < ks.getVariableCount(); ++i)
204  EXPECT_NEAR(v1[i], v2[i], 1e-5);
205 }
206 
208 {
209  moveit::core::RobotModel robot_model(urdf_model_, srdf_model_);
210  const moveit::core::JointModelGroup* jmg = robot_model.getJointModelGroup("arms");
211  ASSERT_TRUE(jmg);
212  EXPECT_EQ(jmg->getSubgroupNames().size(), 2u);
213  EXPECT_TRUE(jmg->isSubgroup("right_arm"));
214 
215  const moveit::core::JointModelGroup* jmg2 = robot_model.getJointModelGroup("whole_body");
216  EXPECT_EQ(jmg2->getSubgroupNames().size(), 5u);
217  EXPECT_TRUE(jmg2->isSubgroup("arms"));
218  EXPECT_TRUE(jmg2->isSubgroup("right_arm"));
219 }
220 
221 TEST_F(LoadPlanningModelsPr2, AssociatedFixedLinks)
222 {
223  moveit::core::RobotModelPtr model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
224  EXPECT_TRUE(model->getLinkModel("r_gripper_palm_link")->getAssociatedFixedTransforms().size() > 1);
225 }
226 
228 {
229  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
230 
232  ks.setToDefaultValues();
233 
235  ks2.setToDefaultValues();
236 
237  const auto identity = Eigen::Isometry3d::Identity();
238  std::vector<shapes::ShapeConstPtr> shapes;
240  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
241  shapes.push_back(shapes::ShapeConstPtr(shape));
242  poses.push_back(identity);
243  std::set<std::string> touch_links;
244 
245  trajectory_msgs::JointTrajectory empty_state;
246 
247  ks.attachBody(std::make_unique<moveit::core::AttachedBody>(robot_model->getLinkModel("r_gripper_palm_link"), "box",
248  identity, shapes, poses, touch_links, empty_state));
249 
250  std::vector<const moveit::core::AttachedBody*> attached_bodies_1;
251  ks.getAttachedBodies(attached_bodies_1);
252  ASSERT_EQ(attached_bodies_1.size(), 1u);
253 
254  std::vector<const moveit::core::AttachedBody*> attached_bodies_2;
255  ks2 = ks;
256  ks2.getAttachedBodies(attached_bodies_2);
257  ASSERT_EQ(attached_bodies_2.size(), 1u);
258 
259  ks.clearAttachedBody("box");
260  attached_bodies_1.clear();
261  ks.getAttachedBodies(attached_bodies_1);
262  ASSERT_EQ(attached_bodies_1.size(), 0u);
263 
264  ks2 = ks;
265  attached_bodies_2.clear();
266  ks2.getAttachedBodies(attached_bodies_2);
267  ASSERT_EQ(attached_bodies_2.size(), 0u);
268 }
269 
270 TEST_F(LoadPlanningModelsPr2, ObjectPoseAndSubframes)
271 {
272  moveit::core::RobotModelPtr robot_model(new moveit::core::RobotModel(urdf_model_, srdf_model_));
273 
275  ks.setToDefaultValues();
276 
277  std::vector<shapes::ShapeConstPtr> shapes;
279  shapes::Shape* shape = new shapes::Box(.1, .1, .1);
280  shapes.push_back(shapes::ShapeConstPtr(shape));
281  poses.push_back(Eigen::Isometry3d::Identity());
282  std::set<std::string> touch_links;
283  Eigen::Isometry3d pose_a = Eigen::Isometry3d::Identity();
284  Eigen::Isometry3d pose_b = Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1));
286  subframes["frame1"] = Eigen::Isometry3d(Eigen::Translation3d(0, 0, 1));
287 
288  trajectory_msgs::JointTrajectory empty_state;
289  ks.attachBody(std::make_unique<moveit::core::AttachedBody>(robot_model->getLinkModel("r_gripper_palm_link"), "boxA",
290  pose_a, shapes, poses, touch_links, empty_state,
291  subframes));
292  ks.attachBody(std::make_unique<moveit::core::AttachedBody>(robot_model->getLinkModel("r_gripper_palm_link"), "boxB",
293  pose_b, shapes, poses, touch_links, empty_state,
294  subframes));
295 
296  // Check position of shape in each body
297  Eigen::Isometry3d p;
298  p = ks.getAttachedBody("boxA")->getShapePosesInLinkFrame()[0];
299  EXPECT_EQ(0.0, p(2, 3)); // check translation.z
300  p = ks.getAttachedBody("boxB")->getShapePosesInLinkFrame()[0];
301  EXPECT_EQ(1.0, p(2, 3)); // z
302 
303  // Expect the pose and the subframe to have the same effect
304  Eigen::Isometry3d p2;
305 
306  p = ks.getFrameTransform("boxA/frame1");
307  p2 = ks.getFrameTransform("boxB");
308  EXPECT_TRUE(p.isApprox(p2, EPSILON));
309 
310  // Ensure that conversion to and from message in conversions.cpp works
311  moveit_msgs::RobotState msg;
312  robotStateToRobotStateMsg(ks, msg, true);
313 
314  // Add another object C that is defined in a frame that is not the link.
315  // The object will be transformed into the link's frame, which
316  // uses an otherwise inactive section of _msgToAttachedBody.
317  Eigen::Isometry3d pose_c = Eigen::Isometry3d(Eigen::Translation3d(0.1, 0.2, 0.3)) *
318  Eigen::AngleAxisd(0.1 * M_TAU, Eigen::Vector3d::UnitX()) *
319  Eigen::AngleAxisd(0.2 * M_TAU, Eigen::Vector3d::UnitY()) *
320  Eigen::AngleAxisd(0.4 * M_TAU, Eigen::Vector3d::UnitZ());
321  Eigen::Quaterniond q(pose_c.linear());
322  moveit_msgs::AttachedCollisionObject new_aco = msg.attached_collision_objects[0];
323  new_aco.object.id = "boxC";
324  new_aco.object.header.frame_id = "r_shoulder_pan_link";
325  new_aco.object.pose.position.x = pose_c.translation()[0];
326  new_aco.object.pose.position.y = pose_c.translation()[1];
327  new_aco.object.pose.position.z = pose_c.translation()[2];
328  new_aco.object.pose.orientation.x = q.vec()[0];
329  new_aco.object.pose.orientation.y = q.vec()[1];
330  new_aco.object.pose.orientation.z = q.vec()[2];
331  new_aco.object.pose.orientation.w = q.w();
332  msg.attached_collision_objects.push_back(new_aco);
333 
334  // Confirm that object B is unchanged after the conversion
336  robotStateMsgToRobotState(msg, ks3, true);
337  Eigen::Isometry3d p_original, p_reconverted;
338  p_original = ks.getAttachedBody("boxB")->getPose();
339  p_reconverted = ks3.getAttachedBody("boxB")->getPose();
340  EXPECT_TRUE(p_original.isApprox(p_reconverted, EPSILON));
341 
342  // Confirm that the position of object C is what we expect
343  Eigen::Isometry3d p_link, p_header_frame;
344  p_link = ks3.getFrameTransform("r_gripper_palm_link");
345  p_header_frame = ks3.getFrameTransform("r_shoulder_pan_link");
346 
347  p = p_header_frame * pose_c; // Object pose in world
348  p2 = ks3.getAttachedBody("boxC")->getGlobalPose();
349  EXPECT_TRUE(p.isApprox(p2, EPSILON));
350 
351  p = p_link.inverse() * p_header_frame * pose_c; // Object pose in link frame
352  p2 = ks3.getAttachedBody("boxC")->getPose();
353  EXPECT_TRUE(p.isApprox(p2, EPSILON));
354 }
355 
356 int main(int argc, char** argv)
357 {
358  testing::InitGoogleTest(&argc, argv);
359  return RUN_ALL_TESTS();
360 }
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:83
moveit::core::RobotModel::getModelFrame
const std::string & getModelFrame() const
Get the frame in which the transforms for this model are computed (when using a RobotState)....
Definition: robot_model.h:160
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:434
shapes
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
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:356
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:491
EPSILON
constexpr double EPSILON
Definition: test_kinematic_complex.cpp:49
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:141
moveit::core::AttachedBody::getShapePosesInLinkFrame
const EigenSTL::vector_Isometry3d & getShapePosesInLinkFrame() const
Get the fixed transforms (the transforms to the shapes of this body, relative to the link)....
Definition: attached_body.h:195
EXPECT_NEAR
#define EXPECT_NEAR(val1, val2, abs_error)
LoadPlanningModelsPr2
Definition: test_constraint_samplers.cpp:57
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:141
moveit::core::RobotState::getAttachedBody
const AttachedBody * getAttachedBody(const std::string &name) const
Get the attached body named name. Return NULL if not found.
Definition: robot_state.cpp:1045
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
moveit::core::AttachedBody::getGlobalPose
const Eigen::Isometry3d & getGlobalPose() const
Get the pose of the attached body, relative to the world.
Definition: attached_body.h:150
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:547
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:125
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::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_kinematic_complex.cpp:55
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:465
moveit::core::RobotState::attachBody
void attachBody(std::unique_ptr< AttachedBody > attached_body)
Add an attached body to this state.
Definition: robot_state.cpp:1057
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:425
LoadPlanningModelsPr2::robot_model_
moveit::core::RobotModelPtr robot_model_
Definition: test_constraint_samplers.cpp:137
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:1083
moveit::core::FixedTransformsMap
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:117
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:146
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
moveit::core::RobotState::getFrameTransform
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
Definition: robot_state.cpp:1161
LoadPlanningModelsPr2::TearDown
void TearDown() override
Definition: test_kinematic_complex.cpp:63
LoadPlanningModelsPr2::urdf_model_
urdf::ModelInterfaceSharedPtr urdf_model_
Definition: test_kinematic_complex.cpp:68
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:1147
M_TAU
constexpr double M_TAU
Definition: test_kinematic_complex.cpp:50
LoadPlanningModelsPr2::srdf_model_
srdf::ModelSharedPtr srdf_model_
Definition: test_kinematic_complex.cpp:69
M_PI
#define M_PI
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
TEST_F
TEST_F(LoadPlanningModelsPr2, InitOK)
Definition: test_kinematic_complex.cpp:73
srdf::Model
moveit::core::AttachedBody::getPose
const Eigen::Isometry3d & getPose() const
Get the pose of the attached body relative to the parent link.
Definition: attached_body.h:144
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:473
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:500


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Nov 24 2022 03:32:15