-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
robot_clearance.h
138 lines (112 loc) · 5.71 KB
/
robot_clearance.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
#pragma once
#include <vector>
#include "drake/common/drake_copyable.h"
#include "drake/common/eigen_types.h"
#include "drake/multibody/tree/multibody_tree_indexes.h"
#include "drake/planning/robot_collision_type.h"
namespace drake {
namespace planning {
/** A summary of the clearance -- a collection of distance measurements --
between the robot and everything in the world. This data can be used to define
collision avoidance strategies.
Conceptually, this class represents a table:
| body index R | body index O | type | ϕᴼ(R) | Jq_ϕᴼ(R) |
| :----------: | :----------: | :--: | :---: | :------: |
| ... | ... | .. | ... | ... |
Member functions return each column of the table as an ordered collection. The
iᵗʰ entry in each collection belongs to the iᵗʰ row.
Each row characterizes the relationship between a particular *robot* body
(annotated as body R) and some "other" body (annotated as body O) in the model.
That other body O may be part of the robot or the environment.
- `body index R` is the BodyIndex of body R.
- `body index O` is the BodyIndex of body O.
- `type` implies the type of body O. Given that we know body R is a robot
body, `type` indicates that body O is also a robot body with the value
RobotCollisionType::kSelfCollision or an environment body with the value
RobotCollisionType::kEnvironmentCollision. For a correct implementation of
CollisionChecker, it will never be
RobotCollisionType::kEnvironmentAndSelfCollision.
- `ϕᴼ(R)` is the signed distance function of the other body O evaluated on
body R. The reported distance is offset by the padding value for the body
pair recorded in the CollisionChecker. It is the minimum padded distance
between bodies O and R. A point on the padded surface of body O would
report a distance of zero. Points inside that boundary report negative
distance and points outside have positive distance.
- `Jqᵣ_ϕᴼ(R)` is the Jacobian of ϕᴼ(R) with respect to the robot
configuration vector `qᵣ`. The Jacobian is the derivative as observed in
the world frame.
- The vector `qᵣ` will be a subset of the plant's full configuration `q`
when there are floating bodies or joints in the plant other than the
robot. The Jacobian is only taken with respect to the robot.
- The `jacobians()` matrix has `plant.num_positions()` columns and the
column order matches up with the full `plant.GetPositions()` order.
The columns associated with non-robot joints will be zero.
Several important notes:
- A single robot body index can appear many times as there may be many
measured distances between that robot body and other bodies in the model.
- A row *may* contain a zero-valued Jacobian Jqᵣ_ϕᴼ(R). Appropriately
filtering collisions will cull most of these Jacobians. But depending on
the structure of the robot and its representative collision geometry, it
is still possible to evaluate the Jacobian at a configuration that
represents a local optimum (zero-valued Jacobian).
@ingroup planning_collision_checker */
class RobotClearance {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(RobotClearance)
/** Creates an empty clearance with size() == 0 and num_positions as given. */
explicit RobotClearance(int num_positions) : nq_(num_positions) {
DRAKE_THROW_UNLESS(num_positions >= 0);
}
~RobotClearance();
/** @returns the number of distance measurements (rows in the table). */
int size() const { return robot_indices_.size(); }
/** @returns the number of positions (i.e., columns) in jacobians(). */
int num_positions() const { return nq_; }
// TODO(sean.curtis) Provide a guaranteed order on the rows, based on body
// index.
/** @returns the vector of *robot* body indices. */
const std::vector<multibody::BodyIndex>& robot_indices() const {
return robot_indices_;
}
/** @returns the vector of *other* body indices. */
const std::vector<multibody::BodyIndex>& other_indices() const {
return other_indices_;
}
/** @returns the vector of body collision types. */
const std::vector<RobotCollisionType>& collision_types() const {
return collision_types_;
}
/** @returns the vector of distances (`ϕᴼ(R)`). */
Eigen::Map<const Eigen::VectorXd> distances() const {
return Eigen::Map<const Eigen::VectorXd>(distances_.data(), size());
}
/** @returns the vector of distance Jacobians (`Jqᵣ_ϕᴼ(R)`); the return type
is a readonly Eigen::Map with size() rows and num_positions() columns. */
auto jacobians() const {
using RowMatrixXd =
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
return Eigen::Map<const RowMatrixXd>(jacobians_.data(), size(), nq_);
}
/** (Advanced) The mutable flavor of jacobians(). */
auto mutable_jacobians() {
using RowMatrixXd =
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
return Eigen::Map<RowMatrixXd>(jacobians_.data(), size(), nq_);
}
/** Ensures this object has storage for at least `size` rows. */
void Reserve(int size);
/** Appends one measurement to this table. */
void Append(multibody::BodyIndex robot_index,
multibody::BodyIndex other_index,
RobotCollisionType collision_type, double distance,
const Eigen::Ref<const Eigen::RowVectorXd>& jacobian);
private:
std::vector<multibody::BodyIndex> robot_indices_;
std::vector<multibody::BodyIndex> other_indices_;
std::vector<RobotCollisionType> collision_types_;
std::vector<double> distances_;
std::vector<double> jacobians_; // Stored in row-major order.
int nq_{};
};
} // namespace planning
} // namespace drake