moveit2
The MoveIt Motion Planning Framework for ROS 2.
jointconfiguration.cpp
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 
36 
38 
40 {
42 {
43 }
44 
45 JointConfiguration::JointConfiguration(const std::string& group_name, const std::vector<double>& config,
46  CreateJointNameFunc&& create_joint_name_func)
47  : RobotConfiguration(group_name), joints_(config), create_joint_name_func_(create_joint_name_func)
48 {
49 }
50 
51 JointConfiguration::JointConfiguration(const std::string& group_name, const std::vector<double>& config,
52  const moveit::core::RobotModelConstPtr& robot_model)
53  : RobotConfiguration(group_name, robot_model), joints_(config)
54 {
55 }
56 
57 moveit_msgs::msg::Constraints JointConfiguration::toGoalConstraintsWithoutModel() const
58 {
59  if (!create_joint_name_func_)
60  {
61  throw JointConfigurationException("Create-Joint-Name function not set");
62  }
63 
64  moveit_msgs::msg::Constraints gc;
65 
66  for (size_t i = 0; i < joints_.size(); ++i)
67  {
68  moveit_msgs::msg::JointConstraint jc;
69  jc.joint_name = create_joint_name_func_(i);
70  jc.position = joints_.at(i);
71  gc.joint_constraints.push_back(jc);
72  }
73 
74  return gc;
75 }
76 
77 moveit_msgs::msg::Constraints JointConfiguration::toGoalConstraintsWithModel() const
78 {
79  if (!robot_model_)
80  {
81  throw JointConfigurationException("No robot model set");
82  }
83 
85  state.setToDefaultValues();
86  state.setJointGroupPositions(group_name_, joints_);
87 
88  return kinematic_constraints::constructGoalConstraints(state, state.getRobotModel()->getJointModelGroup(group_name_));
89 }
90 
91 moveit_msgs::msg::RobotState JointConfiguration::toMoveitMsgsRobotStateWithoutModel() const
92 {
93  if (!create_joint_name_func_)
94  {
95  throw JointConfigurationException("Create-Joint-Name function not set");
96  }
97 
98  moveit_msgs::msg::RobotState robot_state;
99  for (size_t i = 0; i < joints_.size(); ++i)
100  {
101  robot_state.joint_state.name.emplace_back(create_joint_name_func_(i));
102  robot_state.joint_state.position.push_back(joints_.at(i));
103  }
104  return robot_state;
105 }
106 
108 {
109  if (!robot_model_)
110  {
111  throw JointConfigurationException("No robot model set");
112  }
113 
115  robot_state.setToDefaultValues();
116  robot_state.setJointGroupPositions(group_name_, joints_);
117  return robot_state;
118 }
119 
120 moveit_msgs::msg::RobotState JointConfiguration::toMoveitMsgsRobotStateWithModel() const
121 {
122  moveit::core::RobotState start_state(toRobotState());
123  moveit_msgs::msg::RobotState rob_state_msg;
124  moveit::core::robotStateToRobotStateMsg(start_state, rob_state_msg, false);
125  return rob_state_msg;
126 }
127 
128 sensor_msgs::msg::JointState JointConfiguration::toSensorMsg() const
129 {
130  if (!create_joint_name_func_)
131  {
132  throw JointConfigurationException("Create-Joint-Name function not set");
133  }
134 
135  sensor_msgs::msg::JointState state;
136  for (size_t i = 0; i < joints_.size(); ++i)
137  {
138  state.name.emplace_back(create_joint_name_func_(i));
139  state.position.push_back(joints_.at(i));
140  }
141  return state;
142 }
143 
144 std::ostream& operator<<(std::ostream& os, const JointConfiguration& obj)
145 {
146  const size_t n{ obj.size() };
147  os << "JointConfiguration: [";
148  for (size_t i = 0; i < n; ++i)
149  {
150  os << obj.getJoint(i);
151  if (i != n - 1)
152  {
153  os << ", ";
154  }
155  }
156  os << "]";
157 
158  return os;
159 }
160 
161 } // namespace pilz_industrial_motion_planner_testutils
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
Definition: robot_state.h:605
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
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
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
std::function< std::string(const size_t &)> CreateJointNameFunc
std::ostream & operator<<(std::ostream &, const CartesianConfiguration &)