-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
brick_static_equilibrium_constraint.cc
150 lines (139 loc) · 5.97 KB
/
brick_static_equilibrium_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
#include "drake/examples/planar_gripper/brick_static_equilibrium_constraint.h"
#include <memory>
#include "drake/examples/planar_gripper/gripper_brick_planning_constraint_helper.h"
#include "drake/math/autodiff.h"
#include "drake/math/autodiff_gradient.h"
#include "drake/multibody/inverse_kinematics/kinematic_evaluator_utilities.h"
namespace drake {
namespace examples {
namespace planar_gripper {
BrickStaticEquilibriumNonlinearConstraint::
BrickStaticEquilibriumNonlinearConstraint(
const GripperBrickHelper<double>& gripper_brick_system,
std::vector<std::pair<Finger, BrickFace>> finger_face_contacts,
systems::Context<double>* plant_mutable_context)
: solvers::Constraint(
3, // planar case, only constrain the (y, z) component
// of the force, and x component of the torque.
gripper_brick_system.plant().num_positions() +
finger_face_contacts.size() * 2,
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()),
gripper_brick_system_{gripper_brick_system},
finger_face_contacts_(std::move(finger_face_contacts)),
plant_mutable_context_(plant_mutable_context) {
brick_mass_ = gripper_brick_system_.plant()
.GetBodyByName("brick_link")
.default_mass();
}
template <typename T>
void BrickStaticEquilibriumNonlinearConstraint::DoEvalGeneric(
const Eigen::Ref<const VectorX<T>>& x, VectorX<T>* y) const {
// y = [R_WBᵀ * mg + ∑ᵢ f_Cbi_B]
// [∑ᵢp_Cbi_B.cross(f_Cbi_B)]
using std::cos;
using std::sin;
y->resize(3);
const auto& plant = gripper_brick_system_.plant();
multibody::internal::UpdateContextConfiguration(
plant_mutable_context_, plant, x.head(plant.num_positions()));
const T theta = x(gripper_brick_system_.brick_revolute_x_position_index());
const T sin_theta = sin(theta);
const T cos_theta = cos(theta);
Matrix2<T> R_WB;
R_WB << cos_theta, -sin_theta, sin_theta, cos_theta;
// Compute R_WBᵀ * mg, the gravitational force expressed in the brick frame.
const Vector2<T> f_B =
R_WB.transpose() *
Eigen::Vector2d(
0,
-brick_mass_ *
multibody::UniformGravityFieldElement<double>::kDefaultStrength);
y->template head<2>() = f_B;
(*y)(2) = T(0);
for (int i = 0; i < static_cast<int>(finger_face_contacts_.size()); ++i) {
// Compute ∑ᵢ f_Cbi_B.
y->template head<2>() +=
x.template segment<2>(plant.num_positions() + i * 2); // f_Cbi_B
// Compute the fingertip contact point Cb in the brick frame.
// We first compute finger tip (sphere center) position in the brick frame.
const Vector3<T> p_BFingertip = ComputeFingerTipInBrickFrame(
gripper_brick_system_, finger_face_contacts_[i].first,
*plant_mutable_context_, x.head(plant.num_positions()));
// p_BCb is to shift p_BFingertip along the face inward normal direction by
// finger tip sphere radius.
Vector2<T> p_BCb = p_BFingertip.template tail<2>();
switch (finger_face_contacts_[i].second) {
case BrickFace::kPosY: {
p_BCb(0) -= T(gripper_brick_system_.finger_tip_radius());
break;
}
case BrickFace::kNegY: {
p_BCb(0) += T(gripper_brick_system_.finger_tip_radius());
break;
}
case BrickFace::kPosZ: {
p_BCb(1) -= T(gripper_brick_system_.finger_tip_radius());
break;
}
case BrickFace::kNegZ: {
p_BCb(1) += T(gripper_brick_system_.finger_tip_radius());
break;
}
}
// Now compute the torque about the COM
(*y)(2) += p_BCb(0) * x(plant.num_positions() + 2 * i + 1) -
p_BCb(1) * x(plant.num_positions() + 2 * i);
}
}
void BrickStaticEquilibriumNonlinearConstraint::DoEval(
const Eigen::Ref<const Eigen::VectorXd>& x, Eigen::VectorXd* y) const {
DoEvalGeneric<double>(x, y);
}
void BrickStaticEquilibriumNonlinearConstraint::DoEval(
const Eigen::Ref<const AutoDiffVecXd>& x, AutoDiffVecXd* y) const {
DoEvalGeneric<AutoDiffXd>(x, y);
}
void BrickStaticEquilibriumNonlinearConstraint::DoEval(
const Eigen::Ref<const VectorX<symbolic::Variable>>&,
VectorX<symbolic::Expression>*) const {
throw std::runtime_error(
"BrickStaticEquilibriumNonlinearConstraint::DoEval does not support "
"symbolic computation.");
}
Eigen::Matrix<symbolic::Variable, 2, Eigen::Dynamic>
AddBrickStaticEquilibriumConstraint(
const GripperBrickHelper<double>& gripper_brick_system,
const std::vector<std::pair<Finger, BrickFace>>& finger_face_contacts,
const Eigen::Ref<const VectorX<symbolic::Variable>>& q_vars,
systems::Context<double>* plant_mutable_context,
solvers::MathematicalProgram* prog) {
const int num_contacts = static_cast<int>(finger_face_contacts.size());
const auto f_Cb_B =
prog->NewContinuousVariables<2, Eigen::Dynamic>(2, num_contacts);
const auto& plant = gripper_brick_system.plant();
// Now add the nonlinear constraint that the total wrench is 0.
VectorX<symbolic::Variable> nonlinear_constraint_bound_vars(
plant.num_positions() + 2 * num_contacts);
nonlinear_constraint_bound_vars.head(plant.num_positions()) = q_vars;
for (int i = 0; i < num_contacts; ++i) {
nonlinear_constraint_bound_vars.segment<2>(plant.num_positions() + 2 * i) =
f_Cb_B.col(i);
}
prog->AddConstraint(
std::make_shared<BrickStaticEquilibriumNonlinearConstraint>(
gripper_brick_system, finger_face_contacts, plant_mutable_context),
nonlinear_constraint_bound_vars);
// Add the linear constraint that the contact force is within the friction
// cone.
for (int i = 0; i < num_contacts; ++i) {
const double friction_cone_shrink_factor = 1;
AddFrictionConeConstraint(gripper_brick_system,
finger_face_contacts[i].first,
finger_face_contacts[i].second, f_Cb_B,
friction_cone_shrink_factor, prog);
}
return f_Cb_B;
}
} // namespace planar_gripper
} // namespace examples
} // namespace drake