util.hpp

Go to the documentation of this file.

#ifndef AIKIDO_ROBOT_UTIL_HPP_
#define AIKIDO_ROBOT_UTIL_HPP_
 
#include <dart/dart.hpp>
#include <dart/dynamics/dynamics.hpp>
 
#include "aikido/common/ExecutorThread.hpp"
#include "aikido/common/RNG.hpp"
#include "aikido/constraint/Testable.hpp"
#include "aikido/constraint/dart/CollisionFree.hpp"
#include "aikido/constraint/dart/TSR.hpp"
#include "aikido/control/TrajectoryExecutor.hpp"
#include "aikido/distance/ConfigurationRanker.hpp"
#include "aikido/io/yaml.hpp"
#include "aikido/statespace/dart/MetaSkeletonStateSpace.hpp"
#include "aikido/trajectory/Interpolated.hpp"
#include "aikido/trajectory/Spline.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace robot {
 
// TODO: These are mostly planning methods used in Robot classes.
// The planning methods will be removed once we have a Planner API.
namespace util {
 
struct VectorFieldPlannerParameters
{
  VectorFieldPlannerParameters() = default;
  VectorFieldPlannerParameters(
      double linearVelocity,
      double negativeDistanceTolerance,
      double positiveDistanceTolerance,
      double initialStepSize,
      double jointLimitTolerance,
      double constraintCheckResolution,
      double linearGain = 1.0,
      double angularGain = 0.2,
      double timestep = 0.1)
    : linearVelocity(linearVelocity)
    , negativeDistanceTolerance(negativeDistanceTolerance)
    , positiveDistanceTolerance(positiveDistanceTolerance)
    , initialStepSize(initialStepSize)
    , jointLimitTolerance(jointLimitTolerance)
    , constraintCheckResolution(constraintCheckResolution)
    , linearGain(linearGain)
    , angularGain(angularGain)
    , timestep(timestep){
          // Do nothing
      };
 
  double linearVelocity;
  double negativeDistanceTolerance;
  double positiveDistanceTolerance;
  double initialStepSize;
  double jointLimitTolerance;
  double constraintCheckResolution;
  double linearGain;
  double angularGain;
  double timestep;
};
 
struct CRRTPlannerParameters
{
  CRRTPlannerParameters(
      common::RNG* rng = nullptr,
      std::size_t maxNumTrials = 5,
      double maxExtensionDistance = std::numeric_limits<double>::infinity(),
      double maxDistanceBtwProjections = 0.1,
      double minStepSize = 0.05,
      double minTreeConnectionDistance = 0.1,
      std::size_t projectionMaxIteration = 20,
      double projectionTolerance = 1e-4)
    : rng(rng)
    , maxNumTrials(maxNumTrials)
    , maxExtensionDistance(maxExtensionDistance)
    , maxDistanceBtwProjections(maxDistanceBtwProjections)
    , minStepSize(minStepSize)
    , minTreeConnectionDistance(minTreeConnectionDistance)
    , projectionMaxIteration(projectionMaxIteration)
    , projectionTolerance(projectionTolerance)
  {
    // Do nothing
  }
 
  common::RNG* rng;
  std::size_t maxNumTrials;
  double maxExtensionDistance;
  double maxDistanceBtwProjections;
  double minStepSize;
  double minTreeConnectionDistance;
  std::size_t projectionMaxIteration;
  double projectionTolerance;
};
 
trajectory::TrajectoryPtr planToConfiguration(
    const statespace::dart::MetaSkeletonStateSpacePtr& space,
    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
    const statespace::StateSpace::State* goalState,
    const constraint::TestablePtr& collisionTestable,
    common::RNG* rng,
    double timelimit);
 
trajectory::TrajectoryPtr planToConfigurations(
    const statespace::dart::MetaSkeletonStateSpacePtr& space,
    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
    const std::vector<statespace::StateSpace::State*>& goalStates,
    const constraint::TestablePtr& collisionTestable,
    common::RNG* rng,
    double timelimit);
 
trajectory::TrajectoryPtr planToTSR(
    const statespace::dart::MetaSkeletonStateSpacePtr& space,
    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
    const dart::dynamics::BodyNodePtr& bodyNode,
    const constraint::dart::TSRPtr& tsr,
    const constraint::TestablePtr& collisionTestable,
    common::RNG* rng,
    double timelimit,
    std::size_t maxNumTrials,
    const distance::ConstConfigurationRankerPtr& ranker = nullptr);
 
trajectory::InterpolatedPtr planToTSRwithTrajectoryConstraint(
    const statespace::dart::MetaSkeletonStateSpacePtr& space,
    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
    const dart::dynamics::BodyNodePtr& bodyNode,
    const constraint::dart::TSRPtr& goalTsr,
    const constraint::dart::TSRPtr& constraintTsr,
    const constraint::TestablePtr& collisionTestable,
    double timelimit,
    const CRRTPlannerParameters& crrtParameters = CRRTPlannerParameters());
 
trajectory::TrajectoryPtr planToEndEffectorOffset(
    const statespace::dart::MetaSkeletonStateSpacePtr& space,
    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
    const dart::dynamics::BodyNodePtr& body,
    const Eigen::Vector3d& direction,
    const constraint::TestablePtr& collisionTestable,
    double distance,
    double timelimit,
    double positionTolerance = 1e-3,
    double angularTolerance = 1e-3,
    const VectorFieldPlannerParameters& vfParameters
    = VectorFieldPlannerParameters(),
    const CRRTPlannerParameters& crrtParameters = CRRTPlannerParameters());
 
trajectory::InterpolatedPtr planToEndEffectorOffsetByCRRT(
    const statespace::dart::MetaSkeletonStateSpacePtr& space,
    const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
    const dart::dynamics::BodyNodePtr& bodyNode,
    const constraint::TestablePtr& collisionTestable,
    const Eigen::Vector3d& direction,
    double distance,
    double timelimit,
    double positionTolerance = 1e-3,
    double angularTolerance = 1e-3,
    const CRRTPlannerParameters& crrtParameters = CRRTPlannerParameters());
 
std::unordered_map<std::string, const Eigen::VectorXd>
parseYAMLToNamedConfigurations(const YAML::Node& node);
 
bool getGoalAndConstraintTSRForEndEffectorOffset(
    const dart::dynamics::BodyNodePtr& bodyNode,
    const Eigen::Vector3d& direction,
    double distance,
    const constraint::dart::TSRPtr& goal,
    const constraint::dart::TSRPtr& constraint,
    double positionTolerance = 1e-3,
    double angularTolerance = 1e-3);
 
Eigen::Isometry3d getLookAtIsometry(
    const Eigen::Vector3d& positionFrom, const Eigen::Vector3d& positionTo);
 
const dart::dynamics::BodyNode* getBodyNodeOrThrow(
    const dart::dynamics::MetaSkeleton& skeleton,
    const std::string& bodyNodeName);
 
dart::dynamics::BodyNode* getBodyNodeOrThrow(
    dart::dynamics::MetaSkeleton& skeleton, const std::string& bodyNodeName);
 
} // namespace util
} // namespace robot
} // namespace aikido
 
#endif // AIKIDO_ROBOT_UTIL_HPP_