pose_model_state_space.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h>
00038 #include <ompl/base/spaces/SE3StateSpace.h>
00039 #include <moveit/profiler/profiler.h>
00040 
00041 const std::string ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel";
00042 
00043 ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec)
00044   : ModelBasedStateSpace(spec)
00045 {
00046   jump_factor_ = 3;  // \todo make this a param
00047 
00048   if (spec.joint_model_group_->getGroupKinematics().first)
00049     poses_.push_back(PoseComponent(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first));
00050   else if (!spec.joint_model_group_->getGroupKinematics().second.empty())
00051   {
00052     const robot_model::JointModelGroup::KinematicsSolverMap& m = spec.joint_model_group_->getGroupKinematics().second;
00053     for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = m.begin(); it != m.end(); ++it)
00054       poses_.push_back(PoseComponent(it->first, it->second));
00055   }
00056   if (poses_.empty())
00057     logError("No kinematics solvers specified. Unable to construct a PoseModelStateSpace");
00058   else
00059     std::sort(poses_.begin(), poses_.end());
00060   setName(getName() + "_" + PARAMETERIZATION_TYPE);
00061 }
00062 
00063 ompl_interface::PoseModelStateSpace::~PoseModelStateSpace()
00064 {
00065 }
00066 
00067 double ompl_interface::PoseModelStateSpace::distance(const ompl::base::State* state1,
00068                                                      const ompl::base::State* state2) const
00069 {
00070   double total = 0;
00071   for (std::size_t i = 0; i < poses_.size(); ++i)
00072     total += poses_[i].state_space_->distance(state1->as<StateType>()->poses[i], state2->as<StateType>()->poses[i]);
00073   return total;
00074 }
00075 
00076 double ompl_interface::PoseModelStateSpace::getMaximumExtent() const
00077 {
00078   double total = 0.0;
00079   for (std::size_t i = 0; i < poses_.size(); ++i)
00080     total += poses_[i].state_space_->getMaximumExtent();
00081   return total;
00082 }
00083 
00084 ompl::base::State* ompl_interface::PoseModelStateSpace::allocState() const
00085 {
00086   StateType* state = new StateType();
00087   state->values =
00088       new double[variable_count_];  // need to allocate this here since ModelBasedStateSpace::allocState() is not called
00089   state->poses = new ompl::base::SE3StateSpace::StateType*[poses_.size()];
00090   for (std::size_t i = 0; i < poses_.size(); ++i)
00091     state->poses[i] = poses_[i].state_space_->allocState()->as<ompl::base::SE3StateSpace::StateType>();
00092   return state;
00093 }
00094 
00095 void ompl_interface::PoseModelStateSpace::freeState(ompl::base::State* state) const
00096 {
00097   for (std::size_t i = 0; i < poses_.size(); ++i)
00098     poses_[i].state_space_->freeState(state->as<StateType>()->poses[i]);
00099   delete[] state->as<StateType>()->poses;
00100   ModelBasedStateSpace::freeState(state);
00101 }
00102 
00103 void ompl_interface::PoseModelStateSpace::copyState(ompl::base::State* destination,
00104                                                     const ompl::base::State* source) const
00105 {
00106   // copy the state data
00107   ModelBasedStateSpace::copyState(destination, source);
00108 
00109   for (std::size_t i = 0; i < poses_.size(); ++i)
00110     poses_[i].state_space_->copyState(destination->as<StateType>()->poses[i], source->as<StateType>()->poses[i]);
00111 
00112   // compute additional stuff if needed
00113   computeStateK(destination);
00114 }
00115 
00116 void ompl_interface::PoseModelStateSpace::sanityChecks() const
00117 {
00118   ModelBasedStateSpace::sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(),
00119                                      ~ompl::base::StateSpace::STATESPACE_TRIANGLE_INEQUALITY);
00120 }
00121 
00122 void ompl_interface::PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to,
00123                                                       const double t, ompl::base::State* state) const
00124 {
00125   //  moveit::Profiler::ScopedBlock sblock("interpolate");
00126 
00127   // we want to interpolate in Cartesian space; we do not have a guarantee that from and to
00128   // have their poses computed, but this is very unlikely to happen (depends how the planner gets its input states)
00129 
00130   // interpolate in joint space
00131   ModelBasedStateSpace::interpolate(from, to, t, state);
00132 
00133   // interpolate SE3 components
00134   for (std::size_t i = 0; i < poses_.size(); ++i)
00135     poses_[i].state_space_->interpolate(from->as<StateType>()->poses[i], to->as<StateType>()->poses[i], t,
00136                                         state->as<StateType>()->poses[i]);
00137 
00138   // the call above may reset all flags for state; but we know the pose we want flag should be set
00139   state->as<StateType>()->setPoseComputed(true);
00140 
00141   /*
00142   std::cout << "*********** interpolate\n";
00143   printState(from, std::cout);
00144   printState(to, std::cout);
00145   printState(state, std::cout);
00146   std::cout << "\n\n";
00147   */
00148 
00149   // after interpolation we cannot be sure about the joint values (we use them as seed only)
00150   // so we recompute IK if needed
00151   if (computeStateIK(state))
00152   {
00153     double dj = jump_factor_ * ModelBasedStateSpace::distance(from, to);
00154     double d_from = ModelBasedStateSpace::distance(from, state);
00155     double d_to = ModelBasedStateSpace::distance(state, to);
00156 
00157     // if the joint value jumped too much
00158     if (d_from + d_to > std::max(0.2, dj))  // \todo make 0.2 a param
00159       state->as<StateType>()->markInvalid();
00160   }
00161 }
00162 
00163 void ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY,
00164                                                             double minZ, double maxZ)
00165 {
00166   ModelBasedStateSpace::setPlanningVolume(minX, maxX, minY, maxY, minZ, maxZ);
00167   ompl::base::RealVectorBounds b(3);
00168   b.low[0] = minX;
00169   b.low[1] = minY;
00170   b.low[2] = minZ;
00171   b.high[0] = maxX;
00172   b.high[1] = maxY;
00173   b.high[2] = maxZ;
00174   for (std::size_t i = 0; i < poses_.size(); ++i)
00175     poses_[i].state_space_->as<ompl::base::SE3StateSpace>()->setBounds(b);
00176 }
00177 
00178 ompl_interface::PoseModelStateSpace::PoseComponent::PoseComponent(
00179     const robot_model::JointModelGroup* subgroup, const robot_model::JointModelGroup::KinematicsSolver& k)
00180   : subgroup_(subgroup), kinematics_solver_(k.allocator_(subgroup)), bijection_(k.bijection_)
00181 {
00182   state_space_.reset(new ompl::base::SE3StateSpace());
00183   state_space_->setName(subgroup_->getName() + "_Workspace");
00184   fk_link_.resize(1, kinematics_solver_->getTipFrame());
00185   if (!fk_link_[0].empty() && fk_link_[0][0] == '/')
00186     fk_link_[0] = fk_link_[0].substr(1);
00187 }
00188 
00189 bool ompl_interface::PoseModelStateSpace::PoseComponent::computeStateFK(StateType* full_state, unsigned int idx) const
00190 {
00191   // read the values from the joint state, in the order expected by the kinematics solver
00192   std::vector<double> values(bijection_.size());
00193   for (unsigned int i = 0; i < bijection_.size(); ++i)
00194     values[i] = full_state->values[bijection_[i]];
00195 
00196   // compute forward kinematics for the link of interest
00197   std::vector<geometry_msgs::Pose> poses;
00198   if (!kinematics_solver_->getPositionFK(fk_link_, values, poses))
00199     return false;
00200 
00201   // copy the resulting data to the desired location in the state
00202   ompl::base::SE3StateSpace::StateType* se3_state = full_state->poses[idx];
00203   se3_state->setXYZ(poses[0].position.x, poses[0].position.y, poses[0].position.z);
00204   ompl::base::SO3StateSpace::StateType& so3_state = se3_state->rotation();
00205   so3_state.x = poses[0].orientation.x;
00206   so3_state.y = poses[0].orientation.y;
00207   so3_state.z = poses[0].orientation.z;
00208   so3_state.w = poses[0].orientation.w;
00209 
00210   return true;
00211 }
00212 
00213 bool ompl_interface::PoseModelStateSpace::PoseComponent::computeStateIK(StateType* full_state, unsigned int idx) const
00214 {
00215   // read the values from the joint state, in the order expected by the kinematics solver; use these as the seed
00216   std::vector<double> seed_values(bijection_.size());
00217   for (std::size_t i = 0; i < bijection_.size(); ++i)
00218     seed_values[i] = full_state->values[bijection_[i]];
00219 
00220   /*
00221   std::cout << "seed: ";
00222   for (std::size_t i = 0 ; i < seed_values.size() ; ++i)
00223     std::cout << seed_values[i] << " ";
00224   std::cout << std::endl;
00225   */
00226 
00227   // construct the pose
00228   geometry_msgs::Pose pose;
00229   const ompl::base::SE3StateSpace::StateType* se3_state = full_state->poses[idx];
00230   pose.position.x = se3_state->getX();
00231   pose.position.y = se3_state->getY();
00232   pose.position.z = se3_state->getZ();
00233   const ompl::base::SO3StateSpace::StateType& so3_state = se3_state->rotation();
00234   pose.orientation.x = so3_state.x;
00235   pose.orientation.y = so3_state.y;
00236   pose.orientation.z = so3_state.z;
00237   pose.orientation.w = so3_state.w;
00238 
00239   // run IK
00240   std::vector<double> solution(bijection_.size());
00241   moveit_msgs::MoveItErrorCodes err_code;
00242   if (!kinematics_solver_->getPositionIK(pose, seed_values, solution, err_code))
00243   {
00244     if (err_code.val != moveit_msgs::MoveItErrorCodes::TIMED_OUT ||
00245         !kinematics_solver_->searchPositionIK(pose, seed_values, kinematics_solver_->getDefaultTimeout() * 2.0,
00246                                               solution, err_code))
00247       return false;
00248   }
00249 
00250   for (std::size_t i = 0; i < bijection_.size(); ++i)
00251     full_state->values[bijection_[i]] = solution[i];
00252 
00253   return true;
00254 }
00255 
00256 bool ompl_interface::PoseModelStateSpace::computeStateFK(ompl::base::State* state) const
00257 {
00258   if (state->as<StateType>()->poseComputed())
00259     return true;
00260   for (std::size_t i = 0; i < poses_.size(); ++i)
00261     if (!poses_[i].computeStateFK(state->as<StateType>(), i))
00262     {
00263       state->as<StateType>()->markInvalid();
00264       return false;
00265     }
00266   state->as<StateType>()->setPoseComputed(true);
00267   return true;
00268 }
00269 
00270 bool ompl_interface::PoseModelStateSpace::computeStateIK(ompl::base::State* state) const
00271 {
00272   if (state->as<StateType>()->jointsComputed())
00273     return true;
00274   for (std::size_t i = 0; i < poses_.size(); ++i)
00275     if (!poses_[i].computeStateIK(state->as<StateType>(), i))
00276     {
00277       state->as<StateType>()->markInvalid();
00278       return false;
00279     }
00280   state->as<StateType>()->setJointsComputed(true);
00281   return true;
00282 }
00283 
00284 bool ompl_interface::PoseModelStateSpace::computeStateK(ompl::base::State* state) const
00285 {
00286   if (state->as<StateType>()->jointsComputed() && !state->as<StateType>()->poseComputed())
00287     return computeStateFK(state);
00288   if (!state->as<StateType>()->jointsComputed() && state->as<StateType>()->poseComputed())
00289     return computeStateIK(state);
00290   if (state->as<StateType>()->jointsComputed() && state->as<StateType>()->poseComputed())
00291     return true;
00292   state->as<StateType>()->markInvalid();
00293   return false;
00294 }
00295 
00296 ompl::base::StateSamplerPtr ompl_interface::PoseModelStateSpace::allocDefaultStateSampler() const
00297 {
00298   class PoseModelStateSampler : public ompl::base::StateSampler
00299   {
00300   public:
00301     PoseModelStateSampler(const ompl::base::StateSpace* space, const ompl::base::StateSamplerPtr& sampler)
00302       : ompl::base::StateSampler(space), sampler_(sampler)
00303     {
00304     }
00305 
00306     virtual void sampleUniform(ompl::base::State* state)
00307     {
00308       sampler_->sampleUniform(state);
00309       afterStateSample(state);
00310     }
00311 
00312     virtual void sampleUniformNear(ompl::base::State* state, const ompl::base::State* near, const double distance)
00313     {
00314       sampler_->sampleUniformNear(state, near, distance);
00315       afterStateSample(state);
00316     }
00317 
00318     virtual void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, const double stdDev)
00319     {
00320       sampler_->sampleGaussian(state, mean, stdDev);
00321       afterStateSample(state);
00322     }
00323 
00324   protected:
00325     void afterStateSample(ompl::base::State* sample) const
00326     {
00327       sample->as<StateType>()->setJointsComputed(true);
00328       sample->as<StateType>()->setPoseComputed(false);
00329       space_->as<PoseModelStateSpace>()->computeStateFK(sample);
00330     }
00331 
00332     ompl::base::StateSamplerPtr sampler_;
00333   };
00334 
00335   return ompl::base::StateSamplerPtr(static_cast<ompl::base::StateSampler*>(
00336       new PoseModelStateSampler(this, ModelBasedStateSpace::allocDefaultStateSampler())));
00337 }
00338 
00339 void ompl_interface::PoseModelStateSpace::copyToOMPLState(ompl::base::State* state,
00340                                                           const robot_state::RobotState& rstate) const
00341 {
00342   ModelBasedStateSpace::copyToOMPLState(state, rstate);
00343   state->as<StateType>()->setJointsComputed(true);
00344   state->as<StateType>()->setPoseComputed(false);
00345   computeStateFK(state);
00346   /*
00347   std::cout << "COPY STATE IN:\n";
00348   printState(state, std::cout);
00349   std::cout << "---------- COPY STATE IN\n"; */
00350 }


ompl
Author(s): Ioan Sucan
autogenerated on Fri Apr 20 2018 03:31:50