Skip to content

Commit 32ea7ee

Browse files
dellaertdellaert
committed
Fix bug in interpolate
1 parent 5a95eae commit 32ea7ee

File tree

4 files changed

+171
-58
lines changed

4 files changed

+171
-58
lines changed

‎gtdynamics/kinematics/KinematicsInterval.cpp‎

Lines changed: 5 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -149,9 +149,12 @@ Values Kinematics::interpolate<Interval>(
149149
const ContactGoals& contact_goals1,
150150
const ContactGoals& contact_goals2) const {
151151
Values result;
152-
const double dt = 1.0 / (interval.k_start - interval.k_end); // 5 6 7 8 9 [10
152+
const double denominator =
153+
static_cast<double>(interval.k_end - interval.k_start);
153154
for (size_t k = interval.k_start; k <= interval.k_end; k++) {
154-
const double t = dt * (k - interval.k_start);
155+
const double t = denominator > 0.0
156+
? static_cast<double>(k - interval.k_start) / denominator
157+
: 0.0;
155158
ContactGoals goals;
156159
transform(contact_goals1.begin(), contact_goals1.end(),
157160
contact_goals2.begin(), std::back_inserter(goals),

‎gtdynamics/kinematics/KinematicsSlice.cpp‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,10 +14,10 @@
1414
#include <gtdynamics/kinematics/ContactHeightFactor.h>
1515
#include <gtdynamics/kinematics/ContactKinematicsTwistFactor.h>
1616
#include <gtdynamics/kinematics/JointLimitFactor.h>
17+
#include <gtdynamics/kinematics/Kinematics.h>
1718
#include <gtdynamics/kinematics/PointGoalFactor.h>
1819
#include <gtdynamics/kinematics/PoseFactor.h>
1920
#include <gtdynamics/kinematics/TwistFactor.h>
20-
#include <gtdynamics/kinematics/Kinematics.h>
2121
#include <gtdynamics/utils/Slice.h>
2222
#include <gtsam/linear/Sampler.h>
2323
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@@ -371,6 +371,7 @@ Values Kinematics::inverse<Slice>(const Slice& slice, const Robot& robot,
371371
// graph.addPrior<gtsam::Pose3>(PoseKey(0, slice.k),
372372
// gtsam::Pose3(), nullptr);
373373

374+
// TODO(frank): we should allow warm start when used in interval context.
374375
auto initial_values = initialValues(slice, robot);
375376

376377
return optimize(graph, constraints, initial_values);

‎gtdynamics/kinematics/tests/testKinematicsInterval.cpp‎

Lines changed: 70 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,44 @@ using std::string;
2626

2727
#include "contactGoalsExample.h"
2828

29+
namespace {
30+
31+
ContactGoals InterpolateContactGoals(const ContactGoals& contact_goals_start,
32+
const ContactGoals& contact_goals_end,
33+
double alpha) {
34+
ContactGoals contact_goals;
35+
std::transform(
36+
contact_goals_start.begin(), contact_goals_start.end(),
37+
contact_goals_end.begin(), std::back_inserter(contact_goals),
38+
[alpha](const ContactGoal& start_goal, const ContactGoal& end_goal) {
39+
return ContactGoal{
40+
start_goal.point_on_link,
41+
(1.0 - alpha) * start_goal.goal_point + alpha * end_goal.goal_point};
42+
});
43+
return contact_goals;
44+
}
45+
46+
gtsam::Values ManualInterpolateByInverse(
47+
const Kinematics& kinematics, const Interval& interval, const Robot& robot,
48+
const ContactGoals& contact_goals_start,
49+
const ContactGoals& contact_goals_end) {
50+
gtsam::Values result;
51+
const double denominator =
52+
static_cast<double>(interval.k_end - interval.k_start);
53+
for (size_t k = interval.k_start; k <= interval.k_end; ++k) {
54+
const double alpha =
55+
denominator > 0.0
56+
? static_cast<double>(k - interval.k_start) / denominator
57+
: 0.0;
58+
const auto goals =
59+
InterpolateContactGoals(contact_goals_start, contact_goals_end, alpha);
60+
result.insert(kinematics.inverse(Slice(k), robot, goals));
61+
}
62+
return result;
63+
}
64+
65+
} // namespace
66+
2967
TEST(Interval, InverseKinematics) {
3068
// Load robot and establish contact/goal pairs
3169
// TODO(frank): the goals for contact will differ for a Interval vs Slice.
@@ -73,7 +111,7 @@ TEST(Interval, Interpolate) {
73111
parameters.method = OptimizationParameters::Method::SOFT_CONSTRAINTS;
74112
Kinematics kinematics(parameters);
75113
auto result1 = kinematics.inverse(Slice(5), robot, contact_goals);
76-
auto result2 = kinematics.inverse(Slice(9), robot, contact_goals);
114+
auto result2 = kinematics.inverse(Slice(9), robot, contact_goals2);
77115

78116
// Create a kinematic trajectory over timesteps 5, 6, 7, 8, 9 that
79117
// interpolates between goal configurations at timesteps 5 and 9.
@@ -85,6 +123,37 @@ TEST(Interval, Interpolate) {
85123
EXPECT(assert_equal(Pose(result2, 0, 9), Pose(result, 0, 9)));
86124
}
87125

126+
TEST(Interval, InterpolateMatchesManualInverseLoop) {
127+
using namespace contact_goals_example;
128+
129+
// Move RF foot by 10 cm in +x at the end of interval.
130+
auto contact_goals2 = contact_goals;
131+
contact_goals2[2] = {{RF, contact_in_com}, {0.4, -0.16, -0.2}};
132+
133+
KinematicsParameters parameters;
134+
parameters.method = OptimizationParameters::Method::AUGMENTED_LAGRANGIAN;
135+
Kinematics kinematics(parameters);
136+
137+
const Interval interval(5, 9);
138+
const auto manual_result = ManualInterpolateByInverse(
139+
kinematics, interval, robot, contact_goals, contact_goals2);
140+
const auto interpolate_result =
141+
kinematics.interpolate(interval, robot, contact_goals, contact_goals2);
142+
143+
// Interpolate should match an explicit per-k call to inverse on interpolated
144+
// goal points.
145+
constexpr double tol = 1e-5;
146+
for (size_t k = interval.k_start; k <= interval.k_end; ++k) {
147+
for (const auto& joint : robot.joints()) {
148+
const auto key = JointAngleKey(joint->id(), k);
149+
EXPECT(manual_result.exists(key));
150+
EXPECT(interpolate_result.exists(key));
151+
EXPECT_DOUBLES_EQUAL(manual_result.at<double>(key),
152+
interpolate_result.at<double>(key), tol);
153+
}
154+
}
155+
}
156+
88157
int main() {
89158
TestResult tr;
90159
return TestRegistry::runAllTests(tr);

‎python/notebooks/inverse_kinematics.ipynb‎

Lines changed: 94 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -1,53 +1,31 @@
11
{
2-
"metadata": {
3-
"language_info": {
4-
"codemirror_mode": {
5-
"name": "ipython",
6-
"version": 3
7-
},
8-
"file_extension": ".py",
9-
"mimetype": "text/x-python",
10-
"name": "python",
11-
"nbconvert_exporter": "python",
12-
"pygments_lexer": "ipython3",
13-
"version": "3.9.6"
14-
},
15-
"orig_nbformat": 2,
16-
"kernelspec": {
17-
"name": "python3",
18-
"display_name": "Python 3.9.6 64-bit ('base': conda)"
19-
},
20-
"interpreter": {
21-
"hash": "95ec9ec1504d83f612128e0fb229072f90bbb4cb09d9d5d93b5dd26e0ca2cfd1"
22-
}
23-
},
24-
"nbformat": 4,
25-
"nbformat_minor": 2,
262
"cells": [
273
{
284
"cell_type": "code",
295
"execution_count": null,
6+
"metadata": {},
7+
"outputs": [],
308
"source": [
319
"import gtdynamics as gtd\n",
3210
"from gtdynamics import ContactGoal, PointOnLink, Slice, Interval\n",
3311
"from gtsam import Pose3, Point3"
34-
],
35-
"outputs": [],
36-
"metadata": {}
12+
]
3713
},
3814
{
3915
"cell_type": "code",
4016
"execution_count": null,
17+
"metadata": {},
18+
"outputs": [],
4119
"source": [
4220
"# Load the vision 60 quadruped by Ghost robotics: https://youtu.be/wrBNJKZKg10\n",
4321
"robot = gtd.CreateRobotFromFile(gtd.URDF_PATH + \"/vision60.urdf\");"
44-
],
45-
"outputs": [],
46-
"metadata": {}
22+
]
4723
},
4824
{
4925
"cell_type": "code",
5026
"execution_count": null,
27+
"metadata": {},
28+
"outputs": [],
5129
"source": [
5230
"# feet\n",
5331
"contact_in_com =(0.14, 0, 0)\n",
@@ -64,55 +42,117 @@
6442
" ContactGoal(RH, [-0.4, -0.16, 0])\n",
6543
" ]\n",
6644
"print(contact_goals)"
67-
],
68-
"outputs": [],
69-
"metadata": {}
45+
]
7046
},
7147
{
7248
"cell_type": "code",
7349
"execution_count": null,
74-
"source": [
75-
"kinematics = gtd.Kinematics()\n",
76-
"result = kinematics.inverse(Slice(4), robot, contact_goals)\n",
77-
"print(result)"
78-
],
50+
"metadata": {},
7951
"outputs": [],
80-
"metadata": {}
52+
"source": [
53+
"parameters = gtd.KinematicsParameters()\n",
54+
"# parameters.method = gtd.KinematicsParameters.Method.SOFT_CONSTRAINTS # not yet wrapped\n",
55+
"kinematics = gtd.Kinematics(parameters)\n",
56+
"result = kinematics.inverse(Slice(4), robot, contact_goals)"
57+
]
8158
},
8259
{
8360
"cell_type": "code",
8461
"execution_count": null,
62+
"metadata": {},
63+
"outputs": [],
8564
"source": [
8665
"for goal in contact_goals:\n",
8766
" print(goal.link().name(), goal.satisfied(result,k=4,tol=1e-3))"
88-
],
89-
"outputs": [],
90-
"metadata": {}
67+
]
9168
},
9269
{
9370
"cell_type": "code",
9471
"execution_count": null,
72+
"metadata": {},
73+
"outputs": [],
9574
"source": [
9675
"# interpolate\n",
76+
"\n",
9777
"contact_goals2 = [\n",
9878
" ContactGoal(LH, [-0.4, 0.16, 0]),\n",
9979
" ContactGoal(LF, [0.3, 0.16, 0]),\n",
100-
" ContactGoal(RF, [0.4, -0.16, 0]), # 10 cm on!\n",
101-
" ContactGoal(RH, [-0.4, -0.16, 0])\n",
102-
" ]\n",
103-
"interpolated_values = kinematics.interpolate(Interval(4,8), robot, contact_goals, contact_goals2)"
104-
],
105-
"outputs": [],
106-
"metadata": {}
80+
" ContactGoal(RF, [0.3 + 0.10, -0.16, 0]), # 10 cm on!\n",
81+
" ContactGoal(RH, [-0.4, -0.16, 0]),\n",
82+
"]\n",
83+
"\n",
84+
"k_start, k_end = 4, 8\n",
85+
"interpolated_values = kinematics.interpolate(\n",
86+
" Interval(k_start, k_end), robot, contact_goals, contact_goals2\n",
87+
")\n",
88+
"# interpolated_values.print(\"interpolated_values\", gtd.GTDKeyFormatter)"
89+
]
10790
},
10891
{
10992
"cell_type": "code",
11093
"execution_count": null,
94+
"metadata": {},
95+
"outputs": [],
11196
"source": [
112-
"print(interpolated_values)"
113-
],
97+
"import plotly.graph_objects as go\n",
98+
"\n",
99+
"fig = go.Figure()\n",
100+
"\n",
101+
"for joint in robot.joints():\n",
102+
" joint_id = joint.id()\n",
103+
" ks = []\n",
104+
" angles = []\n",
105+
" for k in range(k_start, k_end + 1):\n",
106+
" key = gtd.JointAngleKey(joint_id, k)\n",
107+
" if interpolated_values.exists(key):\n",
108+
" ks.append(k)\n",
109+
" angles.append(interpolated_values.atDouble(key))\n",
110+
"\n",
111+
" if angles:\n",
112+
" fig.add_trace(go.Scatter(x=ks, y=angles, mode=\"lines+markers\", name=joint.name()))\n",
113+
"\n",
114+
"fig.update_layout(\n",
115+
" title=\"Interpolated Joint Angles\",\n",
116+
" xaxis_title=\"k\",\n",
117+
" yaxis_title=\"angle (rad)\",\n",
118+
" template=\"plotly_white\",\n",
119+
")\n",
120+
"fig.show()"
121+
]
122+
},
123+
{
124+
"cell_type": "code",
125+
"execution_count": null,
126+
"metadata": {},
114127
"outputs": [],
115-
"metadata": {}
128+
"source": [
129+
"for k in range(k_start, k_end + 1):\n",
130+
" print(f\"--- k={k} ---\")\n",
131+
" for goal in contact_goals:\n",
132+
" print(goal.link().name(), goal.satisfied(interpolated_values,k=k,tol=1e-3))"
133+
]
116134
}
117-
]
118-
}
135+
],
136+
"metadata": {
137+
"kernelspec": {
138+
"display_name": "py312",
139+
"language": "python",
140+
"name": "python3"
141+
},
142+
"language_info": {
143+
"codemirror_mode": {
144+
"name": "ipython",
145+
"version": 3
146+
},
147+
"file_extension": ".py",
148+
"mimetype": "text/x-python",
149+
"name": "python",
150+
"nbconvert_exporter": "python",
151+
"pygments_lexer": "ipython3",
152+
"version": "3.12.6"
153+
},
154+
"orig_nbformat": 2
155+
},
156+
"nbformat": 4,
157+
"nbformat_minor": 2
158+
}

0 commit comments

Comments
 (0)