-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
gaze_target_constraint.h
109 lines (97 loc) · 4.52 KB
/
gaze_target_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
100
101
102
103
104
105
106
107
108
109
#pragma once
#include <memory>
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/solvers/constraint.h"
namespace drake {
namespace multibody {
/**
* Constrains a target point T to be within a cone K. The point T ("T" stands
* for "target") is fixed in a frame B, with position p_BT. The cone
* originates from a point S ("S" stands for "source"), fixed in frame A with
* position p_AS, with the axis of the cone being n, also fixed
* in frame A. The half angle of the cone is θ. A common usage of this
* constraint is that a camera should gaze at some target; namely the target
* falls within a gaze cone, originating from the camera eye.
*
* Mathematically the constraint is
* p_ST_Aᵀ * n_unit_A ≥ 0
* (p_ST_Aᵀ * n_unit_A)² ≥ (cosθ)²p_ST_Aᵀ * p_ST_A
* where p_ST_A is the vector from S to T, expressed in frame A. n_unit_A is the
* unit length directional vector representing the center ray of the cone.
*
* @ingroup solver_evaluators
*/
class GazeTargetConstraint : public solvers::Constraint {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(GazeTargetConstraint)
/**
* @param plant The MultibodyPlant on which the constraint is imposed.
* `plant` should be alive during the lifetime of this constraint.
* @param frameA The frame to which the gaze cone is fixed.
* @param p_AS The position of the cone source point S, measured and expressed
* in frame A.
* @param n_A The directional vector representing the center ray of the cone,
* expressed in frame A.
* @param frameB The frame to which the target point T is fixed.
* @param p_BT The position of the target point T, measured and expressed in
* frame B.
* @param cone_half_angle The half angle of the cone. We denote it as θ in the
* class documentation. `cone_half_angle` is in radians.
* @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 `n_A` is close to zero.
* @throws std::exception if `cone_half_angle` ∉ [0, π/2].
* @throws std::exception if `plant_context` is nullptr.
* @pydrake_mkdoc_identifier{double}
*/
GazeTargetConstraint(const MultibodyPlant<double>* plant,
const Frame<double>& frameA,
const Eigen::Ref<const Eigen::Vector3d>& p_AS,
const Eigen::Ref<const Eigen::Vector3d>& n_A,
const Frame<double>& frameB,
const Eigen::Ref<const Eigen::Vector3d>& p_BT,
double cone_half_angle,
systems::Context<double>* plant_context);
/**
* Overloaded constructor.
* Construct from MultibodyPlant<AutoDiffXd> instead of
* MultibodyPlant<double>.
* @pydrake_mkdoc_identifier{autodiff}
*/
GazeTargetConstraint(const MultibodyPlant<AutoDiffXd>* plant,
const Frame<AutoDiffXd>& frameA,
const Eigen::Ref<const Eigen::Vector3d>& p_AS,
const Eigen::Ref<const Eigen::Vector3d>& n_A,
const Frame<AutoDiffXd>& frameB,
const Eigen::Ref<const Eigen::Vector3d>& p_BT,
double cone_half_angle,
systems::Context<AutoDiffXd>* plant_context);
~GazeTargetConstraint() 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(
"GazeTargetConstraint::DoEval() does not work for symbolic variables.");
}
bool use_autodiff() const { return plant_autodiff_; }
const MultibodyPlant<double>* const plant_double_;
const FrameIndex frameA_index_;
const FrameIndex frameB_index_;
const Eigen::Vector3d p_AS_;
const Eigen::Vector3d n_A_;
const Eigen::Vector3d p_BT_;
const double cone_half_angle_;
const double cos_cone_half_angle_;
systems::Context<double>* const context_double_;
const MultibodyPlant<AutoDiffXd>* const plant_autodiff_;
systems::Context<AutoDiffXd>* const context_autodiff_;
};
} // namespace multibody
} // namespace drake