FramePairDifferentiable.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONSTRAINT_DART_FRAMEPAIRDIFFERENTIABLE_HPP_
#define AIKIDO_CONSTRAINT_DART_FRAMEPAIRDIFFERENTIABLE_HPP_
 
#include <Eigen/Dense>
#include <dart/dynamics/dynamics.hpp>
 
#include "aikido/constraint/Differentiable.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
namespace aikido {
namespace constraint {
namespace dart {
 
class FramePairDifferentiable : public Differentiable
{
public:
  FramePairDifferentiable(
      statespace::dart::MetaSkeletonStateSpacePtr _metaSkeletonStateSpace,
      ::dart::dynamics::MetaSkeletonPtr _metaskeleton,
      ::dart::dynamics::ConstJacobianNodePtr _jacobianNodeTarget,
      ::dart::dynamics::ConstJacobianNodePtr _jacobianNodeBase,
      DifferentiablePtr _relPoseConstraint);
 
  // Documentation inherited.
  std::size_t getConstraintDimension() const override;
 
  // Documentation inherited.
  void getValue(const statespace::StateSpace::State* _s, Eigen::VectorXd& _out)
      const override;
 
  // Documentation inherited.
  void getJacobian(
      const statespace::StateSpace::State* _s,
      Eigen::MatrixXd& _out) const override;
 
  // Documentation inherited.
  void getValueAndJacobian(
      const statespace::StateSpace::State* _s,
      Eigen::VectorXd& _val,
      Eigen::MatrixXd& _jac) const override;
 
  // Documentation inherited.
  std::vector<ConstraintType> getConstraintTypes() const override;
 
  // Documentation inherited.
  statespace::ConstStateSpacePtr getStateSpace() const override;
 
private:
  statespace::dart::ConstMetaSkeletonStateSpacePtr mMetaSkeletonStateSpace;
  ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
  ::dart::dynamics::ConstJacobianNodePtr mJacobianNode1;
  ::dart::dynamics::ConstJacobianNodePtr mJacobianNode2;
  DifferentiablePtr mRelPoseConstraint;
};
 
} // namespace dart
} // namespace constraint
} // namespace aikido
 
#endif // AIKIDO_CONSTRAINT_DART_FRAMEPAIRDIFFERENTIABLE_HPP_