helper.cc

// Copyright (c) 2016, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 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
// HOLDER 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.

#include <array>
#include <boost/regex.hpp>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/manipulation/graph/edge.hh>
#include <hpp/manipulation/graph/guided-state-selector.hh>
#include <hpp/manipulation/graph/helper.hh>
#include <hpp/manipulation/graph/state-selector.hh>
#include <hpp/manipulation/graph/state.hh>
#include <hpp/manipulation/handle.hh>
#include <hpp/manipulation/problem-solver.hh>
#include <hpp/pinocchio/gripper.hh>
#include <hpp/util/debug.hh>
#include <iterator>
#include <pinocchio/multibody/model.hpp>
#include <unordered_map>
#include <unordered_set>

#define CASE_TO_STRING(var, value) \
  ((var & value) ? std::string(#value) : std::string())

namespace hpp {
namespace manipulation {
namespace graph {
namespace helper {
typedef constraints::Implicit Implicit;
typedef constraints::ImplicitPtr_t ImplicitPtr_t;

template <bool forPath>
void addToComp(const NumericalConstraints_t& nc, GraphComponentPtr_t comp) {
  if (nc.empty()) return;
  StatePtr_t n;
  if (forPath) {
    n = HPP_DYNAMIC_PTR_CAST(State, comp);
    if (!n) throw std::logic_error("Wrong type: expect a State");
  }
  for (const auto& c : nc)
    if (c) {
      if (forPath)
        n->addNumericalConstraintForPath(c);
      else
        comp->addNumericalConstraint(c);
    }
}

template <bool param>
void specifyFoliation(const NumericalConstraints_t& nc, LevelSetEdgePtr_t lse) {
  for (const auto& c : nc)
    if (c) {
      if (param)
        lse->insertParamConstraint(c);
      else
        lse->insertConditionConstraint(c);
    }
}

void FoliatedManifold::addToState(StatePtr_t comp) const {
  addToComp<false>(nc, comp);
  addToComp<false>(nc_path, comp);
}

void FoliatedManifold::addToEdge(EdgePtr_t comp) const {
  addToComp<false>(nc_fol, comp);
}

void FoliatedManifold::specifyFoliation(LevelSetEdgePtr_t lse) const {
  for (const auto& c : nc) lse->insertConditionConstraint(c);
  for (const auto& c : nc_fol) lse->insertConditionConstraint(c);
}

namespace {
template <int gCase>
struct CaseTraits {
  static const bool pregrasp = (gCase & WithPreGrasp);
  static const bool preplace = (gCase & WithPrePlace);
  static const bool intersec = !((gCase & NoGrasp) || (gCase & NoPlace));

  static const bool valid =
      ((gCase & WithPreGrasp) || (gCase & GraspOnly) || (gCase & NoGrasp)) &&
      ((gCase & WithPrePlace) || (gCase & PlaceOnly) || (gCase & NoPlace)) &&
      !((gCase & NoGrasp) && (gCase & NoPlace));

  static const std::size_t nbWaypoints =
      (pregrasp ? 1 : 0) + (intersec ? 1 : 0) + (preplace ? 1 : 0);
  static const std::size_t Nstates = 2 + nbWaypoints;
  static const std::size_t Nedges = 1 + nbWaypoints;
  // static const std::size_t iNpregrasp = pregrasp?1 + 1:nbWaypoints;
  // static const std::size_t iNpreplace = pregrasp?1 + 1:nbWaypoints;
  typedef std::array<StatePtr_t, Nstates> StateArray;
  typedef std::array<EdgePtr_t, Nedges> EdgeArray;

  static inline const StatePtr_t& Npregrasp(const StateArray& n) {
    assert(pregrasp);
    return n[1];
  }
  static inline const StatePtr_t& Nintersec(const StateArray& n) {
    assert(intersec);
    return n[1 + (pregrasp ? 1 : 0)];
  }
  static inline const StatePtr_t& Npreplace(const StateArray& n) {
    assert(preplace);
    return n[1 + (pregrasp ? 1 : 0) + (intersec ? 1 : 0)];
  }

  static inline std::string caseToString() {
    return CASE_TO_STRING(gCase, NoGrasp) + CASE_TO_STRING(gCase, GraspOnly) +
           CASE_TO_STRING(gCase, WithPreGrasp) + " - " +
           CASE_TO_STRING(gCase, NoPlace) + CASE_TO_STRING(gCase, PlaceOnly) +
           CASE_TO_STRING(gCase, WithPrePlace);
  }

  static inline EdgePtr_t makeWE(const std::string& name,
                                 const StatePtr_t& from, const StatePtr_t& to,
                                 const size_type& w) {
    if (Nedges > 1) {
      WaypointEdgePtr_t we = static_pointer_cast<WaypointEdge>(
          from->linkTo(name, to, w, WaypointEdge::create));
      we->nbWaypoints(nbWaypoints);
      return we;
    } else
      return from->linkTo(name, to, w, Edge::create);
  }

  static inline StateArray makeWaypoints(const StatePtr_t& from,
                                         const StatePtr_t& to,
                                         const std::string& name) {
    StateSelectorPtr_t ns = from->parentGraph()->stateSelector();
    StateArray states;
    std::size_t r = 0;
    states[r] = from;
    ++r;
    if (pregrasp) {
      states[r] = ns->createState(name + "_pregrasp", true);
      ++r;
    }
    if (intersec) {
      states[r] = ns->createState(name + "_intersec", true);
      ++r;
    }
    if (preplace) {
      states[r] = ns->createState(name + "_preplace", true);
      ++r;
    }
    states[r] = to;
    return states;
  }

  static inline EdgePtr_t makeLSEgrasp(const std::string& name,
                                       const StateArray& n, const EdgeArray& e,
                                       const size_type w,
                                       LevelSetEdgePtr_t& gls) {
    if (Nedges > 1) {
      const std::size_t T = (pregrasp ? 1 : 0) + (intersec ? 1 : 0);
      WaypointEdgePtr_t we = static_pointer_cast<WaypointEdge>(
          n.front()->linkTo(name + "_ls", n.back(), w, WaypointEdge::create));
      we->nbWaypoints(nbWaypoints);
      gls = linkWaypoint<LevelSetEdge>(n, T - 1, T, name, "ls");
      for (std::size_t i = 0; i < Nedges; ++i)
        we->setWaypoint(i, e[i], n[i + 1]);
      we->setWaypoint(T - 1, gls, n[T]);
      gls->state(n.front());
      gls->setShort(pregrasp);
      return we;
    } else {
      assert(gCase == (GraspOnly | NoPlace) &&
             "Cannot implement a LevelSetEdge for grasping");
      gls = static_pointer_cast<LevelSetEdge>(
          n.front()->linkTo(name + "_ls", n.back(), w, LevelSetEdge::create));
      return gls;
    }
  }