-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
orientation_constraint.cc
158 lines (145 loc) · 6.63 KB
/
orientation_constraint.cc
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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
#include "drake/multibody/inverse_kinematics/orientation_constraint.h"
#include "drake/math/autodiff_gradient.h"
#include "drake/multibody/inverse_kinematics/kinematic_evaluator_utilities.h"
using drake::multibody::internal::RefFromPtrOrThrow;
using drake::multibody::internal::UpdateContextConfiguration;
namespace drake {
namespace multibody {
OrientationConstraint::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)
: solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
Vector1d(2 * std::cos(theta_bound) + 1), Vector1d(3)),
plant_double_{plant},
frameAbar_index_{frameAbar.index()},
frameBbar_index_{frameBbar.index()},
R_AAbar_{R_AbarA.inverse()},
R_BbarB_{R_BbarB},
context_double_(plant_context),
plant_autodiff_{nullptr},
context_autodiff_{nullptr} {
if (plant_context == nullptr)
throw std::invalid_argument("plant_context is nullptr.");
if (theta_bound < 0) {
throw std::invalid_argument(
"OrientationConstraint: theta_bound should be non-negative.\n");
}
}
OrientationConstraint::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)
: solvers::Constraint(1, RefFromPtrOrThrow(plant).num_positions(),
Vector1d(2 * std::cos(theta_bound) + 1), Vector1d(3)),
plant_double_{nullptr},
frameAbar_index_{frameAbar.index()},
frameBbar_index_{frameBbar.index()},
R_AAbar_{R_AbarA.inverse()},
R_BbarB_{R_BbarB},
context_double_(nullptr),
plant_autodiff_{plant},
context_autodiff_{plant_context} {
if (plant_context == nullptr)
throw std::invalid_argument("plant_context is nullptr.");
if (theta_bound < 0) {
throw std::invalid_argument(
"OrientationConstraint: theta_bound should be non-negative.\n");
}
}
namespace {
void EvalConstraintGradient(const systems::Context<double>& context,
const MultibodyPlant<double>& plant,
const Frame<double>& frameAbar,
const Frame<double>& frameBbar,
const math::RotationMatrix<double>& R_AAbar,
const math::RotationMatrix<double>& R_AB,
const Eigen::Ref<const AutoDiffVecXd>& x,
AutoDiffVecXd* y) {
// The constraint function is
// g(q) = tr(R_AB(q)).
// To derive the Jacobian of g, ∂g/∂q, we first differentiate
// w.r.t. time to obtain
// ġ = tr(Ṙ_AB),
// where we have dropped the dependence on q for readability. Next we expand
// Ṙ_AB to yield
// ġ = tr(ω̂_AB R_AB).
// ⎛⎡ 0 -ω₃ ω₂⎤⎡R₁₁ R₁₂ R₁₃⎤⎞
// = tr⎜⎢ ω₃ 0 -ω₁⎥⎢R₂₁ R₂₂ R₂₃⎥⎟
// ⎝⎣-ω₂ ω₁ 0 ⎦⎣R₃₁ R₃₂ R₃₃⎦⎠
// = (R₂₃ - R₃₂)ω₁ + (R₃₁ - R₁₃)ω₂ + (R₁₂ - R₂₁)ω₃,
// where we have dropped the _AB for brevity. Let
// r = (R₂₃ - R₃₂, R₃₁ - R₁₃, R₁₂ - R₂₁)ᵀ.
// Then,
// ġ = r_ABᵀ ω_AB.
// The angular velocity of B relative to A can be written as
// ω_AB = Jq_w_AB(q) q̇,
// where Jq_w_AB is the Jacobian of ω_AB with respect to q̇. Therefore,
// ġ = r_ABᵀ Jq_w_AB q̇.
// By the chain rule, ġ = ∂g/∂q q̇. Comparing this with the previous equation,
// we see that
// ∂g/∂q = r_ABᵀ Jq_w_AB.
const Eigen::Matrix3d& m = R_AB.matrix();
const Eigen::Vector3d r_AB{m(1, 2) - m(2, 1), m(2, 0) - m(0, 2),
m(0, 1) - m(1, 0)};
Eigen::MatrixXd Jq_w_AbarBbar(3, plant.num_positions());
plant.CalcJacobianAngularVelocity(context, JacobianWrtVariable::kQDot,
frameBbar, frameAbar, frameAbar,
&Jq_w_AbarBbar);
// Jq_w_AB = Jq_w_AbarBbar_A.
const Eigen::MatrixXd Jq_w_AB = R_AAbar.matrix() * Jq_w_AbarBbar;
(*y)(0).value() = R_AB.matrix().trace();
(*y)(0).derivatives() = r_AB.transpose() * Jq_w_AB * math::ExtractGradient(x);
}
template <typename T, typename S>
void DoEvalGeneric(const MultibodyPlant<T>& plant, systems::Context<T>* context,
FrameIndex frameAbar_index, FrameIndex frameBbar_index,
const math::RotationMatrix<double>& R_AAbar,
const math::RotationMatrix<double>& R_BbarB,
const Eigen::Ref<const VectorX<S>>& x, VectorX<S>* y) {
y->resize(1);
UpdateContextConfiguration(context, plant, x);
const Frame<T>& frameAbar = plant.get_frame(frameAbar_index);
const Frame<T>& frameBbar = plant.get_frame(frameBbar_index);
const math::RotationMatrix<T> R_AbarBbar =
plant.CalcRelativeRotationMatrix(*context, frameAbar, frameBbar);
// Note: The expression below has quantities with different scalar types.
// The casts from `double` to `T` preserves derivative or symbolic information
// in R_AbarBbar (if it exists).
const math::RotationMatrix<T> R_AB = R_AAbar.cast<T>() * R_AbarBbar
* R_BbarB.cast<T>();
if constexpr (std::is_same_v<T, S>) {
(*y)(0) = R_AB.matrix().trace();
} else {
EvalConstraintGradient(*context, plant, frameAbar, frameBbar, R_AAbar, R_AB,
x, y);
}
}
} // namespace
void OrientationConstraint::DoEval(const Eigen::Ref<const Eigen::VectorXd>& x,
Eigen::VectorXd* y) const {
if (use_autodiff()) {
AutoDiffVecXd y_t;
Eval(x.cast<AutoDiffXd>(), &y_t);
*y = math::ExtractValue(y_t);
} else {
DoEvalGeneric(*plant_double_, context_double_, frameAbar_index_,
frameBbar_index_, R_AAbar_, R_BbarB_, x, y);
}
}
void OrientationConstraint::DoEval(const Eigen::Ref<const AutoDiffVecXd>& x,
AutoDiffVecXd* y) const {
if (use_autodiff()) {
DoEvalGeneric(*plant_autodiff_, context_autodiff_, frameAbar_index_,
frameBbar_index_, R_AAbar_, R_BbarB_, x, y);
} else {
DoEvalGeneric(*plant_double_, context_double_, frameAbar_index_,
frameBbar_index_, R_AAbar_, R_BbarB_, x, y);
}
}
} // namespace multibody
} // namespace drake