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 {
 
const std::vector<double> defaultVFParams{
    0.03,  // distanceTolerance (offset-only)
    0.004, // positionTolerance
    0.01,  // angularTolerance
    0.01,  // initialStepSize
    1e-3,  // jointlimitTolerance
    1e-3,  // constraintCheckResolution
    1.5,   // timeout (s)
    0.01,  // goalTolerance (twist-only)
    1.0    // angle-to-distance ratio (twist-only)
};
 
struct VectorFieldPlannerParameters
{
  VectorFieldPlannerParameters(
      double distanceTolerance = defaultVFParams[0],
      double positionTolerance = defaultVFParams[1],
      double angularTolerance = defaultVFParams[2],
      double initialStepSize = defaultVFParams[3],
      double jointLimitTolerance = defaultVFParams[4],
      double constraintCheckResolution = defaultVFParams[5],
      std::chrono::duration<double> timeout
      = std::chrono::duration<double>(defaultVFParams[6]),
      double goalTolerance = defaultVFParams[7],
      double angleDistanceRatio = defaultVFParams[8])
    : distanceTolerance(distanceTolerance)
    , positionTolerance(positionTolerance)
    , angularTolerance(angularTolerance)
    , initialStepSize(initialStepSize)
    , jointLimitTolerance(jointLimitTolerance)
    , constraintCheckResolution(constraintCheckResolution)
    , timeout(timeout)
    , goalTolerance(goalTolerance)
    , angleDistanceRatio(angleDistanceRatio){
          // Do nothing
      };
 
  double distanceTolerance;
  double positionTolerance;
  double angularTolerance;
  double initialStepSize;
  double jointLimitTolerance;
  double constraintCheckResolution;
  std::chrono::duration<double> timeout;
  double goalTolerance;
  double angleDistanceRatio;
};
 
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;
};
 
struct PlanToTSRParameters
{
  PlanToTSRParameters(
      std::size_t maxSamplingTries = 1,
      std::size_t batchSize = 100,
      std::size_t maxBatches = 1,
      std::size_t numMaxIterations = 500)
    : maxSamplingTries(maxSamplingTries)
    , batchSize(batchSize)
    , maxBatches(maxBatches)
    , numMaxIterations(numMaxIterations)
  {
    // Do nothing
  }
 
  std::size_t maxSamplingTries;
  std::size_t batchSize;
  std::size_t maxBatches;
  std::size_t numMaxIterations;
};
 
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);
 
inline std::set<std::string> dofNamesFromSkeleton(
    const dart::dynamics::MetaSkeletonPtr& skeleton)
{
  std::set<std::string> ret;
  if (!skeleton)
    return ret;
  for (const auto& dof : skeleton->getDofs())
  {
    ret.insert(dof->getName());
  }
  return ret;
}
 
} // namespace util
} // namespace robot
} // namespace aikido
 
#endif // AIKIDO_ROBOT_UTIL_HPP_