pose_model_state_space.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#include <moveit/ompl_interface/parameterization/work_space/pose_model_state_space.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <moveit/profiler/profiler.h>

const std::string ompl_interface::PoseModelStateSpace::PARAMETERIZATION_TYPE = "PoseModel";

ompl_interface::PoseModelStateSpace::PoseModelStateSpace(const ModelBasedStateSpaceSpecification& spec)
  : ModelBasedStateSpace(spec)
{
  jump_factor_ = 3;  // \todo make this a param

  if (spec.joint_model_group_->getGroupKinematics().first)
    poses_.push_back(PoseComponent(spec.joint_model_group_, spec.joint_model_group_->getGroupKinematics().first));
  else if (!spec.joint_model_group_->getGroupKinematics().second.empty())
  {
    const robot_model::JointModelGroup::KinematicsSolverMap& m = spec.joint_model_group_->getGroupKinematics().second;
    for (robot_model::JointModelGroup::KinematicsSolverMap::const_iterator it = m.begin(); it != m.end(); ++it)
      poses_.push_back(PoseComponent(it->first, it->second));
  }
  if (poses_.empty())
    logError("No kinematics solvers specified. Unable to construct a PoseModelStateSpace");
  else
    std::sort(poses_.begin(), poses_.end());
  setName(getName() + "_" + PARAMETERIZATION_TYPE);
}

ompl_interface::PoseModelStateSpace::~PoseModelStateSpace()
{
}

double ompl_interface::PoseModelStateSpace::distance(const ompl::base::State* state1,
                                                     const ompl::base::State* state2) const
{
  double total = 0;
  for (std::size_t i = 0; i < poses_.size(); ++i)
    total += poses_[i].state_space_->distance(state1->as<StateType>()->poses[i], state2->as<StateType>()->poses[i]);
  return total;
}

double ompl_interface::PoseModelStateSpace::getMaximumExtent() const
{
  double total = 0.0;
  for (std::size_t i = 0; i < poses_.size(); ++i)
    total += poses_[i].state_space_->getMaximumExtent();
  return total;
}

ompl::base::State* ompl_interface::PoseModelStateSpace::allocState() const
{
  StateType* state = new StateType();
  state->values =
      new double[variable_count_];  // need to allocate this here since ModelBasedStateSpace::allocState() is not called
  state->poses = new ompl::base::SE3StateSpace::StateType*[poses_.size()];
  for (std::size_t i = 0; i < poses_.size(); ++i)
    state->poses[i] = poses_[i].state_space_->allocState()->as<ompl::base::SE3StateSpace::StateType>();
  return state;
}

void ompl_interface::PoseModelStateSpace::freeState(ompl::base::State* state) const
{
  for (std::size_t i = 0; i < poses_.size(); ++i)
    poses_[i].state_space_->freeState(state->as<StateType>()->poses[i]);
  delete[] state->as<StateType>()->poses;
  ModelBasedStateSpace::freeState(state);
}

void ompl_interface::PoseModelStateSpace::copyState(ompl::base::State* destination,
                                                    const ompl::base::State* source) const
{
  // copy the state data
  ModelBasedStateSpace::copyState(destination, source);

  for (std::size_t i = 0; i < poses_.size(); ++i)
    poses_[i].state_space_->copyState(destination->as<StateType>()->poses[i], source->as<StateType>()->poses[i]);

  // compute additional stuff if needed
  computeStateK(destination);
}

void ompl_interface::PoseModelStateSpace::sanityChecks() const
{
  ModelBasedStateSpace::sanityChecks(std::numeric_limits<double>::epsilon(), std::numeric_limits<float>::epsilon(),
                                     ~ompl::base::StateSpace::STATESPACE_TRIANGLE_INEQUALITY);
}

void ompl_interface::PoseModelStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to,
                                                      const double t, ompl::base::State* state) const
{
  //  moveit::Profiler::ScopedBlock sblock("interpolate");

  // we want to interpolate in Cartesian space; we do not have a guarantee that from and to
  // have their poses computed, but this is very unlikely to happen (depends how the planner gets its input states)

  // interpolate in joint space
  ModelBasedStateSpace::interpolate(from, to, t, state);

  // interpolate SE3 components
  for (std::size_t i = 0; i < poses_.size(); ++i)
    poses_[i].state_space_->interpolate(from->as<StateType>()->poses[i], to->as<StateType>()->poses[i], t,
                                        state->as<StateType>()->poses[i]);

  // the call above may reset all flags for state; but we know the pose we want flag should be set
  state->as<StateType>()->setPoseComputed(true);

  /*
  std::cout << "*********** interpolate\n";
  printState(from, std::cout);
  printState(to, std::cout);
  printState(state, std::cout);
  std::cout << "\n\n";
  */

  // after interpolation we cannot be sure about the joint values (we use them as seed only)
  // so we recompute IK if needed
  if (computeStateIK(state))
  {
    double dj = jump_factor_ * ModelBasedStateSpace::distance(from, to);
    double d_from = ModelBasedStateSpace::distance(from, state);
    double d_to = ModelBasedStateSpace::distance(state, to);

    // if the joint value jumped too much
    if (d_from + d_to > std::max(0.2, dj))  // \todo make 0.2 a param
      state->as<StateType>()->markInvalid();
  }
}

void ompl_interface::PoseModelStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY,
                                                            double minZ, double maxZ)
{
  ModelBasedStateSpace::setPlanningVolume(minX, maxX, minY, maxY, minZ, maxZ);
  ompl::base::RealVectorBounds b(3);
  b.low[0] = minX;
  b.low[1] = minY;
  b.low[2] = minZ;
  b.high[0] = maxX;
  b.high[1] = maxY;
  b.high[2] = maxZ;
  for (std::size_t i = 0; i < poses_.size(); ++i)
    poses_[i].state_space_->as<ompl::base::SE3StateSpace>()->setBounds(b);
}

ompl_interface::PoseModelStateSpace::PoseComponent::PoseComponent(
    const robot_model::JointModelGroup* subgroup, const robot_model::JointModelGroup::KinematicsSolver& k)
  : subgroup_(subgroup), kinematics_solver_(k.allocator_(subgroup)), bijection_(k.bijection_)
{
  state_space_.reset(new ompl::base::SE3StateSpace());
  state_space_->setName(subgroup_->getName() + "_Workspace");
  fk_link_.resize(1, kinematics_solver_->getTipFrame());
  if (!fk_link_[0].empty() && fk_link_[0][0] == '/')
    fk_link_[0] = fk_link_[0].substr(1);
}

bool ompl_interface::PoseModelStateSpace::PoseComponent::computeStateFK(StateType* full_state, unsigned int idx) const
{
  // read the values from the joint state, in the order expected by the kinematics solver
  std::vector<double> values(bijection_.size());
  for (unsigned int i = 0; i < bijection_.size(); ++i)
    values[i] = full_state->values[bijection_[i]];

  // compute forward kinematics for the link of interest
  std::vector<geometry_msgs::Pose> poses;
  if (!kinematics_solver_->getPositionFK(fk_link_, values, poses))
    return false;

  // copy the resulting data to the desired location in the state
  ompl::base::SE3StateSpace::StateType* se3_state = full_state->poses[idx];
  se3_state->setXYZ(poses[0].position.x, poses[0].position.y, poses[0].position.z);
  ompl::base::SO3StateSpace::StateType& so3_state = se3_state->rotation();
  so3_state.x = poses[0].orientation.x;
  so3_state.y = poses[0].orientation.y;
  so3_state.z = poses[0].orientation.z;
  so3_state.w = poses[0].orientation.w;

  return true;
}

bool ompl_interface::PoseModelStateSpace::PoseComponent::computeStateIK(StateType* full_state, unsigned int idx) const
{
  // read the values from the joint state, in the order expected by the kinematics solver; use these as the seed
  std::vector<double> seed_values(bijection_.size());
  for (std::size_t i = 0; i < bijection_.size(); ++i)
    seed_values[i] = full_state->values[bijection_[i]];

  /*
  std::cout << "seed: ";
  for (std::size_t i = 0 ; i < seed_values.size() ; ++i)
    std::cout << seed_values[i] << " ";
  std::cout << std::endl;
  */

  // construct the pose
  geometry_msgs::Pose pose;
  const ompl::base::SE3StateSpace::StateType* se3_state = full_state->poses[idx];
  pose.position.x = se3_state->getX();
  pose.position.y = se3_state->getY();
  pose.position.z = se3_state->getZ();
  const ompl::base::SO3StateSpace::StateType& so3_state = se3_state->rotation();
  pose.orientation.x = so3_state.x;
  pose.orientation.y = so3_state.y;
  pose.orientation.z = so3_state.z;
  pose.orientation.w = so3_state.w;

  // run IK
  std::vector<double> solution(bijection_.size());
  moveit_msgs::MoveItErrorCodes err_code;
  if (!kinematics_solver_->getPositionIK(pose, seed_values, solution, err_code))
  {
    if (err_code.val != moveit_msgs::MoveItErrorCodes::TIMED_OUT ||
        !kinematics_solver_->searchPositionIK(pose, seed_values, kinematics_solver_->getDefaultTimeout() * 2.0,
                                              solution, err_code))
      return false;
  }

  for (std::size_t i = 0; i < bijection_.size(); ++i)
    full_state->values[bijection_[i]] = solution[i];

  return true;
}

bool ompl_interface::PoseModelStateSpace::computeStateFK(ompl::base::State* state) const
{
  if (state->as<StateType>()->poseComputed())
    return true;
  for (std::size_t i = 0; i < poses_.size(); ++i)
    if (!poses_[i].computeStateFK(state->as<StateType>(), i))
    {
      state->as<StateType>()->markInvalid();
      return false;
    }
  state->as<StateType>()->setPoseComputed(true);
  return true;
}

bool ompl_interface::PoseModelStateSpace::computeStateIK(ompl::base::State* state) const
{
  if (state->as<StateType>()->jointsComputed())
    return true;
  for (std::size_t i = 0; i < poses_.size(); ++i)
    if (!poses_[i].computeStateIK(state->as<StateType>(), i))
    {
      state->as<StateType>()->markInvalid();
      return false;
    }
  state->as<StateType>()->setJointsComputed(true);
  return true;
}

bool ompl_interface::PoseModelStateSpace::computeStateK(ompl::base::State* state) const
{
  if (state->as<StateType>()->jointsComputed() && !state->as<StateType>()->poseComputed())
    return computeStateFK(state);
  if (!state->as<StateType>()->jointsComputed() && state->as<StateType>()->poseComputed())
    return computeStateIK(state);
  if (state->as<StateType>()->jointsComputed() && state->as<StateType>()->poseComputed())
    return true;
  state->as<StateType>()->markInvalid();
  return false;
}

ompl::base::StateSamplerPtr ompl_interface::PoseModelStateSpace::allocDefaultStateSampler() const
{
  class PoseModelStateSampler : public ompl::base::StateSampler
  {
  public:
    PoseModelStateSampler(const ompl::base::StateSpace* space, const ompl::base::StateSamplerPtr& sampler)
      : ompl::base::StateSampler(space), sampler_(sampler)
    {
    }

    virtual void sampleUniform(ompl::base::State* state)
    {
      sampler_->sampleUniform(state);
      afterStateSample(state);
    }

    virtual void sampleUniformNear(ompl::base::State* state, const ompl::base::State* near, const double distance)
    {
      sampler_->sampleUniformNear(state, near, distance);
      afterStateSample(state);
    }

    virtual void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, const double stdDev)
    {
      sampler_->sampleGaussian(state, mean, stdDev);
      afterStateSample(state);
    }

  protected:
    void afterStateSample(ompl::base::State* sample) const
    {
      sample->as<StateType>()->setJointsComputed(true);
      sample->as<StateType>()->setPoseComputed(false);
      space_->as<PoseModelStateSpace>()->computeStateFK(sample);
    }

    ompl::base::StateSamplerPtr sampler_;
  };

  return ompl::base::StateSamplerPtr(static_cast<ompl::base::StateSampler*>(
      new PoseModelStateSampler(this, ModelBasedStateSpace::allocDefaultStateSampler())));
}

void ompl_interface::PoseModelStateSpace::copyToOMPLState(ompl::base::State* state,
                                                          const robot_state::RobotState& rstate) const
{
  ModelBasedStateSpace::copyToOMPLState(state, rstate);
  state->as<StateType>()->setJointsComputed(true);
  state->as<StateType>()->setPoseComputed(false);
  computeStateFK(state);
  /*
  std::cout << "COPY STATE IN:\n";
  printState(state, std::cout);
  std::cout << "---------- COPY STATE IN\n"; */
}