BodyNodePoseVectorField.hpp

Go to the documentation of this file.

#ifndef AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_
#define AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_
 
#include <dart/dynamics/BodyNode.hpp>
 
#include "aikido/planner/vectorfield/VectorField.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
 
namespace aikido {
namespace planner {
namespace vectorfield {
 
AIKIDO_DECLARE_POINTERS(BodyNodePoseVectorField)
 
class BodyNodePoseVectorField : public VectorField
{
public:
  BodyNodePoseVectorField(
      aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr
          metaSkeletonStateSpace,
      ::dart::dynamics::MetaSkeletonPtr metaSkeleton,
      ::dart::dynamics::ConstBodyNodePtr bodyNode,
      double maxStepSize,
      double jointLimitPadding,
      bool enforceJointVelocityLimits = false);
 
  // Documentation inherited.
  bool evaluateVelocity(
      const aikido::statespace::StateSpace::State* state,
      Eigen::VectorXd& qd) const override;
 
  // Documentation inherited.
  VectorFieldPlannerStatus evaluateStatus(
      const aikido::statespace::StateSpace::State* state) const override;
 
  virtual bool evaluateCartesianVelocity(
      const Eigen::Isometry3d& pose,
      Eigen::Vector6d& cartesianVelocity) const = 0;
 
  virtual VectorFieldPlannerStatus evaluateCartesianStatus(
      const Eigen::Isometry3d& pose) const = 0;
 
  // Documentation inherited.
  bool evaluateTrajectory(
      const aikido::trajectory::Trajectory& trajectory,
      const aikido::constraint::Testable* constraint,
      double evalStepSize,
      double& evalTimePivot,
      bool includeEndTime) const override;
 
  aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr
  getMetaSkeletonStateSpace() const;
 
  ::dart::dynamics::MetaSkeletonPtr getMetaSkeleton();
 
  ::dart::dynamics::ConstMetaSkeletonPtr getMetaSkeleton() const;
 
  ::dart::dynamics::ConstBodyNodePtr getBodyNode() const;
 
protected:
  aikido::statespace::dart::ConstMetaSkeletonStateSpacePtr
      mMetaSkeletonStateSpace;
 
  ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
 
  ::dart::dynamics::ConstBodyNodePtr mBodyNode;
 
  double mMaxStepSize;
 
  double mJointLimitPadding;
 
  Eigen::VectorXd mVelocityLowerLimits;
 
  Eigen::VectorXd mVelocityUpperLimits;
 
  bool mEnforceJointVelocityLimits;
};
 
} // namespace vectorfield
} // namespace planner
} // namespace aikido
 
#endif // AIKIDO_PLANNER_VECTORFIELD_BODYNODEPOSEVECTORFIELD_HPP_