moveit2
The MoveIt Motion Planning Framework for ROS 2.
cartesianconfiguration.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019 Pilz GmbH & Co. KG
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 Pilz GmbH & Co. KG 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 #pragma once
36 
37 #include <vector>
38 #include <sstream>
39 #include <optional>
40 
41 #include <geometry_msgs/msg/pose.hpp>
42 #include <geometry_msgs/msg/pose_stamped.hpp>
47 
48 #include "robotconfiguration.h"
49 #include "jointconfiguration.h"
50 
52 {
58 {
59 public:
61 
62  CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector<double>& config);
63 
64  CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector<double>& config,
65  const moveit::core::RobotModelConstPtr& robot_model);
66 
67 public:
68  moveit_msgs::msg::Constraints toGoalConstraints() const override;
69  moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override;
70 
71  void setLinkName(const std::string& link_name);
72  const std::string& getLinkName() const;
73 
74  void setPose(const geometry_msgs::msg::Pose& pose);
75  const geometry_msgs::msg::Pose& getPose() const;
76  geometry_msgs::msg::Pose& getPose();
77 
78  void setSeed(const JointConfiguration& config);
79  const JointConfiguration& getSeed() const;
81  bool hasSeed() const;
82 
83  void setPoseTolerance(const double tol);
84  const std::optional<double> getPoseTolerance() const;
85 
86  void setAngleTolerance(const double tol);
87  const std::optional<double> getAngleTolerance() const;
88 
89 private:
90  static geometry_msgs::msg::Pose toPose(const std::vector<double>& pose);
91  static geometry_msgs::msg::PoseStamped toStampedPose(const geometry_msgs::msg::Pose& pose);
92 
93 private:
94  std::string link_name_;
95  geometry_msgs::msg::Pose pose_;
96 
99  std::optional<double> tolerance_pose_;
100 
103  std::optional<double> tolerance_angle_;
104 
106  std::optional<JointConfiguration> seed_;
107 };
108 
109 std::ostream& operator<<(std::ostream& /*os*/, const CartesianConfiguration& /*obj*/);
110 
111 inline void CartesianConfiguration::setLinkName(const std::string& link_name)
112 {
113  link_name_ = link_name;
114 }
115 
116 inline const std::string& CartesianConfiguration::getLinkName() const
117 {
118  return link_name_;
119 }
120 
121 inline void CartesianConfiguration::setPose(const geometry_msgs::msg::Pose& pose)
122 {
123  pose_ = pose;
124 }
125 
126 inline const geometry_msgs::msg::Pose& CartesianConfiguration::getPose() const
127 {
128  return pose_;
129 }
130 
131 inline geometry_msgs::msg::Pose& CartesianConfiguration::getPose()
132 {
133  return pose_;
134 }
135 
136 inline moveit_msgs::msg::Constraints CartesianConfiguration::toGoalConstraints() const
137 {
138  if (!tolerance_pose_ || !tolerance_angle_)
139  {
140  return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_));
141  }
142  else
143  {
144  return kinematic_constraints::constructGoalConstraints(link_name_, toStampedPose(pose_), tolerance_pose_.value(),
145  tolerance_angle_.value());
146  }
147 }
148 
150 {
151  seed_ = config;
152 }
153 
155 {
156  return seed_.value();
157 }
158 
160 {
161  return seed_.has_value();
162 }
163 
164 inline void CartesianConfiguration::setPoseTolerance(const double tol)
165 {
166  tolerance_pose_ = tol;
167 }
168 
169 inline const std::optional<double> CartesianConfiguration::getPoseTolerance() const
170 {
171  return tolerance_pose_;
172 }
173 
174 inline void CartesianConfiguration::setAngleTolerance(const double tol)
175 {
176  tolerance_angle_ = tol;
177 }
178 
179 inline const std::optional<double> CartesianConfiguration::getAngleTolerance() const
180 {
181  return tolerance_angle_;
182 }
183 } // namespace pilz_industrial_motion_planner_testutils
Class to define a robot configuration in space with the help of cartesian coordinates.
bool hasSeed() const
States if a seed for the cartesian configuration is set.
moveit_msgs::msg::Constraints toGoalConstraints() const override
moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override
Class to define a robot configuration in space with the help of joint values.
Class to define robot configuration in space.
moveit_msgs::msg::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance_below, double tolerance_above)
Generates a constraint message intended to be used as a goal constraint for a joint group....
Definition: utils.cpp:144
std::ostream & operator<<(std::ostream &, const CartesianConfiguration &)