calderpg / common_robotics_utilities Goto Github PK
View Code? Open in Web Editor NEWCommon utility functions and algorithms for robotics work used by ARC & ARM labs and TRI.
License: Other
Common utility functions and algorithms for robotics work used by ARC & ARM labs and TRI.
License: Other
TOTP has lots of constants, these should be consolidated.
Current zlib compression code involves unnecessary copies and is limited to 4GB buffer sizes.
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)
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
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:
ResamplePath
should ever resample a path to a single waypoint, and correct if not.operator []
with .at()
to get such bounds checks).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.
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.
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.