-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
position_cost_test.cc
117 lines (95 loc) · 3.98 KB
/
position_cost_test.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
#include "drake/multibody/inverse_kinematics/position_cost.h"
#include <limits>
#include <gtest/gtest.h>
#include "drake/multibody/inverse_kinematics/test/inverse_kinematics_test_utilities.h"
namespace drake {
namespace multibody {
namespace {
using Eigen::Matrix3d;
using Eigen::Vector3d;
using Eigen::VectorXd;
using math::InitializeAutoDiff;
using math::RigidTransform;
using math::RollPitchYaw;
using systems::Context;
template <typename T>
auto ConstructTwoFreeBodiesCost(
const Eigen::Ref<const Vector3d>& p_AP,
const Eigen::Ref<const Vector3d>& p_BQ,
const Eigen::Ref<const Matrix3d>& C) {
struct ReturnValues {
std::unique_ptr<MultibodyPlant<T>> plant;
std::unique_ptr<Context<T>> context;
const Body<T>* body1{};
const Body<T>* body2{};
std::unique_ptr<PositionCost> cost;
};
ReturnValues v;
v.plant = ConstructTwoFreeBodiesPlant<T>();
v.context = v.plant->CreateDefaultContext();
v.body1 = &v.plant->GetBodyByName("body1");
v.body2 = &v.plant->GetBodyByName("body2");
v.cost = std::make_unique<PositionCost>(v.plant.get(), v.body1->body_frame(),
p_AP, v.body2->body_frame(), p_BQ, C,
v.context.get());
return v;
}
// Given two free bodies with some given poses, evaluate the position cost.
GTEST_TEST(PositionCostTest, TwoFreeBodies) {
const Vector3d p_AP(1, 2, 3);
const Vector3d p_BQ(4, 5, 6);
Matrix3d C;
C << 1, 2, 3, 4, 5, 6, 7, 8, 9;
auto [plant, context, body1, body2, cost] =
ConstructTwoFreeBodiesCost<double>(p_AP, p_BQ, C);
auto [plant_ad, context_ad, body1_ad, body2_ad, cost_ad] =
ConstructTwoFreeBodiesCost<AutoDiffXd>(p_AP, p_BQ, C);
// Note: We are passing PositionCost here instead of capturing them due to a
// known issue in C++:
// https://stackoverflow.com/questions/46114214/lambda-implicit-capture-fails-with-variable-declared-from-structured-binding
auto CheckCases = [](const PositionCost& position_cost,
const PositionCost& position_cost_ad, const VectorXd& q,
double y_expected) {
// MultibodyPlant double, Eval double.
VectorXd y(1);
position_cost.Eval(q, &y);
EXPECT_NEAR(y[0], y_expected, 1e-12);
// MultibodyPlant AutoDiffXd, Eval double.
VectorXd y_ad_d(1);
position_cost_ad.Eval(q, &y_ad_d);
EXPECT_NEAR(y_ad_d[0], y_expected, 1e-12);
// MultibodyPlant double, Eval AutoDiffXd
const VectorX<AutoDiffXd> q_ad = InitializeAutoDiff(q);
VectorX<AutoDiffXd> y_d_ad(1);
position_cost.Eval(q_ad, &y_d_ad);
EXPECT_NEAR(y_d_ad[0].value(), y_expected, 1e-12);
// MultibodyPlant AutoDiffXd, Eval AutoDiffXd
VectorX<AutoDiffXd> y_ad_ad(1);
position_cost_ad.Eval(q_ad, &y_ad_ad);
EXPECT_NEAR(y_ad_ad[0].value(), y_expected, 1e-12);
// Check that the two gradients match.
EXPECT_EQ(y_d_ad[0].derivatives().size(), q.size());
EXPECT_TRUE(CompareMatrices(y_d_ad[0].derivatives(),
y_ad_ad[0].derivatives(), 1e-11));
};
// X_WA = X_WB = Identity;
plant->SetFreeBodyPose(context.get(), *body1, RigidTransform<double>());
plant->SetFreeBodyPose(context.get(), *body2, RigidTransform<double>());
VectorXd q = plant->GetPositions(*context);
Vector3d err = p_BQ - p_AP; // because A == B.
CheckCases(*cost, *cost_ad, q, err.dot(C * err));
// X_WA, X_WB are arbitrary non-indentity transforms.
RigidTransform<double> X_WA(RollPitchYaw<double>(0.32, -0.24, -0.51),
Vector3d(0.1, 0.3, 0.72));
RigidTransform<double> X_WB(RollPitchYaw<double>(8.1, 0.42, -0.2),
Vector3d(-0.84, 0.2, 1.4));
plant->SetFreeBodyPose(context.get(), *body1, X_WA);
plant->SetFreeBodyPose(context.get(), *body2, X_WB);
q = plant->GetPositions(*context);
const Vector3d p_AQ = X_WA.inverse() * X_WB * p_BQ;
err = p_AQ - p_AP;
CheckCases(*cost, *cost_ad, q, err.dot(C * err));
}
} // namespace
} // namespace multibody
} // namespace drake