Aikido
ConcreteRobot-impl.hpp
Go to the documentation of this file.
1 namespace aikido {
2 namespace robot {
3 
4 //==============================================================================
5 template <typename PostProcessor>
6 std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
8  const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
9  const typename PostProcessor::Params& postProcessorParams) const
10 {
11  return getTrajectoryPostProcessor<PostProcessor>(
12  getVelocityLimits(*metaSkeleton),
13  getAccelerationLimits(*metaSkeleton),
14  postProcessorParams);
15 }
16 
17 //==============================================================================
18 template <typename PostProcessor>
19 std::shared_ptr<aikido::planner::TrajectoryPostProcessor>
21  const Eigen::VectorXd& velocityLimits,
22  const Eigen::VectorXd& accelerationLimits,
23  const typename PostProcessor::Params& postProcessorParams) const
24 {
25  static_assert(
26  std::is_base_of<aikido::planner::TrajectoryPostProcessor, PostProcessor>::
27  value,
28  "PostProcessor must derive from "
29  "aikido::planner::TrajectoryPostProcessor");
30 
31  return std::make_shared<PostProcessor>(
32  velocityLimits, accelerationLimits, postProcessorParams);
33 }
34 
35 //==============================================================================
36 template <typename PostProcessor>
38  const dart::dynamics::MetaSkeletonPtr& metaSkeleton,
40  const constraint::TestablePtr& constraint,
41  const typename PostProcessor::Params& postProcessorParams)
42 {
43  return postProcessPath<PostProcessor>(
44  getVelocityLimits(*metaSkeleton),
45  getAccelerationLimits(*metaSkeleton),
46  path,
47  constraint,
48  postProcessorParams);
49 }
50 
51 //==============================================================================
52 template <typename PostProcessor>
54  const Eigen::VectorXd& velocityLimits,
55  const Eigen::VectorXd& accelerationLimits,
57  const constraint::TestablePtr& constraint,
58  const typename PostProcessor::Params& postProcessorParams)
59 {
60  auto postProcessor = getTrajectoryPostProcessor<PostProcessor>(
61  velocityLimits, accelerationLimits, postProcessorParams);
62 
63  auto interpolated
64  = dynamic_cast<const aikido::trajectory::Interpolated*>(path);
65  if (interpolated)
66  return postProcessor->postprocess(
67  *interpolated, *(cloneRNG().get()), constraint);
68 
69  auto spline = dynamic_cast<const aikido::trajectory::Spline*>(path);
70  if (spline)
71  return postProcessor->postprocess(*spline, *(cloneRNG().get()), constraint);
72 
73  std::cerr
74  << "[postProcessPath]: Path should be either Spline or Interpolated."
75  << std::endl;
76  return nullptr;
77 }
78 
79 } // namespace robot
80 } // namespace aikido
aikido::robot::ConcreteRobot::postProcessPath
aikido::trajectory::UniqueSplinePtr postProcessPath(const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const aikido::trajectory::Trajectory *path, const constraint::TestablePtr &constraint, const typename PostProcessor::Params &postProcessorParams)
Returns a post-processed trajectory that can be executed by the robot.
Definition: ConcreteRobot-impl.hpp:37
aikido
Format of serialized trajectory in YAML.
Definition: algorithm.hpp:4
aikido::robot::ConcreteRobot::getVelocityLimits
Eigen::VectorXd getVelocityLimits(const dart::dynamics::MetaSkeleton &metaSkeleton) const
Computes velocity limits from the MetaSkeleton.
aikido::robot::ConcreteRobot::getTrajectoryPostProcessor
std::shared_ptr< aikido::planner::TrajectoryPostProcessor > getTrajectoryPostProcessor(const dart::dynamics::MetaSkeletonPtr &metaSkeleton, const typename PostProcessor::Params &postProcessorParams) const
Get a postprocessor that respects velocity and acceleration limits.
Definition: ConcreteRobot-impl.hpp:7
aikido::trajectory::UniqueSplinePtr
std::unique_ptr< Spline > UniqueSplinePtr
Definition: Spline.hpp:10
aikido::robot::ConcreteRobot::getAccelerationLimits
Eigen::VectorXd getAccelerationLimits(const dart::dynamics::MetaSkeleton &metaSkeleton) const
Computes acceleration limits from the MetaSkeleton.
aikido::robot::ConcreteRobot::cloneRNG
std::unique_ptr< aikido::common::RNG > cloneRNG()
aikido::constraint::TestablePtr
std::shared_ptr< Testable > TestablePtr
Definition: Testable.hpp:13
aikido::trajectory::Interpolated
Trajectory that uses an Interpolator to interpolate between waypoints.
Definition: Interpolated.hpp:14
aikido::trajectory::Spline
Polynomial spline trajectory defined in a StateSpace.
Definition: Spline.hpp:25
aikido::trajectory::Trajectory
Time-parameterized path in a StateSpace.
Definition: Trajectory.hpp:20