Drake
Drake C++ Documentation
drake::planning Namespace Reference

Namespaces

 graph_algorithms
 
 test
 
 trajectory_optimization
 

Classes

class  BodyShapeDescription
 BodyShapeDescription captures all the information necessary to describe a SceneGraph collision shape associated with a MultibodyPlant Body: a shape S, the MultibodyPlant body B (identified by model instance and body names), and the rigid pose of the shape S relative to the body B, X_BS. More...
 
class  CollisionChecker
 Interface for collision checkers to use. More...
 
class  CollisionCheckerContext
 This class represents the data necessary for CollisionChecker to operate safely across multiple threads in its const API. More...
 
struct  CollisionCheckerParams
 A set of common constructor parameters for a CollisionChecker. More...
 
class  DistanceAndInterpolationProvider
 This class represents the base interface for performing configuration distance and interpolation operations, used by CollisionChecker. More...
 
class  EdgeMeasure
 The measure of the distance of the edge from q1 to q2 and the portion of that is collision free. More...
 
struct  IrisFromCliqueCoverOptions
 
class  LinearDistanceAndInterpolationProvider
 This class represents a basic "linear" implementation of DistanceAndInterpolationProvider. More...
 
class  RobotClearance
 A summary of the clearance – a collection of distance measurements – between the robot and everything in the world. More...
 
class  RobotDiagram
 Storage for a combined diagram, plant, and scene graph. More...
 
class  RobotDiagramBuilder
 Storage for a combined diagram builder, plant, and scene graph. More...
 
class  SceneGraphCollisionChecker
 An implementation of CollisionChecker that uses SceneGraph to provide collision checks. More...
 
class  UnimplementedCollisionChecker
 A concrete collision checker implementation that throws an exception for every virtual function hook. More...
 
class  ZmpPlanner
 Given a desired two dimensional (X and Y) zero-moment point (ZMP) trajectory parameterized as a piecewise polynomial, an optimal center of mass (CoM) trajectory is planned using a linear inverted pendulum model (LIPM). More...
 
struct  ZmpTestTraj
 A structure for storing trajectories from simulating a linear inverted pendulum model (LIPM) using the policy from a ZmpPlanner. More...
 

Typedefs

using ConfigurationDistanceFunction = std::function< double(const Eigen::VectorXd &, const Eigen::VectorXd &)>
 Configuration distance takes two configurations of the robot, q1 and q2, both as Eigen::VectorXd, and returns (potentially weighted) C-space distance as a double. More...
 
using ConfigurationInterpolationFunction = std::function< Eigen::VectorXd(const Eigen::VectorXd &, const Eigen::VectorXd &, double)>
 Configuration interpolation function takes two configurations of the robot, q1, and q2, both as Eigen::VectorXd, plus a ratio, r, in [0, 1] and returns the interpolated configuration. More...
 

Enumerations

enum  RobotCollisionType : uint8_t { kNoCollision = 0x00, kEnvironmentCollision = 0x01, kSelfCollision = 0x02, kEnvironmentAndSelfCollision = kEnvironmentCollision | kSelfCollision }
 Enumerates these predicates (and their combinations): More...
 

Functions

BodyShapeDescription MakeBodyShapeDescription (const multibody::MultibodyPlant< double > &plant, const systems::Context< double > &plant_context, const geometry::GeometryId &geometry_id)
 Constructs a BodyShapeDescription by extracting the shape, pose, and names associated with the provided geometry_id. More...
 
void IrisInConfigurationSpaceFromCliqueCover (const CollisionChecker &checker, const IrisFromCliqueCoverOptions &options, RandomGenerator *generator, std::vector< geometry::optimization::HPolyhedron > *sets, const planning::graph_algorithms::MaxCliqueSolverBase *max_clique_solver=nullptr)
 Cover the configuration space in Iris regions using the Visibility Clique Cover Algorithm as described in. More...
 
ZmpTestTraj SimulateZmpPolicy (const ZmpPlanner &zmp_planner, const Eigen::Vector4d &x0, double dt, double extra_time_at_the_end)
 Forward simulation using the linear policy from zmp_planner starting from the initial condition x0 using explicit Euler integration. More...
 
std::vector< trajectories::PiecewisePolynomial< double > > GenerateDesiredZmpTrajs (const std::vector< Eigen::Vector2d > &footsteps, double double_support_duration, double single_support_duration)
 Generates desired ZMP trajectories as piecewise polynomials given the desired footsteps. More...
 
RobotCollisionType SetInEnvironmentCollision (RobotCollisionType collision_type, bool in_environment_collision)
 
RobotCollisionType SetInSelfCollision (RobotCollisionType collision_type, bool in_self_collision)
 
Eigen::SparseMatrix< bool > VisibilityGraph (const CollisionChecker &checker, const Eigen::Ref< const Eigen::MatrixXd > &points, Parallelism parallelize=Parallelism::Max())
 Given some number of sampled points in the configuration space of checker's plant(), computes the "visibility graph" – two points have an edge between them if the line segment connecting them is collision free. More...
 

Typedef Documentation

◆ ConfigurationDistanceFunction

using ConfigurationDistanceFunction = std::function<double(const Eigen::VectorXd&, const Eigen::VectorXd&)>

Configuration distance takes two configurations of the robot, q1 and q2, both as Eigen::VectorXd, and returns (potentially weighted) C-space distance as a double.

The returned distance will be strictly non-negative.

To be valid, the function must satisfy the following condition:

  • dist(q, q) ≡ 0

for values of q that are valid for the CollisionChecker's plant.

◆ ConfigurationInterpolationFunction

using ConfigurationInterpolationFunction = std::function<Eigen::VectorXd( const Eigen::VectorXd&, const Eigen::VectorXd&, double)>

Configuration interpolation function takes two configurations of the robot, q1, and q2, both as Eigen::VectorXd, plus a ratio, r, in [0, 1] and returns the interpolated configuration.

Behavior of the function for values of r outside of the range [0,1] is undefined.

To be valid, the function must satisfy the following conditions:

  • interpolate(q1, q2, 0) ≡ q1
  • interpolate(q1, q2, 1) ≡ q2
  • interpolate(q, q, r) ≡ q, for all r in [0, 1]

for values of q, q1, and q2 that are valid for the CollisionChecker's plant.

Function Documentation

◆ GenerateDesiredZmpTrajs()

std::vector<trajectories::PiecewisePolynomial<double> > drake::planning::GenerateDesiredZmpTrajs ( const std::vector< Eigen::Vector2d > &  footsteps,
double  double_support_duration,
double  single_support_duration 
)

Generates desired ZMP trajectories as piecewise polynomials given the desired footsteps.

The knot points are generated as follows:

T: 0, knots: fs[0]
T: ss, knots: fs[0]
T: ss + ds, knots: fs[1]
T: 2 * ss + ds, knots: fs[1]
T: 2 * ss + 2 * ds, knots: fs[2]
T: 3 * ss + 2 * ds, knots: fs[2]
 ...

ss stands for single_support_duration, and ds for double_support_duration.

Parameters
footsteps,XY pair
double_support_duration,Durationfor double support.
single_support_duration,Durationfor single support.
Returns
Three trajectories: 0 is zero-order-hold, 1 is linear, 2 is cubic.

◆ IrisInConfigurationSpaceFromCliqueCover()

void drake::planning::IrisInConfigurationSpaceFromCliqueCover ( const CollisionChecker checker,
const IrisFromCliqueCoverOptions options,
RandomGenerator generator,
std::vector< geometry::optimization::HPolyhedron > *  sets,
const planning::graph_algorithms::MaxCliqueSolverBase max_clique_solver = nullptr 
)

Cover the configuration space in Iris regions using the Visibility Clique Cover Algorithm as described in.

P. Werner, A. Amice, T. Marcucci, D. Rus, R. Tedrake "Approximating Robot Configuration Spaces with few Convex Sets using Clique Covers of Visibility Graphs" In 2024 IEEE Internation Conference on Robotics and Automation. https://arxiv.org/abs/2310.02875

Parameters
checkerThe collision checker containing the plant and its associated scene_graph.
generatorThere are points in the algorithm requiring randomness. The generator controls this source of randomness.
sets[in/out] initial sets covering the space (potentially empty). The cover is written into this vector.
max_clique_solverThe min clique cover problem is approximatley solved by repeatedly solving max clique on the uncovered graph and adding this largest clique to the cover. The max clique problem is solved by this solver. If parallelism is set to allow more than 1 thread, then the solver must be implemented in C++.

If nullptr is passed as the max_clique_solver, then max clique will be solved using an instance of MaxCliqueSolverViaGreedy, which is a fast heuristic. If higher quality cliques are desired, consider changing the solver to an instance of MaxCliqueSolverViaMip. Currently, the padding in the collision checker is not forwarded to the algorithm, and therefore the final regions do not necessarily respect this padding. Effectively, this means that the regions are generated as if the padding is set to 0. This behavior may be adjusted in the future at the resolution of #18830.

Note that MaxCliqueSolverViaMip requires the availability of a Mixed-Integer Linear Programming solver (e.g. Gurobi and/or Mosek). We recommend enabling those solvers if possible because they produce higher quality cliques (https://drake.mit.edu/bazel.html#proprietary_solvers). The method will throw if max_clique_solver cannot solve the max clique problem.

◆ MakeBodyShapeDescription()

BodyShapeDescription drake::planning::MakeBodyShapeDescription ( const multibody::MultibodyPlant< double > &  plant,
const systems::Context< double > &  plant_context,
const geometry::GeometryId geometry_id 
)

Constructs a BodyShapeDescription by extracting the shape, pose, and names associated with the provided geometry_id.

Precondition
plant_context is compatible with plant.
plant is connected to a scene graph.
geometry_id refers to a geometry rigidly affixed to a body of plant.

◆ SetInEnvironmentCollision()

RobotCollisionType drake::planning::SetInEnvironmentCollision ( RobotCollisionType  collision_type,
bool  in_environment_collision 
)
Returns
a RobotCollisionType where the environment-collision value is that asserted by in_environment_collision, and the self-collision value is that asserted by collision_type.

◆ SetInSelfCollision()

RobotCollisionType drake::planning::SetInSelfCollision ( RobotCollisionType  collision_type,
bool  in_self_collision 
)
Returns
a RobotCollisionType where the self-collision value is that asserted by in_self_collision, and the environment-collision value is that asserted by collision_type.

◆ SimulateZmpPolicy()

ZmpTestTraj drake::planning::SimulateZmpPolicy ( const ZmpPlanner zmp_planner,
const Eigen::Vector4d &  x0,
double  dt,
double  extra_time_at_the_end 
)

Forward simulation using the linear policy from zmp_planner starting from the initial condition x0 using explicit Euler integration.

Parameters
zmp_planner,Containsplanned center of mass trajectory, value function, and linear policy.
x0,Initialcondition.
dt,Timestep.
extra_time_at_the_end,Simulateextra_time_at_the_end seconds past the end of the trajectories for convergence.
Returns
ZmpTestTraj that contains all the information.

◆ VisibilityGraph()

Eigen::SparseMatrix<bool> drake::planning::VisibilityGraph ( const CollisionChecker checker,
const Eigen::Ref< const Eigen::MatrixXd > &  points,
Parallelism  parallelize = Parallelism::Max() 
)

Given some number of sampled points in the configuration space of checker's plant(), computes the "visibility graph" – two points have an edge between them if the line segment connecting them is collision free.

See CollisionChecker documentation for more information on how edge collision checks are performed.

Note that this method assumes the collision checker has symmetric behavior (i.e. checking edge (q1, q2) is the same as checking edge (q2, q1)). This is true for many collision checkers (e.g. those using LinearDistanceAndInterpolationProvider, which is the default), but some more complex spaces with non-linear interpolation (e.g. a Dubin's car) are not symmetric.

If parallelize specifies more than one thread, then the CollisionCheckerParams::distance_and_interpolation_provider for checker must be implemented in C++, either by providing the C++ implementation directly directly or by using the default provider.

Returns
the adjacency matrix, A(i,j) == true iff points.col(i) is visible from points.col(j). A is always symmetric.
Precondition
points.rows() == total number of positions in the collision checker plant.