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 
42  const std::string& link)
43  : ompl::base::ProjectionEvaluator(pc->getOMPLStateSpace())
44  , planning_context_(pc)
45  , link_(planning_context_->getJointModelGroup()->getLinkModel(link))
46  , tss_(planning_context_->getCompleteInitialRobotState())
47 {
48 }
49 
51 {
52  return 3;
53 }
54 
56 {
57  cellSizes_.resize(3);
58  cellSizes_[0] = 0.1;
59  cellSizes_[1] = 0.1;
60  cellSizes_[2] = 0.1;
61 }
62 
63 void ompl_interface::ProjectionEvaluatorLinkPose::project(const ompl::base::State* state,
64  ompl::base::EuclideanProjection& projection) const
65 {
66  robot_state::RobotState* s = tss_.getStateStorage();
67  planning_context_->getOMPLStateSpace()->copyToRobotState(*s, state);
68 
69  const Eigen::Vector3d& o = s->getGlobalLinkTransform(link_).translation();
70  projection(0) = o.x();
71  projection(1) = o.y();
72  projection(2) = o.z();
73 }
74 
76  const std::vector<unsigned int>& variables)
77  : ompl::base::ProjectionEvaluator(pc->getOMPLStateSpace()), planning_context_(pc), variables_(variables)
78 {
79 }
80 
82 {
83  return variables_.size();
84 }
85 
87 {
88  cellSizes_.clear();
89  cellSizes_.resize(variables_.size(), 0.1);
90 }
91 
92 void ompl_interface::ProjectionEvaluatorJointValue::project(const ompl::base::State* state,
93  ompl::base::EuclideanProjection& projection) const
94 {
95  for (std::size_t i = 0; i < variables_.size(); ++i)
96  projection(i) = state->as<ModelBasedStateSpace::StateType>()->values[variables_[i]];
97 }
std::vector< double > values
virtual void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
XmlRpcServer s
const ModelBasedPlanningContext * planning_context_
robot_state::RobotState * getStateStorage() const
ProjectionEvaluatorJointValue(const ModelBasedPlanningContext *pc, const std::vector< unsigned int > &variables)
virtual void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
ProjectionEvaluatorLinkPose(const ModelBasedPlanningContext *pc, const std::string &link)
const ModelBasedStateSpacePtr & getOMPLStateSpace() const


ompl
Author(s): Ioan Sucan
autogenerated on Mon Jan 15 2018 03:51:38