-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
orientation_constraint.h
112 lines (100 loc) · 4.74 KB
/
orientation_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
110
111
112
#pragma once
#include <memory>
#include "drake/math/rotation_matrix.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/solvers/constraint.h"
namespace drake {
namespace multibody {
/**
* Constrains that the angle difference θ between the orientation of frame A
* and the orientation of frame B to satisfy θ ≤ θ_bound. The angle
* difference between frame A's orientation R_WA and B's orientation R_WB is θ
* (θ ∈ [0, π]), if there exists a rotation axis a, such that rotating frame
* A by angle θ about axis a aligns it with frame B. Namely
* R_AB = I + sinθ â + (1-cosθ)â² (1)
* where R_AB is the orientation of frame B expressed in frame A. â is the skew
* symmetric matrix of the rotation axis a. Equation (1) is the Rodrigues
* formula that computes the rotation matrix froma rotation axis a and an angle
* θ, https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
* If the users want frame A and frame B to align perfectly, they can set
* θ_bound = 0.
* Mathematically, this constraint is imposed as
* trace(R_AB) ≥ 2cos(θ_bound) + 1 (1)
* To derive (1), using Rodrigues formula
* R_AB = I + sinθ â + (1-cosθ)â²
* where
* trace(R_AB) = 2cos(θ) + 1 ≥ 2cos(θ_bound) + 1
*
* @ingroup solver_evaluators
*/
class OrientationConstraint : public solvers::Constraint {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(OrientationConstraint)
/**
* Constructs an OrientationConstraint object.
* The frame A is fixed to a frame A̅, with orientatation `R_AbarA` measured
* in frame A̅. The frame B is fixed to a frame B̅, with orientation `R_BbarB`
* measured in frame B. We constrain the angle between frame A and B to be
* less than `theta_bound`.
* @param plant The MultibodyPlant on which the constraint is imposed. `plant`
* should be alive during the lifetime of this constraint.
* @param frameAbar The frame A̅ in the model to which frame A is fixed.
* @param R_AbarA The orientation of frame A measured in frame A̅.
* @param frameBbar The frame B̅ in the model to which frame B is fixed.
* @param R_BbarB The orientation of frame B measured in frame B̅.
* @param theta_bound The bound on the angle difference between frame A's
* orientation and frame B's orientation. It is denoted as θ_bound in the
* class documentation. `theta_bound` 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.
* @throws std::exception if `plant` is nullptr.
* @throws std::exception if `frameAbar` or `frameBbar` does not belong to
* `plant`.
* @throws std::exception if angle_bound < 0.
* @throws std::exception if `plant_context` is nullptr.
* @pydrake_mkdoc_identifier{double}
*/
OrientationConstraint(
const MultibodyPlant<double>* const plant,
const Frame<double>& frameAbar,
const math::RotationMatrix<double>& R_AbarA,
const Frame<double>& frameBbar,
const math::RotationMatrix<double>& R_BbarB, double theta_bound,
systems::Context<double>* plant_context);
/**
* Overloaded constructor.
* Constructs the constraint using MultibodyPlant<AutoDiffXd>
* @pydrake_mkdoc_identifier{autodiff}
*/
OrientationConstraint(const MultibodyPlant<AutoDiffXd>* const plant,
const Frame<AutoDiffXd>& frameAbar,
const math::RotationMatrix<double>& R_AbarA,
const Frame<AutoDiffXd>& frameBbar,
const math::RotationMatrix<double>& R_BbarB,
double theta_bound,
systems::Context<AutoDiffXd>* plant_context);
~OrientationConstraint() 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(
"OrientationConstraint::DoEval() does not work for symbolic "
"variables.");
}
bool use_autodiff() const { return plant_autodiff_; }
const MultibodyPlant<double>* const plant_double_;
const FrameIndex frameAbar_index_;
const FrameIndex frameBbar_index_;
const math::RotationMatrix<double> R_AAbar_;
const math::RotationMatrix<double> R_BbarB_;
systems::Context<double>* const context_double_;
const MultibodyPlant<AutoDiffXd>* const plant_autodiff_;
systems::Context<AutoDiffXd>* context_autodiff_;
};
} // namespace multibody
} // namespace drake