moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
jointconfiguration.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 <string>
38#include <utility>
39#include <vector>
40#include <functional>
41#include <stdexcept>
42
43#include <sensor_msgs/msg/joint_state.hpp>
46
47#include "robotconfiguration.h"
48
50{
51class JointConfigurationException : public std::runtime_error
52{
53public:
54 JointConfigurationException(const std::string& error_desc) : std::runtime_error(error_desc)
55 {
56 }
57};
58
59using CreateJointNameFunc = std::function<std::string(const size_t&)>;
60
66{
67public:
69
70 JointConfiguration(const std::string& group_name, const std::vector<double>& config,
71 CreateJointNameFunc&& create_joint_name_func);
72
73 JointConfiguration(const std::string& group_name, const std::vector<double>& config,
74 const moveit::core::RobotModelConstPtr& robot_model);
75
76public:
77 void setJoint(const size_t index, const double value);
78 double getJoint(const size_t index) const;
79 const std::vector<double> getJoints() const;
80
81 size_t size() const;
82
83 moveit_msgs::msg::Constraints toGoalConstraints() const override;
84 moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override;
85
86 sensor_msgs::msg::JointState toSensorMsg() const;
87
89
90 void setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func);
91
92private:
93 moveit_msgs::msg::RobotState toMoveitMsgsRobotStateWithoutModel() const;
94 moveit_msgs::msg::RobotState toMoveitMsgsRobotStateWithModel() const;
95
96 moveit_msgs::msg::Constraints toGoalConstraintsWithoutModel() const;
97 moveit_msgs::msg::Constraints toGoalConstraintsWithModel() const;
98
99private:
101 std::vector<double> joints_;
102
103 CreateJointNameFunc create_joint_name_func_;
104};
105
106std::ostream& operator<<(std::ostream& /*os*/, const JointConfiguration& /*obj*/);
107
108inline moveit_msgs::msg::Constraints JointConfiguration::toGoalConstraints() const
109{
110 return robot_model_ ? toGoalConstraintsWithModel() : toGoalConstraintsWithoutModel();
111}
112
113inline moveit_msgs::msg::RobotState JointConfiguration::toMoveitMsgsRobotState() const
114{
115 return robot_model_ ? toMoveitMsgsRobotStateWithModel() : toMoveitMsgsRobotStateWithoutModel();
116}
117
118inline void JointConfiguration::setJoint(const size_t index, const double value)
119{
120 joints_.at(index) = value;
121}
122
123inline double JointConfiguration::getJoint(const size_t index) const
124{
125 return joints_.at(index);
126}
127
128inline const std::vector<double> JointConfiguration::getJoints() const
129{
130 return joints_;
131}
132
133inline size_t JointConfiguration::size() const
134{
135 return joints_.size();
136}
137
139{
140 create_joint_name_func_ = std::move(create_joint_name_func);
141}
142} // namespace pilz_industrial_motion_planner_testutils
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition robot_state.h:90
Class to define a robot configuration in space with the help of joint values.
moveit_msgs::msg::Constraints toGoalConstraints() const override
void setJoint(const size_t index, const double value)
moveit_msgs::msg::RobotState toMoveitMsgsRobotState() const override
void setCreateJointNameFunc(CreateJointNameFunc create_joint_name_func)
Class to define robot configuration in space.
std::function< std::string(const size_t &)> CreateJointNameFunc
std::ostream & operator<<(std::ostream &, const CartesianConfiguration &)