-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
angle_between_vectors_constraint.h
99 lines (87 loc) · 4.22 KB
/
angle_between_vectors_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
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
#pragma once
#include <memory>
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/solvers/constraint.h"
namespace drake {
namespace multibody {
/**
* Constrains that the angle between a vector `a` and another vector `b` is
* between [θ_lower, θ_upper]. `a` is fixed to a frame A, while `b` is fixed to
* a frame B.
* Mathematically, if we denote a_unit_A as `a` expressed in frame A after
* normalization (a_unit_A has unit length), and b_unit_B as `b` expressed in
* frame B after normalization, the constraint is
* cos(θ_upper) ≤ a_unit_Aᵀ * R_AB * b_unit_B ≤ cos(θ_lower)
*
* @ingroup solver_evaluators
*/
class AngleBetweenVectorsConstraint : public solvers::Constraint {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(AngleBetweenVectorsConstraint)
/**
* Constructs an AngleBetweenVectorsConstraint.
* @param plant The MultibodyPlant on which the constraint is imposed. `plant`
* should be alive during the lifetime of this constraint.
* @param frameA The Frame object for frame A.
* @param a_A The vector `a` fixed to frame A, expressed in frame A.
* @param frameB The Frame object for frame B.
* @param b_B The vector `b` fixed to frame B, expressed in frameB.
* @param angle_lower The lower bound on the angle between `a` and `b`. It is
* denoted as θ_lower in the class documentation.
* @param angle_upper The upper bound on the angle between `a` and `b`. it is
* denoted as θ_upper in the class documentation.
* @param plant_context The Context that has been allocated for this
* `plant`. We will update the context when evaluating the constraint.
* `plant_context` should be alive during the lifetime of this constraint.
* @pre `frameA` and `frameB` must belong to `plant`.
* @throws std::exception if `plant` is nullptr.
* @throws std::exception if `a_A` is close to zero.
* @throws std::exception if `b_B` is close to zero.
* @throws std::exception if `angle_lower` is negative.
* @throws std::exception if `angle_upper` ∉ [`angle_lower`, π].
* @throws std::exception if `plant_context` is nullptr.
* @pydrake_mkdoc_identifier{double}
*/
AngleBetweenVectorsConstraint(const MultibodyPlant<double>* plant,
const Frame<double>& frameA,
const Eigen::Ref<const Eigen::Vector3d>& a_A,
const Frame<double>& frameB,
const Eigen::Ref<const Eigen::Vector3d>& b_B,
double angle_lower, double angle_upper,
systems::Context<double>* plant_context);
/**
* Overloaded constructor. Use MultibodyPlant<AutoDiffXd> instead of
* MultibodyPlant<double>.
* @pydrake_mkdoc_identifier{autodiff}
*/
AngleBetweenVectorsConstraint(const MultibodyPlant<AutoDiffXd>* plant,
const Frame<AutoDiffXd>& frameA,
const Eigen::Ref<const Eigen::Vector3d>& a_A,
const Frame<AutoDiffXd>& frameB,
const Eigen::Ref<const Eigen::Vector3d>& b_B,
double angle_lower, double angle_upper,
systems::Context<AutoDiffXd>* plant_context);
~AngleBetweenVectorsConstraint() 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(
"AngleBetweenVectorsConstraint::DoEval() does not work for symbolic "
"variables.");
}
bool use_autodiff() const { return plant_autodiff_; }
const MultibodyPlant<double>* plant_double_;
const FrameIndex frameA_index_;
const FrameIndex frameB_index_;
const Eigen::Vector3d a_unit_A_;
const Eigen::Vector3d b_unit_B_;
systems::Context<double>* const context_double_;
const MultibodyPlant<AutoDiffXd>* plant_autodiff_;
systems::Context<AutoDiffXd>* const context_autodiff_;
};
} // namespace multibody
} // namespace drake