generic-transformation.hh

Go to the documentation of this file.

//
// Copyright (c) 2016 CNRS
// Authors: Joseph Mirabel
//
//
 
// 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_GENERIC_TRANSFORMATION_HH
#define HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH
 
#include <hpp/constraints/config.hh>
#include <hpp/constraints/differentiable-function.hh>
#include <hpp/constraints/fwd.hh>
#include <hpp/constraints/matrix-view.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/util/serialization-fwd.hh>
#include <pinocchio/spatial/se3.hpp>
 
namespace hpp {
namespace constraints {
template <bool rel>
struct GenericTransformationModel {
  JointConstPtr_t joint2;
  bool R1isID, R2isID, t1isZero, t2isZero;
  Transform3s F1inJ1, F2inJ2;
  bool fullPos, fullOri;
  size_type rowOri;
  const size_type cols;
 
  inline JointConstPtr_t getJoint1() const { return JointConstPtr_t(); }
  inline void setJoint1(const JointConstPtr_t&) {}
  GenericTransformationModel(const size_type nCols)
      : joint2(),
        R1isID(true),
        R2isID(true),
        t1isZero(true),
        t2isZero(true),
        fullPos(false),
        fullOri(false),
        cols(nCols) {
    F1inJ1.setIdentity();
    F2inJ2.setIdentity();
  }
  void checkIsIdentity1() {
    R1isID = F1inJ1.rotation().isIdentity();
    t1isZero = F1inJ1.translation().isZero();
  }
  void checkIsIdentity2() {
    R2isID = F2inJ2.rotation().isIdentity();
    t2isZero = F2inJ2.translation().isZero();
  }
};
template <>
struct GenericTransformationModel<true> : GenericTransformationModel<false> {
  JointConstPtr_t joint1;
  inline JointConstPtr_t getJoint1() const { return joint1; }
  void setJoint1(const JointConstPtr_t& j);
  GenericTransformationModel(const size_type nCols)
      : GenericTransformationModel<false>(nCols), joint1() {}
};


template <int _Options>
class HPP_CONSTRAINTS_DLLAPI GenericTransformation
    : public DifferentiableFunction {
 public:
  typedef shared_ptr<GenericTransformation> Ptr_t;
  typedef weak_ptr<GenericTransformation> WkPtr_t;
#if __cplusplus >= 201103L
  static constexpr bool IsRelative = _Options & RelativeBit,
                        ComputeOrientation = _Options & OrientationBit,
                        ComputePosition = _Options & PositionBit,
                        OutputR3xSO3 = _Options & OutputR3xSO3Bit,
                        IsPosition = ComputePosition && !ComputeOrientation,
                        IsOrientation = !ComputePosition && ComputeOrientation,
                        IsTransform = ComputePosition && ComputeOrientation;
  static constexpr int ValueSize =
                           (ComputePosition ? 3 : 0) +
                           (ComputeOrientation ? (OutputR3xSO3 ? 4 : 3) : 0),
                       DerSize = (ComputePosition ? 3 : 0) +
                                 (ComputeOrientation ? 3 : 0);
#else
  enum {
    IsRelative = _Options & RelativeBit,
    ComputeOrientation = _Options & OrientationBit,
    ComputePosition = _Options & PositionBit,
    OutputR3xSO3 = _Options & OutputR3xSO3Bit,
    IsPosition = ComputePosition && !ComputeOrientation,
    IsOrientation = !ComputePosition && ComputeOrientation,
    IsTransform = ComputePosition && ComputeOrientation,
    ValueSize = (ComputePosition ? 3 : 0) +
                (ComputeOrientation ? (OutputR3xSO3 ? 4 : 3) : 0),
    DerSize = (ComputePosition ? 3 : 0) + (ComputeOrientation ? 3 : 0)
  };
#endif

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
                      const JointConstPtr_t& joint2,
                      const Transform3s& reference,
                      std::vector<bool> mask = std::vector<bool>(DerSize,
                                                                 true));

  static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
                      /* World frame          */ const JointConstPtr_t& joint2,
                      const Transform3s& frame2, const Transform3s& frame1,
                      std::vector<bool> mask = std::vector<bool>(DerSize,
                                                                 true));

  static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
                      const JointConstPtr_t& joint1,
                      const JointConstPtr_t& joint2,
                      const Transform3s& reference,
                      std::vector<bool> mask = std::vector<bool>(DerSize,
                                                                 true));

  static Ptr_t create(const std::string& name, const DevicePtr_t& robot,
                      const JointConstPtr_t& joint1,
                      const JointConstPtr_t& joint2, const Transform3s& frame1,
                      const Transform3s& frame2,
                      std::vector<bool> mask = std::vector<bool>(DerSize,
                                                                 true));
 
  virtual ~GenericTransformation() {}

  inline void reference(const Transform3s& reference) {
    m_.F1inJ1 = reference;
    m_.checkIsIdentity1();
    m_.F2inJ2.setIdentity();
    m_.checkIsIdentity2();
  }

  inline Transform3s reference() const { return m_.F1inJ1.actInv(m_.F2inJ2); }

  inline void joint1(const JointConstPtr_t& joint) {
    // static_assert(IsRelative);
    m_.setJoint1(joint);
    computeActiveParams();
    assert(!joint || joint->robot() == robot_);
  }

  inline JointConstPtr_t joint1() const { return m_.getJoint1(); }

  inline void joint2(const JointConstPtr_t& joint) {
    m_.joint2 = joint;
    computeActiveParams();
    assert(!joint || (joint->index() > 0 && joint->robot() == robot_));
  }

  inline JointConstPtr_t joint2() const { return m_.joint2; }

  inline void frame1InJoint1(const Transform3s& M) {
    m_.F1inJ1 = M;
    m_.checkIsIdentity1();
  }

  inline const Transform3s& frame1InJoint1() const { return m_.F1inJ1; }
  inline void frame2InJoint2(const Transform3s& M) {
    m_.F2inJ2 = M;
    m_.checkIsIdentity2();
  }

  inline const Transform3s& frame2InJoint2() const { return m_.F2inJ2; }

  std::pair<JointConstPtr_t, JointConstPtr_t> dependsOnRelPoseBetween(
      DeviceConstPtr_t /*robot*/) const {
    JointConstPtr_t j1 = joint1();
    JointConstPtr_t j2 = joint2();
    size_type index1 = Joint::index(j1);
    size_type index2 = Joint::index(j2);
    if (index1 <= index2) {
      return std::pair<JointConstPtr_t, JointConstPtr_t>(j1, j2);
    } else {
      return std::pair<JointConstPtr_t, JointConstPtr_t>(j2, j1);
    }
  };
 
  virtual std::ostream& print(std::ostream& o) const;

  GenericTransformation(const std::string& name, const DevicePtr_t& robot,
                        std::vector<bool> mask);
 
 protected:
  void init(const WkPtr_t& self) {
    self_ = self;
    computeActiveParams();
  }

  virtual void impl_compute(LiegroupElementRef result,
                            ConfigurationIn_t argument) const;
  virtual void impl_jacobian(matrixOut_t jacobian, ConfigurationIn_t arg) const;
 
  bool isEqual(const DifferentiableFunction& other) const {
    const GenericTransformation& castother =
        dynamic_cast<const GenericTransformation&>(other);
    if (!DifferentiableFunction::isEqual(other)) return false;
    if (robot_ != castother.robot_) return false;
    if (mask_ != castother.mask_) return false;
 
    return true;
  }
 
 private:
  void computeActiveParams();
  DevicePtr_t robot_;
  GenericTransformationModel<IsRelative> m_;
  Eigen::RowBlockIndices Vindices_;
  const std::vector<bool> mask_;
  WkPtr_t self_;
 
  GenericTransformation() : m_(0) {}
  HPP_SERIALIZABLE();
};  // class GenericTransformation

}  // namespace constraints
}  // namespace hpp
#endif  // HPP_CONSTRAINTS_GENERIC_TRANSFORMATION_HH