utils.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, 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 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 
41 moveit_msgs::Constraints kinematic_constraints::mergeConstraints(const moveit_msgs::Constraints& first,
42  const moveit_msgs::Constraints& second)
43 {
44  moveit_msgs::Constraints r;
45 
46  // add all joint constraints that are in first but not in second
47  // and merge joint constraints that are for the same joint
48  for (std::size_t i = 0; i < first.joint_constraints.size(); ++i)
49  {
50  bool add = true;
51  for (std::size_t j = 0; j < second.joint_constraints.size(); ++j)
52  if (second.joint_constraints[j].joint_name == first.joint_constraints[i].joint_name)
53  {
54  add = false;
55  // now we merge
56  moveit_msgs::JointConstraint m;
57  const moveit_msgs::JointConstraint& a = first.joint_constraints[i];
58  const moveit_msgs::JointConstraint& b = second.joint_constraints[j];
59  double low = std::max(a.position - a.tolerance_below, b.position - b.tolerance_below);
60  double high = std::min(a.position + a.tolerance_above, b.position + b.tolerance_above);
61  if (low > high)
62  ROS_ERROR_NAMED("kinematic_constraints",
63  "Attempted to merge incompatible constraints for joint '%s'. Discarding constraint.",
64  a.joint_name.c_str());
65  else
66  {
67  m.joint_name = a.joint_name;
68  m.position =
69  std::max(low, std::min((a.position * a.weight + b.position * b.weight) / (a.weight + b.weight), high));
70  m.weight = (a.weight + b.weight) / 2.0;
71  m.tolerance_above = std::max(0.0, high - m.position);
72  m.tolerance_below = std::max(0.0, m.position - low);
73  r.joint_constraints.push_back(m);
74  }
75  break;
76  }
77  if (add)
78  r.joint_constraints.push_back(first.joint_constraints[i]);
79  }
80 
81  // add all joint constraints that are in second but not in first
82  for (std::size_t i = 0; i < second.joint_constraints.size(); ++i)
83  {
84  bool add = true;
85  for (std::size_t j = 0; j < first.joint_constraints.size(); ++j)
86  if (second.joint_constraints[i].joint_name == first.joint_constraints[j].joint_name)
87  {
88  add = false;
89  break;
90  }
91  if (add)
92  r.joint_constraints.push_back(second.joint_constraints[i]);
93  }
94 
95  // merge rest of constraints
96  r.position_constraints = first.position_constraints;
97  for (std::size_t i = 0; i < second.position_constraints.size(); ++i)
98  r.position_constraints.push_back(second.position_constraints[i]);
99 
100  r.orientation_constraints = first.orientation_constraints;
101  for (std::size_t i = 0; i < second.orientation_constraints.size(); ++i)
102  r.orientation_constraints.push_back(second.orientation_constraints[i]);
103 
104  r.visibility_constraints = first.visibility_constraints;
105  for (std::size_t i = 0; i < second.visibility_constraints.size(); ++i)
106  r.visibility_constraints.push_back(second.visibility_constraints[i]);
107 
108  return r;
109 }
110 
111 bool kinematic_constraints::isEmpty(const moveit_msgs::Constraints& constr)
112 {
113  return constr.position_constraints.empty() && constr.orientation_constraints.empty() &&
114  constr.visibility_constraints.empty() && constr.joint_constraints.empty();
115 }
116 
117 std::size_t kinematic_constraints::countIndividualConstraints(const moveit_msgs::Constraints& constr)
118 {
119  return constr.position_constraints.size() + constr.orientation_constraints.size() +
120  constr.visibility_constraints.size() + constr.joint_constraints.size();
121 }
122 
124  const robot_model::JointModelGroup* jmg,
125  double tolerance)
126 {
127  return constructGoalConstraints(state, jmg, tolerance, tolerance);
128 }
129 
131  const robot_model::JointModelGroup* jmg,
132  double tolerance_below, double tolerance_above)
133 {
134  moveit_msgs::Constraints goal;
135  std::vector<double> vals;
136  state.copyJointGroupPositions(jmg, vals);
137  goal.joint_constraints.resize(vals.size());
138  for (std::size_t i = 0; i < vals.size(); ++i)
139  {
140  goal.joint_constraints[i].joint_name = jmg->getVariableNames()[i];
141  goal.joint_constraints[i].position = vals[i];
142  goal.joint_constraints[i].tolerance_above = tolerance_below;
143  goal.joint_constraints[i].tolerance_below = tolerance_above;
144  goal.joint_constraints[i].weight = 1.0;
145  }
146 
147  return goal;
148 }
149 
150 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
151  const geometry_msgs::PoseStamped& pose,
152  double tolerance_pos, double tolerance_angle)
153 {
154  moveit_msgs::Constraints goal;
155 
156  goal.position_constraints.resize(1);
157  moveit_msgs::PositionConstraint& pcm = goal.position_constraints[0];
158  pcm.link_name = link_name;
159  pcm.target_point_offset.x = 0;
160  pcm.target_point_offset.y = 0;
161  pcm.target_point_offset.z = 0;
162  pcm.constraint_region.primitives.resize(1);
163  shape_msgs::SolidPrimitive& bv = pcm.constraint_region.primitives[0];
164  bv.type = shape_msgs::SolidPrimitive::SPHERE;
166  bv.dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance_pos;
167 
168  pcm.header = pose.header;
169  pcm.constraint_region.primitive_poses.resize(1);
170  pcm.constraint_region.primitive_poses[0].position = pose.pose.position;
171 
172  // orientation of constraint region does not affect anything, since it is a sphere
173  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
174  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
175  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
176  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
177  pcm.weight = 1.0;
178 
179  goal.orientation_constraints.resize(1);
180  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
181  ocm.link_name = link_name;
182  ocm.header = pose.header;
183  ocm.orientation = pose.pose.orientation;
184  ocm.absolute_x_axis_tolerance = tolerance_angle;
185  ocm.absolute_y_axis_tolerance = tolerance_angle;
186  ocm.absolute_z_axis_tolerance = tolerance_angle;
187  ocm.weight = 1.0;
188 
189  return goal;
190 }
191 
192 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
193  const geometry_msgs::PoseStamped& pose,
194  const std::vector<double>& tolerance_pos,
195  const std::vector<double>& tolerance_angle)
196 {
197  moveit_msgs::Constraints goal = constructGoalConstraints(link_name, pose);
198  if (tolerance_pos.size() == 3)
199  {
200  shape_msgs::SolidPrimitive& bv = goal.position_constraints[0].constraint_region.primitives[0];
201  bv.type = shape_msgs::SolidPrimitive::BOX;
203  bv.dimensions[shape_msgs::SolidPrimitive::BOX_X] = tolerance_pos[0];
204  bv.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = tolerance_pos[1];
205  bv.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = tolerance_pos[2];
206  }
207  if (tolerance_angle.size() == 3)
208  {
209  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
210  ocm.absolute_x_axis_tolerance = tolerance_angle[0];
211  ocm.absolute_y_axis_tolerance = tolerance_angle[1];
212  ocm.absolute_z_axis_tolerance = tolerance_angle[2];
213  }
214  return goal;
215 }
216 
217 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
218  const geometry_msgs::QuaternionStamped& quat,
219  double tolerance)
220 {
221  moveit_msgs::Constraints goal;
222  goal.orientation_constraints.resize(1);
223  moveit_msgs::OrientationConstraint& ocm = goal.orientation_constraints[0];
224  ocm.link_name = link_name;
225  ocm.header = quat.header;
226  ocm.orientation = quat.quaternion;
227  ocm.absolute_x_axis_tolerance = tolerance;
228  ocm.absolute_y_axis_tolerance = tolerance;
229  ocm.absolute_z_axis_tolerance = tolerance;
230  ocm.weight = 1.0;
231  return goal;
232 }
233 
234 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
235  const geometry_msgs::PointStamped& goal_point,
236  double tolerance)
237 {
238  geometry_msgs::Point p;
239  p.x = 0;
240  p.y = 0;
241  p.z = 0;
242  return constructGoalConstraints(link_name, p, goal_point, tolerance);
243 }
244 
245 moveit_msgs::Constraints kinematic_constraints::constructGoalConstraints(const std::string& link_name,
246  const geometry_msgs::Point& reference_point,
247  const geometry_msgs::PointStamped& goal_point,
248  double tolerance)
249 {
250  moveit_msgs::Constraints goal;
251  goal.position_constraints.resize(1);
252  moveit_msgs::PositionConstraint& pcm = goal.position_constraints[0];
253  pcm.link_name = link_name;
254  pcm.target_point_offset.x = reference_point.x;
255  pcm.target_point_offset.y = reference_point.y;
256  pcm.target_point_offset.z = reference_point.z;
257  pcm.constraint_region.primitives.resize(1);
258  pcm.constraint_region.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
259  pcm.constraint_region.primitives[0].dimensions.resize(
261  pcm.constraint_region.primitives[0].dimensions[shape_msgs::SolidPrimitive::SPHERE_RADIUS] = tolerance;
262 
263  pcm.header = goal_point.header;
264  pcm.constraint_region.primitive_poses.resize(1);
265  pcm.constraint_region.primitive_poses[0].position = goal_point.point;
266 
267  // orientation of constraint region does not affect anything, since it is a sphere
268  pcm.constraint_region.primitive_poses[0].orientation.x = 0.0;
269  pcm.constraint_region.primitive_poses[0].orientation.y = 0.0;
270  pcm.constraint_region.primitive_poses[0].orientation.z = 0.0;
271  pcm.constraint_region.primitive_poses[0].orientation.w = 1.0;
272  pcm.weight = 1.0;
273 
274  return goal;
275 }
bool isEmpty(const moveit_msgs::Constraints &constr)
Check if any constraints were specified.
Definition: utils.cpp:111
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...
std::size_t countIndividualConstraints(const moveit_msgs::Constraints &constr)
Definition: utils.cpp:117
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
Definition: robot_state.h:663
moveit_msgs::Constraints constructGoalConstraints(const robot_state::RobotState &state, const robot_model::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:130
#define ROS_ERROR_NAMED(name,...)
Representation of a robot&#39;s state. This includes position, velocity, acceleration and effort...
Definition: robot_state.h:123
moveit_msgs::Constraints mergeConstraints(const moveit_msgs::Constraints &first, const moveit_msgs::Constraints &second)
Merge two sets of constraints into one.
Definition: utils.cpp:41
r


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Oct 18 2018 02:47:09