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 #ifndef CARTESIANCONFIGURATION_H
36 #define CARTESIANCONFIGURATION_H
37 
38 #include <vector>
39 #include <sstream>
40 
41 #include <boost/optional.hpp>
42 #include <geometry_msgs/Pose.h>
43 #include <geometry_msgs/PoseStamped.h>
48 
49 #include "robotconfiguration.h"
50 #include "jointconfiguration.h"
51 
53 {
58 class CartesianConfiguration : public RobotConfiguration
59 {
60 public:
62 
63  CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector<double>& config);
64 
65  CartesianConfiguration(const std::string& group_name, const std::string& link_name, const std::vector<double>& config,
66  const moveit::core::RobotModelConstPtr& robot_model);
67 
68 public:
69  moveit_msgs::Constraints toGoalConstraints() const override;
70  moveit_msgs::RobotState toMoveitMsgsRobotState() const override;
71 
72  void setLinkName(const std::string& link_name);
73  const std::string& getLinkName() const;
74 
75  void setPose(const geometry_msgs::Pose& pose);
76  const geometry_msgs::Pose& getPose() const;
77  geometry_msgs::Pose& getPose();
78 
79  void setSeed(const JointConfiguration& config);
80  const JointConfiguration& getSeed() const;
82  bool hasSeed() const;
83 
84  void setPoseTolerance(const double tol);
85  const boost::optional<double> getPoseTolerance() const;
86 
87  void setAngleTolerance(const double tol);
88  const boost::optional<double> getAngleTolerance() const;
89 
90 private:
91  static geometry_msgs::Pose toPose(const std::vector<double>& pose);
92  static geometry_msgs::PoseStamped toStampedPose(const geometry_msgs::Pose& pose);
93 
94 private:
95  std::string link_name_;
96  geometry_msgs::Pose pose_;
97 
100  boost::optional<double> tolerance_pose_{ boost::none };
101 
104  boost::optional<double> tolerance_angle_{ boost::none };
105 
107  boost::optional<JointConfiguration> seed_{ boost::none };
108 };
109 
110 std::ostream& operator<<(std::ostream& /*os*/, const CartesianConfiguration& /*obj*/);
111 
112 inline void CartesianConfiguration::setLinkName(const std::string& link_name)
113 {
114  link_name_ = link_name;
115 }
116 
117 inline const std::string& CartesianConfiguration::getLinkName() const
118 {
119  return link_name_;
120 }
121 
122 inline void CartesianConfiguration::setPose(const geometry_msgs::Pose& pose)
123 {
124  pose_ = pose;
125 }
126 
127 inline const geometry_msgs::Pose& CartesianConfiguration::getPose() const
128 {
129  return pose_;
130 }
131 
132 inline geometry_msgs::Pose& CartesianConfiguration::getPose()
133 {
134  return pose_;
135 }
136 
137 inline moveit_msgs::Constraints CartesianConfiguration::toGoalConstraints() const
138 {
140  {
142  }
143  else
144  {
146  tolerance_angle_.value());
147  }
148 }
149 
150 inline void CartesianConfiguration::setSeed(const JointConfiguration& config)
151 {
152  seed_ = config;
153 }
154 
156 {
157  return seed_.value();
158 }
159 
161 {
162  return seed_.is_initialized();
163 }
164 
165 inline void CartesianConfiguration::setPoseTolerance(const double tol)
166 {
167  tolerance_pose_ = tol;
168 }
169 
170 inline const boost::optional<double> CartesianConfiguration::getPoseTolerance() const
171 {
172  return tolerance_pose_;
173 }
174 
175 inline void CartesianConfiguration::setAngleTolerance(const double tol)
176 {
177  tolerance_angle_ = tol;
178 }
179 
180 inline const boost::optional<double> CartesianConfiguration::getAngleTolerance() const
181 {
183 }
184 } // namespace pilz_industrial_motion_planner_testutils
185 
186 #endif // CARTESIANCONFIGURATION_H
robot_model.h
pilz_industrial_motion_planner_testutils::CartesianConfiguration::tolerance_pose_
boost::optional< double > tolerance_pose_
The dimensions of the sphere associated with the target region of the position constraint.
Definition: cartesianconfiguration.h:164
pilz_industrial_motion_planner_testutils::CartesianConfiguration::setPoseTolerance
void setPoseTolerance(const double tol)
Definition: cartesianconfiguration.h:197
pilz_industrial_motion_planner_testutils::CartesianConfiguration::toGoalConstraints
moveit_msgs::Constraints toGoalConstraints() const override
Definition: cartesianconfiguration.h:169
pilz_industrial_motion_planner_testutils::CartesianConfiguration::toStampedPose
static geometry_msgs::PoseStamped toStampedPose(const geometry_msgs::Pose &pose)
Definition: cartesianconfiguration.cpp:118
utils.h
pilz_industrial_motion_planner_testutils::CartesianConfiguration::getPoseTolerance
const boost::optional< double > getPoseTolerance() const
Definition: cartesianconfiguration.h:202
pilz_industrial_motion_planner_testutils::CartesianConfiguration::setSeed
void setSeed(const JointConfiguration &config)
Definition: cartesianconfiguration.h:182
kinematic_constraints::constructGoalConstraints
moveit_msgs::Constraints constructGoalConstraints(const moveit::core::RobotState &state, const moveit::core::JointModelGroup *jmg, double tolerance=std::numeric_limits< double >::epsilon())
pilz_industrial_motion_planner_testutils::CartesianConfiguration::pose_
geometry_msgs::Pose pose_
Definition: cartesianconfiguration.h:160
jointconfiguration.h
robot_state.h
robotconfiguration.h
pilz_industrial_motion_planner_testutils::CartesianConfiguration::getAngleTolerance
const boost::optional< double > getAngleTolerance() const
Definition: cartesianconfiguration.h:212
pilz_industrial_motion_planner_testutils::CartesianConfiguration::setPose
void setPose(const geometry_msgs::Pose &pose)
Definition: cartesianconfiguration.h:154
pilz_industrial_motion_planner_testutils
Definition: basecmd.h:42
pilz_industrial_motion_planner_testutils::CartesianConfiguration::getLinkName
const std::string & getLinkName() const
Definition: cartesianconfiguration.h:149
pilz_industrial_motion_planner_testutils::operator<<
std::ostream & operator<<(std::ostream &, const CartesianConfiguration &)
Definition: cartesianconfiguration.cpp:157
pilz_industrial_motion_planner_testutils::CartesianConfiguration::getPose
const geometry_msgs::Pose & getPose() const
Definition: cartesianconfiguration.h:159
pilz_industrial_motion_planner_testutils::CartesianConfiguration::seed_
boost::optional< JointConfiguration > seed_
The seed for computing the IK solution of the cartesian configuration.
Definition: cartesianconfiguration.h:171
pilz_industrial_motion_planner_testutils::CartesianConfiguration::link_name_
std::string link_name_
Definition: cartesianconfiguration.h:159
pilz_industrial_motion_planner_testutils::CartesianConfiguration::toMoveitMsgsRobotState
moveit_msgs::RobotState toMoveitMsgsRobotState() const override
Definition: cartesianconfiguration.cpp:125
pilz_industrial_motion_planner_testutils::CartesianConfiguration::tolerance_angle_
boost::optional< double > tolerance_angle_
The value to assign to the absolute tolerances of the orientation constraint.
Definition: cartesianconfiguration.h:168
pilz_industrial_motion_planner_testutils::CartesianConfiguration::setLinkName
void setLinkName(const std::string &link_name)
Definition: cartesianconfiguration.h:144
pilz_industrial_motion_planner_testutils::CartesianConfiguration::getSeed
const JointConfiguration & getSeed() const
Definition: cartesianconfiguration.h:187
conversions.h
pilz_industrial_motion_planner_testutils::CartesianConfiguration::CartesianConfiguration
CartesianConfiguration()
Definition: cartesianconfiguration.cpp:74
pilz_industrial_motion_planner_testutils::JointConfiguration
Class to define a robot configuration in space with the help of joint values.
Definition: jointconfiguration.h:98
config
config
pilz_industrial_motion_planner_testutils::CartesianConfiguration::hasSeed
bool hasSeed() const
States if a seed for the cartesian configuration is set.
Definition: cartesianconfiguration.h:192
pilz_industrial_motion_planner_testutils::CartesianConfiguration::setAngleTolerance
void setAngleTolerance(const double tol)
Definition: cartesianconfiguration.h:207
pilz_industrial_motion_planner_testutils::CartesianConfiguration::toPose
static geometry_msgs::Pose toPose(const std::vector< double > &pose)
Definition: cartesianconfiguration.cpp:104


pilz_industrial_motion_planner_testutils
Author(s):
autogenerated on Sat May 3 2025 02:28:27