TSR.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONSTRAINT_DART_TSR_HPP_
#define AIKIDO_CONSTRAINT_DART_TSR_HPP_
 
#include <Eigen/Dense>
#include <dart/math/MathTypes.hpp>
 
#include "aikido/constraint/Differentiable.hpp"
#include "aikido/constraint/Projectable.hpp"
#include "aikido/constraint/Sampleable.hpp"
#include "aikido/constraint/Testable.hpp"
#include "aikido/statespace/SE3.hpp"
 
namespace aikido {
namespace constraint {
namespace dart {
 
AIKIDO_DECLARE_POINTERS(TSR)
 
class TSR
  : public Sampleable
  , public Differentiable
  , public Testable
  , public Projectable
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  using Differentiable::getValueAndJacobian;
 
  TSR(std::unique_ptr<common::RNG> _rng,
      const Eigen::Isometry3d& _T0_w = Eigen::Isometry3d::Identity(),
      const Eigen::Matrix<double, 6, 2>& _Bw
      = Eigen::Matrix<double, 6, 2>::Zero(),
      const Eigen::Isometry3d& _Tw_e = Eigen::Isometry3d::Identity(),
      double _testableTolerance = 1e-6);
 
  TSR(const Eigen::Isometry3d& _T0_w = Eigen::Isometry3d::Identity(),
      const Eigen::Matrix<double, 6, 2>& _Bw
      = Eigen::Matrix<double, 6, 2>::Zero(),
      const Eigen::Isometry3d& _Tw_e = Eigen::Isometry3d::Identity(),
      double _testableTolerance = 1e-6);
 
  TSR(const TSR& other);
  TSR(TSR&& other);
 
  TSR& operator=(const TSR& other);
  TSR& operator=(TSR&& other);
 
  virtual ~TSR() = default;
 
  // Documentation inherited.
  statespace::ConstStateSpacePtr getStateSpace() const override;
 
  std::shared_ptr<statespace::SE3> getSE3() const;
 
  // Documentation inherited.
  std::unique_ptr<SampleGenerator> createSampleGenerator() const override;
 
  // Documentation inherited.
  bool isSatisfied(
      const statespace::StateSpace::State* _s,
      TestableOutcome* outcome = nullptr) const override;
 
  std::unique_ptr<TestableOutcome> createOutcome() const override;
 
  void validate() const;
 
  void setRNG(std::unique_ptr<common::RNG> rng);
 
  // Documentation inherited.
  std::size_t getConstraintDimension() const override;
 
  // Documentation inherited.
  void getValue(const statespace::StateSpace::State* _s, Eigen::VectorXd& _out)
      const override;
 
  void getJacobian(
      const statespace::StateSpace::State* _s,
      Eigen::MatrixXd& _out) const override;
 
  // Documentation inherited.
  std::vector<ConstraintType> getConstraintTypes() const override;
 
  // Documentation inherited.
  bool project(
      const statespace::StateSpace::State* _s,
      statespace::StateSpace::State* _out) const override;
 
  double getTestableTolerance();
 
  void setTestableTolerance(double _testableTolerance);
 
  Eigen::Isometry3d mT0_w;
 
  Eigen::Matrix<double, 6, 2> mBw;
 
  Eigen::Isometry3d mTw_e;
 
private:
  double mTestableTolerance;
  std::unique_ptr<common::RNG> mRng;
  std::shared_ptr<statespace::SE3> mStateSpace;
};
 
} // namespace dart
} // namespace constraint
} // namespace aikido
 
#endif // AIKIDO_CONSTRAINT_DART_TSR_HPP_