moveit2
The MoveIt Motion Planning Framework for ROS 2.
projection_evaluators.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 #include <utility>
42 
44  const std::string& link)
45  : ompl::base::ProjectionEvaluator(pc->getOMPLStateSpace())
46  , planning_context_(pc)
47  , link_(planning_context_->getJointModelGroup()->getLinkModel(link))
48  , tss_(planning_context_->getCompleteInitialRobotState())
49 {
50 }
51 
53 {
54  return 3;
55 }
56 
58 {
59  cellSizes_.resize(3);
60  cellSizes_[0] = 0.1;
61  cellSizes_[1] = 0.1;
62  cellSizes_[2] = 0.1;
63 }
64 
65 void ompl_interface::ProjectionEvaluatorLinkPose::project(const ompl::base::State* state,
66  OMPLProjection projection) const
67 {
68  moveit::core::RobotState* s = tss_.getStateStorage();
69  planning_context_->getOMPLStateSpace()->copyToRobotState(*s, state);
70 
71  const Eigen::Vector3d& o = s->getGlobalLinkTransform(link_).translation();
72  projection(0) = o.x();
73  projection(1) = o.y();
74  projection(2) = o.z();
75 }
76 
78  std::vector<unsigned int> variables)
79  : ompl::base::ProjectionEvaluator(pc->getOMPLStateSpace()), variables_(std::move(variables))
80 {
81 }
82 
84 {
85  return variables_.size();
86 }
87 
89 {
90  cellSizes_.clear();
91  cellSizes_.resize(variables_.size(), 0.1);
92 }
93 
94 void ompl_interface::ProjectionEvaluatorJointValue::project(const ompl::base::State* state,
95  OMPLProjection projection) const
96 {
97  for (std::size_t i = 0; i < variables_.size(); ++i)
98  projection(i) = state->as<ModelBasedStateSpace::StateType>()->values[variables_[i]];
99 }
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
Get the link transform w.r.t. the root link (model frame) of the RobotModel. This is typically the ro...
Definition: robot_state.h:1339
void project(const ompl::base::State *state, OMPLProjection projection) const override
ProjectionEvaluatorJointValue(const ModelBasedPlanningContext *pc, std::vector< unsigned int > variables)
ProjectionEvaluatorLinkPose(const ModelBasedPlanningContext *pc, const std::string &link)
void project(const ompl::base::State *state, OMPLProjection projection) const override
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
ompl::base::EuclideanProjection & OMPLProjection