00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/ompl_interface/parameterization/model_based_state_space.h>
00038 #include <boost/bind.hpp>
00039
00040 ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(const ModelBasedStateSpaceSpecification &spec) : ompl::base::CompoundStateSpace(), spec_(spec)
00041 {
00042
00043 setName(spec_.joint_model_group_->getName());
00044
00045
00046 const std::vector<const robot_model::JointModel*> &joint_model_vector = spec_.joint_model_group_->getJointModels();
00047 for (std::size_t i = 0 ; i < joint_model_vector.size() ; ++i)
00048 if (spec_.joints_bounds_.size() <= i)
00049 spec_.joints_bounds_.push_back(joint_model_vector[i]->getVariableBounds());
00050 else
00051 if (spec_.joints_bounds_[i].size() != joint_model_vector[i]->getVariableBounds().size())
00052 {
00053 logError("Joint '%s' from group '%s' has incorrect bounds specified. Using the default bounds instead.",
00054 joint_model_vector[i]->getName().c_str(), spec_.joint_model_group_->getName().c_str());
00055 spec_.joints_bounds_[i] = joint_model_vector[i]->getVariableBounds();
00056 }
00057
00058
00059 for (std::size_t i = 0 ; i < joint_model_vector.size() ; ++i)
00060 addSubspace(ompl::base::StateSpacePtr(new ModelBasedJointStateSpace(joint_model_vector[i], spec_.joints_bounds_[i])), joint_model_vector[i]->getDistanceFactor());
00061 jointSubspaceCount_ = joint_model_vector.size();
00062
00063
00064 setTagSnapToSegment(0.95);
00065
00067 params_.declareParam<double>("tag_snap_to_segment",
00068 boost::bind(&ModelBasedStateSpace::setTagSnapToSegment, this, _1),
00069 boost::bind(&ModelBasedStateSpace::getTagSnapToSegment, this));
00070 }
00071
00072 ompl_interface::ModelBasedStateSpace::~ModelBasedStateSpace()
00073 {
00074 }
00075
00076 double ompl_interface::ModelBasedStateSpace::getTagSnapToSegment() const
00077 {
00078 return tag_snap_to_segment_;
00079 }
00080
00081 void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap)
00082 {
00083 if (snap < 0.0 || snap > 1.0)
00084 logWarn("Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. Value remains as previously set (%lf)", tag_snap_to_segment_);
00085 else
00086 {
00087 tag_snap_to_segment_ = snap;
00088 tag_snap_to_segment_complement_ = 1.0 - tag_snap_to_segment_;
00089 }
00090 }
00091
00092 ompl::base::State* ompl_interface::ModelBasedStateSpace::allocState() const
00093 {
00094 StateType *state = new StateType();
00095 allocStateComponents(state);
00096 return state;
00097 }
00098
00099 void ompl_interface::ModelBasedStateSpace::freeState(ompl::base::State *state) const
00100 {
00101 CompoundStateSpace::freeState(state);
00102 }
00103
00104 void ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State *destination, const ompl::base::State *source) const
00105 {
00106 CompoundStateSpace::copyState(destination, source);
00107 destination->as<StateType>()->tag = source->as<StateType>()->tag;
00108 destination->as<StateType>()->flags = source->as<StateType>()->flags;
00109 destination->as<StateType>()->distance = source->as<StateType>()->distance;
00110 }
00111
00112 unsigned int ompl_interface::ModelBasedStateSpace::getSerializationLength() const
00113 {
00114 return CompoundStateSpace::getSerializationLength() + sizeof(int);
00115 }
00116
00117 void ompl_interface::ModelBasedStateSpace::serialize(void *serialization, const ompl::base::State *state) const
00118 {
00119 *reinterpret_cast<int*>(serialization) = state->as<StateType>()->tag;
00120 CompoundStateSpace::serialize(reinterpret_cast<char*>(serialization) + sizeof(int) , state);
00121 }
00122
00123 void ompl_interface::ModelBasedStateSpace::deserialize(ompl::base::State *state, const void *serialization) const
00124 {
00125 state->as<StateType>()->tag = *reinterpret_cast<const int*>(serialization);
00126 CompoundStateSpace::deserialize(state, reinterpret_cast<const char*>(serialization) + sizeof(int));
00127 }
00128
00129 double ompl_interface::ModelBasedStateSpace::getMaximumExtent() const
00130 {
00131 double total = 0.0;
00132 for (unsigned int i = 0 ; i < jointSubspaceCount_ ; ++i)
00133 {
00134 double e = components_[i]->getMaximumExtent();
00135 total += e * e * weights_[i];
00136 }
00137 return sqrt(total);
00138 }
00139
00140 double ompl_interface::ModelBasedStateSpace::distance(const ompl::base::State *state1, const ompl::base::State *state2) const
00141 {
00142 if (distance_function_)
00143 return distance_function_(state1, state2);
00144 else
00145 {
00146 double total = 0.0;
00147 for (unsigned int i = 0 ; i < jointSubspaceCount_ ; ++i)
00148 {
00149 double d = components_[i]->distance(state1->as<StateType>()->components[i], state2->as<StateType>()->components[i]);
00150 total += d * d * weights_[i];
00151 }
00152 return sqrt(total);
00153 }
00154 }
00155
00156 void ompl_interface::ModelBasedStateSpace::interpolate(const ompl::base::State *from, const ompl::base::State *to, const double t, ompl::base::State *state) const
00157 {
00158
00159 state->as<StateType>()->clearKnownInformation();
00160
00161 if (!interpolation_function_ || !interpolation_function_(from, to, t, state))
00162 {
00163
00164 CompoundStateSpace::interpolate(from, to, t, state);
00165
00166
00167 if (from->as<StateType>()->tag >= 0 && t < 1.0 - tag_snap_to_segment_)
00168 state->as<StateType>()->tag = from->as<StateType>()->tag;
00169 else
00170 if (to->as<StateType>()->tag >= 0 && t > tag_snap_to_segment_)
00171 state->as<StateType>()->tag = to->as<StateType>()->tag;
00172 else
00173 state->as<StateType>()->tag = -1;
00174 }
00175 }
00176
00177 void ompl_interface::ModelBasedStateSpace::afterStateSample(ompl::base::State *sample) const
00178 {
00179 sample->as<StateType>()->clearKnownInformation();
00180 }
00181
00182 namespace ompl_interface
00183 {
00184 class WrappedStateSampler : public ompl::base::StateSampler
00185 {
00186 public:
00187
00188 WrappedStateSampler(const ompl::base::StateSpace *space, const ompl::base::StateSamplerPtr &wrapped) : ompl::base::StateSampler(space), wrapped_(wrapped)
00189 {
00190 }
00191
00192 virtual void sampleUniform(ompl::base::State *state)
00193 {
00194 wrapped_->sampleUniform(state);
00195 space_->as<ModelBasedStateSpace>()->afterStateSample(state);
00196 }
00197
00198 virtual void sampleUniformNear(ompl::base::State *state, const ompl::base::State *near, const double distance)
00199 {
00200 wrapped_->sampleUniformNear(state, near, distance);
00201 space_->as<ModelBasedStateSpace>()->afterStateSample(state);
00202 }
00203
00204 virtual void sampleGaussian(ompl::base::State *state, const ompl::base::State *mean, const double stdDev)
00205 {
00206 wrapped_->sampleGaussian(state, mean, stdDev);
00207 space_->as<ModelBasedStateSpace>()->afterStateSample(state);
00208 }
00209
00210 protected:
00211
00212 ompl::base::StateSamplerPtr wrapped_;
00213 };
00214 }
00215
00216 ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocSubspaceStateSampler(const ompl::base::StateSpace *subspace) const
00217 {
00218 return ompl::base::StateSamplerPtr(new WrappedStateSampler(this, ompl::base::CompoundStateSpace::allocSubspaceStateSampler(subspace)));
00219 }
00220
00221 ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocStateSampler() const
00222 {
00223 return ompl::base::StateSamplerPtr(new WrappedStateSampler(this, ompl::base::CompoundStateSpace::allocStateSampler()));
00224 }
00225
00226 void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State *state, std::ostream &out) const
00227 {
00228 ompl::base::CompoundStateSpace::printState(state, out);
00229 if (state->as<StateType>()->isStartState())
00230 out << "* start state" << std::endl;
00231 if (state->as<StateType>()->isGoalState())
00232 out << "* goal state" << std::endl;
00233 if (state->as<StateType>()->isValidityKnown())
00234 {
00235 if (state->as<StateType>()->isMarkedValid())
00236 out << "* valid state" << std::endl;
00237 else
00238 out << "* invalid state" << std::endl;
00239 }
00240 out << "Tag: " << state->as<StateType>()->tag << std::endl;
00241 }
00242
00243 void ompl_interface::ModelBasedStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ)
00244 {
00245 for (std::size_t i = 0 ; i < jointSubspaceCount_ ; ++i)
00246 components_[i]->as<ModelBasedJointStateSpace>()->setPlanningVolume(minX, maxX, minY, maxY, minZ, maxZ);
00247 }
00248
00249 void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::JointStateGroup* jsg, const ompl::base::State *state) const
00250 {
00251 const std::vector<robot_state::JointState*> &dest = jsg->getJointStateVector();
00252 for (std::size_t i = 0 ; i < dest.size() ; ++i)
00253 *dest[i] = *state->as<ompl::base::CompoundState>()->as<ModelBasedJointStateSpace::StateType>(i)->joint_state;
00254 jsg->updateLinkTransforms();
00255 }
00256
00257 void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State *state, const robot_state::JointStateGroup* jsg) const
00258 {
00259 const std::vector<robot_state::JointState*> &src = jsg->getJointStateVector();
00260 for (std::size_t i = 0 ; i < src.size() ; ++i)
00261 *state->as<ompl::base::CompoundState>()->as<ModelBasedJointStateSpace::StateType>(i)->joint_state = *src[i];
00262 state->as<ModelBasedStateSpace::StateType>()->clearKnownInformation();
00263 }