calderpg / common_robotics_utilities Goto Github PK

View Code? Open in Web Editor NEW

22.0 22.0 18.0 624 KB

Common utility functions and algorithms for robotics work used by ARC & ARM labs and TRI.

License: Other

CMake 0.04% C++ 99.96%

common_robotics_utilities's People

Contributors

Stargazers

Watchers

Forkers

damrongguoy toyotaresearchinstitute dmcconachie samzapo iantheengineer sichitong xiaoconghua arslan-z jiangchenxu wrangelvid

common_robotics_utilities's Issues

Consolidate constants in TOTP

TOTP has lots of constants, these should be consolidated.

Improve Zlib compression

Current zlib compression code involves unnecessary copies and is limited to 4GB buffer sizes.

std::function arguments make signatures hard to read.

Consider using templated "using" statements to turn this

template<typename StateType,
         typename SampleType=StateType,
         typename StateAllocator=std::allocator<StateType>>
 std::pair<std::vector<std::vector<StateType, StateAllocator>>,
           std::map<std::string, double>> RRTPlanMultiPath(
     std::vector<SimpleRRTPlannerState<StateType>>& tree,
    const std::function<SampleType(void)>& sampling_fn,
    const std::function<int64_t(
        const std::vector<SimpleRRTPlannerState<StateType>>&,
        const SampleType&)>& nearest_neighbor_fn,
    const std::function<std::vector<std::pair<StateType, int64_t>>(
        const StateType&, const SampleType&)>& forward_propagation_fn,
    const std::function<void(
        std::vector<SimpleRRTPlannerState<StateType>>&,
        const int64_t)>& state_added_callback_fn,
    const std::function<bool(const StateType&)>& check_goal_reached_fn,
    const std::function<void(
        std::vector<SimpleRRTPlannerState<StateType>>&,
        const int64_t)>& goal_reached_callback_fn,
    const std::function<bool(const int64_t)>& termination_check_fn)

into this

template<typename StateType,
         typename SampleType=StateType,
         typename StateAllocator=std::allocator<StateType>>
std::pair<std::vector<std::vector<StateType, StateAllocator>>,
          std::map<std::string, double>> RRTPlanMultiPath(
    std::vector<SimpleRRTPlannerState<StateType>>& tree,
    const SamplingFunction<StateType>& sampling_fn,
    const NearestNeighborFunction<StateType, SampleType>& nearest_neighbor_fn,
    const PropagationFunction<StateType, SampleType>& forward_propagation_fn,
    const CallbackFunction<StateType>& state_added_callback_fn,
    const CheckGoalReachedFunction<StateType>& check_goal_reached_fn,
    const CallbackFunction<StateType>& goal_reached_callback_fn,
    const TerminationCheckFunction& termination_check_fn)

TOTP hangs during construction

Occasionally when creating a TOTP object, the constructor appears to hang during a call to IntegrateForward. Eventually (4+ minutes after the constructor is called), the terminal reports Killed.

Some example inputs as logged by the following are included below.

  drake::log()->info("waypoints:");
  for (const auto& waypoint : raw_waypoints){
    drake::log()->info("{}", waypoint.transpose());
  }
  drake::log()->info("velocity_limits: {}", velocity_limits.transpose());
  drake::log()->info("acceleration_limits: {}", acceleration_limits.transpose());
  drake::log()->info("max_path_deviation: {}", max_path_deviation);
  drake::log()->info("timestep: {}", timestep);
  drake::log()->flush();

  return common_robotics_utilities::time_optimal_trajectory_parametrization
      ::Trajectory(raw_waypoints, velocity_limits, acceleration_limits,
                   max_path_deviation, timestep);

Example 1

[2021-09-01 10:57:41.664] [console] [info] waypoints:
[2021-09-01 10:57:41.665] [console] [info]   1.79874   1.09534  -1.28107  0.459357 0.0742109 0.0291835  -2.40435     -0.01      0.01
[2021-09-01 10:57:41.665] [console] [info]   1.20785  0.929724 -0.728769 0.0518051    2.4434 -0.450822  0.565328     -0.01      0.01
[2021-09-01 10:57:41.665] [console] [info]  0.637736   1.26644 -0.164602  0.516272   2.50057 0.0666952   1.13145     -0.01      0.01
[2021-09-01 10:57:41.665] [console] [info] velocity_limits: 5 5 5 5 5 5 5 0 0
[2021-09-01 10:57:41.665] [console] [info] acceleration_limits: 0.85 0.85 0.87 1.13 1.22 1.57 1.57    0    0
[2021-09-01 10:57:41.665] [console] [info] max_path_deviation: 0.01
[2021-09-01 10:57:41.665] [console] [info] timestep: 0.001
Killed

Example 2

[2021-09-01 14:14:28.574] [console] [info] waypoints:
[2021-09-01 14:14:28.574] [console] [info]  1.47535  1.07883 -1.86478  1.52119 0.200813 -0.64916  2.45783    -0.01     0.01
[2021-09-01 14:14:28.574] [console] [info]  -0.587795   0.306845   -1.71638 -0.0474613  -0.706651   0.187446    1.47559      -0.01       0.01
[2021-09-01 14:14:28.574] [console] [info]  -1.35365 -0.364784  -2.79961 -0.424282  -2.60627  0.413107   1.82557     -0.01      0.01
[2021-09-01 14:14:28.574] [console] [info] velocity_limits: 5 5 5 5 5 5 5 0 0
[2021-09-01 14:14:28.574] [console] [info] acceleration_limits: 2.125 2.125 2.175 2.825  3.05 3.925 3.925     0     0
[2021-09-01 14:14:28.574] [console] [info] max_path_deviation: 0.01
[2021-09-01 14:14:28.574] [console] [info] timestep: 0.001
Killed

Example 3

[2021-09-01 14:51:23.278] [console] [info] waypoints:
[2021-09-01 14:51:23.278] [console] [info]  -2.13759  0.987328  -1.98412 0.0896687  0.337348   1.42335   -1.3307     -0.01      0.01
[2021-09-01 14:51:23.278] [console] [info]  0.793133 0.0747637  0.413363 0.0644863   1.52315  0.290163  -1.17068     -0.01      0.01
[2021-09-01 14:51:23.278] [console] [info]  -0.169478   0.744316    1.16333  0.0502231    2.49467 -0.0181687    0.42362      -0.01       0.01
[2021-09-01 14:51:23.278] [console] [info] velocity_limits: 5 5 5 5 5 5 5 0 0
[2021-09-01 14:51:23.278] [console] [info] acceleration_limits: 2.125 2.125 2.175 2.825  3.05 3.925 3.925     0     0
[2021-09-01 14:51:23.278] [console] [info] max_path_deviation: 0.01
[2021-09-01 14:51:23.278] [console] [info] timestep: 0.001
Killed

Undesirable behavior of ResamplePath and TOTP for zero-length paths

ResamplePath will shorten path segments with zero distance, i.e. [[1, 2], [1, 2]] becomes [[1, 2]]. This is normally desirable, but TOTP requires paths with at least two waypoints, and will segfault when given a path with only one waypoint. Additionally, TOTP appears to produce nonsense trajectories for zero-length paths.

Action items:

Decide if ResamplePath should ever resample a path to a single waypoint, and correct if not.
Add safety check in TOTP parametrization for single-waypoint paths (and/or replace offending operator [] with .at() to get such bounds checks).
Assess TOTP trajectories for zero-length paths and see if fixes are necessary.

TOTP negative path velocity assertion

Occasionally when creating a TOTP object, an assertion triggers. An example input as logged by the following is included below.

  drake::log()->info("waypoints:");
  for (const auto& waypoint : raw_waypoints){
    drake::log()->info("{}", waypoint.transpose());
  }
  drake::log()->info("velocity_limits: {}", velocity_limits.transpose());
  drake::log()->info("acceleration_limits: {}", acceleration_limits.transpose());
  drake::log()->info("max_path_deviation: {}", max_path_deviation);
  drake::log()->info("timestep: {}", timestep);
  drake::log()->flush();

  return common_robotics_utilities::time_optimal_trajectory_parametrization
      ::Trajectory(raw_waypoints, velocity_limits, acceleration_limits,
                   max_path_deviation, timestep);

Example:

[2021-09-01 10:53:58.618] [console] [info] waypoints:
[2021-09-01 10:53:58.618] [console] [info]   -2.6094   1.44521   -1.7293  -1.17439   -1.9229 -0.321022  -2.01043     -0.01      0.01
[2021-09-01 10:53:58.618] [console] [info]  -2.01408   1.32902  -1.73112  -1.27455   -1.8832 -0.132581  -1.89372     -0.01      0.01
[2021-09-01 10:53:58.618] [console] [info] -0.851836 -0.126547   0.76479  0.737985  0.286924 -0.779685  -1.50867     -0.01      0.01
[2021-09-01 10:53:58.618] [console] [info]  0.637736   1.26644 -0.164602  0.516272   2.50057 0.0666952   1.13145     -0.01      0.01
[2021-09-01 10:53:58.618] [console] [info] velocity_limits: 5 5 5 5 5 5 5 0 0
[2021-09-01 10:53:58.618] [console] [info] acceleration_limits: 0.85 0.85 0.87 1.13 1.22 1.57 1.57    0    0
[2021-09-01 10:53:58.618] [console] [info] max_path_deviation: 0.01
[2021-09-01 10:53:58.618] [console] [info] timestep: 0.001
terminate called after throwing an instance of 'std::runtime_error'
  what():  Error while integrating backward: Negative path velocity
Aborted (core dumped)

Possibly related to #19.

Add more distinction between @param tags in doc

I love the detailed doc. I think it would be easier to read if there was more distinction between @param tags, e.g.

/// Plan multiple paths using a bidirectional RRT planner. This planner will
/// continue to explore and add new solution paths until terminated.
/// @param start_tree existing starting tree. This tree must contain at least
///   one node, but can contain multiple nodes as well. All nodes in @param
///   start_tree are assumed to be valid starting nodes, and if they are linked
///   together, they must have valid parent-child linkage.
/// @param goal_tree existing goal tree. This tree must contain at least one
///   node, but can contain multiple nodes as well. All nodes in @param
///   goal_tree are assumed to be valid goal nodes, and if they are linked
///   together, they must have valid parent-child linkage.
/// @param state_sampling_fn function to sample a new state.
/// @param nearest_neighbor_fn function to compute the nearest neighbor index
///   in the provided tree given the provided sample. If a negative index
///   (showing that no valid nearest neighbor could be found) planning is
///   terminated.
/// @param forward_propagation_fn function to forward propagate states from
///   the provided state to the provided sample. The propagated states are
///   returned as a vector<pair<state, relative_index>> in which state is the
///   new propagated state and relative_index is a relative index used to
///   connect the new state to the correct parent state. This relative_index is
///   the index of the parent state, relative to the vector of propagated nodes.
///   A negative value means the nearest neighbor in the tree, zero means the
///   first propagated node, and so on. NOTE - the relative parent index *must*
///   be lower than the index in the list of prograted nodes i.e. the first node
///   must have a negative value, and so on. While complicated, this structure
///   allows @param forward_propagation_fn to return an entire subtree at once.
/// @param state_added_callback_fn callback function called once a state is
///   added to the tree, providing a mutable reference to the tree and the index
///   of the newly-added state. This can be used, for example, to update a
///   KD-tree used for nearest neighbors. You can leave this default-constructed
///   ({}) if you do not need it.
/// @param states_connected_fn function to check if the two provided states, one
///   from each tree, are connected, and a solution has been found.
/// @param goal_bridge_callback_fn callback function called once a solution
///   "goal bridge", that is a pair of nodes, one from each tree, has been
///   found. It provides a mutable reference to the start tree, the goal bridge
///   index in the start tree, a mutable reference to the goal tree, the goal
///   bridge index in the goal tree, and a bool flag that specifies which tree
///   is the active tree. If this flag is true, start_tree is the active tree.
///   You can leave this default-constructed ({}) if you do not need it.
/// @param tree_sampling_bias probability that the next sample should be from
///   the target tree, rather than from calling @param state_sampling_fn.
/// @param p_switch_tree probability at each iteration that the active tree
///   should be swapped.
/// @param termination_check_fn Returns true if planning has been
///   terminated. The provided int64_t values are the current size of the start
///   and goal tree, respectively. These may be useful for a size-limited
///   planning problem.
/// @param rng a PRNG for use in internal sampling and tree swaps.
/// @return pair<paths, statistics> where paths is a vector of solution paths
///   and statistics is a map<string, double> of useful statistics collected
///   while planning.

calderpg / common_robotics_utilities Goto Github PK

common_robotics_utilities's People

Contributors

Stargazers

Watchers

Forkers

common_robotics_utilities's Issues

Consolidate constants in TOTP

Improve Zlib compression

std::function arguments make signatures hard to read.

TOTP hangs during construction

Undesirable behavior of ResamplePath and TOTP for zero-length paths

TOTP negative path velocity assertion

Add more distinction between @param tags in doc

Recommend Projects

React

Vue.js

Typescript

TensorFlow

Django

Laravel

D3

Recommend Topics

javascript

web

server

Machine learning

Visualization

Game

Recommend Org

Facebook

Microsoft

Google

Alibaba

D3

Tencent