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