-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
distance_constraint.h
79 lines (66 loc) · 2.89 KB
/
distance_constraint.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
#pragma once
#include <memory>
#include "drake/common/sorted_pair.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/solvers/constraint.h"
#include "drake/systems/framework/context.h"
namespace drake {
namespace multibody {
/**
* Constrains the distance between a pair of geometries to be within a range
* [distance_lower, distance_upper].
*
* @ingroup solver_evaluators
*/
class DistanceConstraint : public solvers::Constraint {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(DistanceConstraint)
/**
* @param plant The plant to which the pair of geometries belong. @p plant
* should outlive this DistanceConstraint object.
* @param geometry_pair The pair of geometries between which the distance is
* constrained. Notice that we only consider the distance between a static
* geometry and a dynamic geometry, or a pair of dynamic geometries. We don't
* allow constraining the distance between two static geometries.
* @param plant_context The context for the plant. @p plant_context should
* outlive this DistanceConstraint object.
* @param distance_lower The lower bound on the distance.
* @param distance_upper The upper bound on the distance.
* @pydrake_mkdoc_identifier{double}
*/
DistanceConstraint(const multibody::MultibodyPlant<double>* const plant,
SortedPair<geometry::GeometryId> geometry_pair,
systems::Context<double>* plant_context,
double distance_lower, double distance_upper);
/**
* Overloaded constructor. Constructs the constraint with
* MultibodyPlant<AutoDiffXd>.
* @pydrake_mkdoc_identifier{autodiff}
*/
DistanceConstraint(const multibody::MultibodyPlant<AutoDiffXd>* const plant,
SortedPair<geometry::GeometryId> geometry_pair,
systems::Context<AutoDiffXd>* plant_context,
double distance_lower, double distance_upper);
~DistanceConstraint() override {}
private:
void DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const override;
void DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
AutoDiffVecXd* y) const override;
void DoEval(const Eigen::Ref<const VectorX<symbolic::Variable>>&,
VectorX<symbolic::Expression>*) const override {
throw std::logic_error(
"DistanceConstraint::DoEval does not work for symbolic variables.");
}
template <typename T>
void DoEvalGeneric(const Eigen::Ref<const VectorX<T>>& x,
VectorX<T>* y) const;
bool use_autodiff() const { return plant_autodiff_; }
const MultibodyPlant<double>* plant_double_;
systems::Context<double>* const plant_context_double_;
SortedPair<geometry::GeometryId> geometry_pair_;
const MultibodyPlant<AutoDiffXd>* const plant_autodiff_;
systems::Context<AutoDiffXd>* plant_context_autodiff_;
};
} // namespace multibody
} // namespace drake