convex-shape-contact.hh

Go to the documentation of this file.

// Copyright (c) 2014, LAAS-CNRS
// 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.
 
#ifndef HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
#define HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH
 
#include <hpp/constraints/config.hh>
#include <hpp/constraints/convex-shape.hh>
#include <hpp/constraints/deprecated.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/generic-transformation.hh>
#include <vector>
 
namespace hpp {
namespace constraints {
 
 
class HPP_CONSTRAINTS_DLLAPI ConvexShapeContact
    : public DifferentiableFunction {
 public:
  friend class ConvexShapeContactComplement;
  friend class ConvexShapeContactHold;
 
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  enum ContactType {
    POINT_ON_PLANE,
    LINE_ON_PLANE,
    PLANE_ON_PLANE
  };
 
  struct ForceData {
    JointPtr_t joint;
    JointPtr_t supportJoint;
    std::vector<vector3_t> points;
    vector3_t normal;
  };
 
  static ConvexShapeContactPtr_t create(const std::string& name,
                                        DevicePtr_t robot,
                                        const JointAndShapes_t& floorSurfaces,
                                        const JointAndShapes_t& objectSurfaces);
 
  static ConvexShapeContactPtr_t create(const DevicePtr_t& robot);
 
  const ConvexShapes_t& floorContactSurfaces() const {
    return floorConvexShapes_;
  }
  const ConvexShapes_t& objectContactSurfaces() const {
    return objectConvexShapes_;
  }
  value_type radius() const { return M_; }
  void setNormalMargin(const value_type& margin);
 
  std::vector<ForceData> computeContactPoints(
      ConfigurationIn_t q, const value_type& normalMargin) const;
 
  std::ostream& print(std::ostream& o) const;
 
  std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
      DeviceConstPtr_t /*robot*/) const {
    if ((floorConvexShapes_.size() == 0) || (objectConvexShapes_.size() == 0)) {
      return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
    }
    JointConstPtr_t floor0_joint = floorConvexShapes_[0].joint_;
    JointConstPtr_t object0_joint = objectConvexShapes_[0].joint_;
 
    size_type index1 = Joint::index(floor0_joint);
    size_type index2 = Joint::index(object0_joint);
    // check that all the joints involved are the same
    for (ConvexShapes_t::const_iterator it(floorConvexShapes_.begin());
         it != floorConvexShapes_.end(); ++it) {
      size_type jointIndex = Joint::index(it->joint_);
      if (jointIndex != index1) {
        return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
      }
    }
 
    for (ConvexShapes_t::const_iterator it(objectConvexShapes_.begin());
         it != objectConvexShapes_.end(); ++it) {
      size_type jointIndex = Joint::index(it->joint_);
      if (jointIndex != index2) {
        return std::pair<JointConstPtr_t, JointConstPtr_t>(nullptr, nullptr);
      }
    }
 
    if (index1 <= index2) {
      return std::pair<JointConstPtr_t, JointConstPtr_t>(floor0_joint,
                                                         object0_joint);
    } else {
      return std::pair<JointConstPtr_t, JointConstPtr_t>(object0_joint,
                                                         floor0_joint);
    }
  };
 
 protected:
  ConvexShapeContact(const std::string& name, DevicePtr_t robot,
                     const JointAndShapes_t& floorSurfaces,
                     const JointAndShapes_t& objectSurfaces);
 
  bool isEqual(const DifferentiableFunction& other) const;
 
 private:
  void addObject(const ConvexShape& t);
 
  void addFloor(const ConvexShape& t);
  void computeRadius();
 
  void impl_compute(LiegroupElementRef result,
                    ConfigurationIn_t argument) const;
  void computeInternalValue(const ConfigurationIn_t& argument, bool& isInside,
                            ContactType& type, vector6_t& value,
                            std::size_t& iobject, std::size_t& ifloor) const;
 
  void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t argument) const;
  void computeInternalJacobian(const ConfigurationIn_t& argument,
                               bool& isInside, ContactType& type,
                               matrix_t& jacobian) const;
 
  bool selectConvexShapes(const pinocchio::DeviceData& data,
                          std::size_t& iobject, std::size_t& ifloor) const;
  ContactType contactType(const ConvexShape& object,
                          const ConvexShape& floor) const;
 
  DevicePtr_t robot_;
  mutable GenericTransformationModel<true> relativeTransformationModel_;
 
  ConvexShapes_t objectConvexShapes_;
  ConvexShapes_t floorConvexShapes_;
 
  value_type normalMargin_;
  // upper bound of distance between center of polygon and vectices for
  // all floor polygons.
  value_type M_;
};
 
class HPP_CONSTRAINTS_DLLAPI ConvexShapeContactComplement
    : public DifferentiableFunction {
 public:
  friend class ConvexShapeContactHold;
  static std::pair<ConvexShapeContactPtr_t, ConvexShapeContactComplementPtr_t>
  createPair(const std::string& name, DevicePtr_t robot,
             const JointAndShapes_t& floorSurfaces,
             const JointAndShapes_t& objectSurfaces);
 
  void computeRelativePoseRightHandSide(
      LiegroupElementConstRef rhs, std::size_t& ifloor, std::size_t& iobject,
      LiegroupElementRef relativePoseRhs) const;
 
  std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
      DeviceConstPtr_t /*robot*/) const {
    return sibling_->dependsOnRelPoseBetween(nullptr);
  };
 
 protected:
  ConvexShapeContactComplement(const std::string& name, DevicePtr_t robot,
                               const JointAndShapes_t& floorSurfaces,
                               const JointAndShapes_t& objectSurfaces);
 
  bool isEqual(const DifferentiableFunction& other) const;
 
 private:
  void impl_compute(LiegroupElementRef result,
                    ConfigurationIn_t argument) const;
 
  void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t argument) const;
 
  ConvexShapeContactPtr_t sibling_;
};  // class ConvexShapeContactComplement
 
class HPP_CONSTRAINTS_DLLAPI ConvexShapeContactHold
    : public DifferentiableFunction {
 public:
  static ConvexShapeContactHoldPtr_t create(
      const std::string& name, DevicePtr_t robot,
      const JointAndShapes_t& floorSurfaces,
      const JointAndShapes_t& objectSurfaces);
 
  ConvexShapeContactPtr_t contactConstraint() const { return constraint_; }
  ConvexShapeContactComplementPtr_t complement() const { return complement_; }
 
  std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
      DeviceConstPtr_t /*robot*/) const {
    return constraint_->dependsOnRelPoseBetween(nullptr);
  }
 
 protected:
  ConvexShapeContactHold(const std::string& name, DevicePtr_t robot,
                         const JointAndShapes_t& floorSurfaces,
                         const JointAndShapes_t& objectSurfaces);
 
  virtual void impl_compute(LiegroupElementRef result,
                            vectorIn_t argument) const;
  virtual void impl_jacobian(matrixOut_t jacobian, vectorIn_t arg) const;
 
  bool isEqual(const DifferentiableFunction& other) const;
 
 private:
  ConvexShapeContactPtr_t constraint_;
  ConvexShapeContactComplementPtr_t complement_;
};  // class ConvexShapeContactHold
}  // namespace constraints
}  // namespace hpp
 
#endif  //  HPP_CONSTRAINTS_CONVEX_SHAPE_CONTACT_HH