1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/**
13 * @file InverseKinematicsExampleExpressions.cpp
14 * @brief Implement inverse kinematics on a three-link arm using expressions.
15 * @date April 15, 2019
16 * @author Frank Dellaert
17 */
18
19#include <gtsam/geometry/Pose2.h>
20#include <gtsam/nonlinear/ExpressionFactorGraph.h>
21#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
22#include <gtsam/nonlinear/Marginals.h>
23#include <gtsam/nonlinear/expressions.h>
24#include <gtsam/slam/BetweenFactor.h>
25#include <gtsam/slam/expressions.h>
26
27#include <cmath>
28
29using namespace std;
30using namespace gtsam;
31
32// Scalar multiplication of a vector, with derivatives.
33inline Vector3 scalarMultiply(const double& s, const Vector3& v,
34 OptionalJacobian<3, 1> Hs,
35 OptionalJacobian<3, 3> Hv) {
36 if (Hs) *Hs = v;
37 if (Hv) *Hv = s * I_3x3;
38 return s * v;
39}
40
41// Expression version of scalar product, using above function.
42inline Vector3_ operator*(const Double_& s, const Vector3_& v) {
43 return Vector3_(&scalarMultiply, s, v);
44}
45
46// Expression version of Pose2::Expmap
47inline Pose2_ Expmap(const Vector3_& xi) { return Pose2_(&Pose2::Expmap, xi); }
48
49// Main function
50int main(int argc, char** argv) {
51 // Three-link planar manipulator specification.
52 const double L1 = 3.5, L2 = 3.5, L3 = 2.5; // link lengths
53 const Pose2 sXt0(0, L1 + L2 + L3, M_PI / 2); // end-effector pose at rest
54 const Vector3 xi1(0, 0, 1), xi2(L1, 0, 1),
55 xi3(L1 + L2, 0, 1); // screw axes at rest
56
57 // Create Expressions for unknowns
58 using symbol_shorthand::Q;
59 Double_ q1(Q(j: 1)), q2(Q(j: 2)), q3(Q(j: 3));
60
61 // Forward kinematics expression as product of exponentials
62 Pose2_ l1Zl1 = Expmap(xi: q1 * Vector3_(xi1));
63 Pose2_ l2Zl2 = Expmap(xi: q2 * Vector3_(xi2));
64 Pose2_ l3Zl3 = Expmap(xi: q3 * Vector3_(xi3));
65 Pose2_ forward = compose(t1: compose(t1: l1Zl1, t2: l2Zl2), t2: compose(t1: l3Zl3, t2: Pose2_(sXt0)));
66
67 // Create a factor graph with a a single expression factor.
68 ExpressionFactorGraph graph;
69 Pose2 desiredEndEffectorPose(3, 2, 0);
70 auto model = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.2, 0.2, 0.1));
71 graph.addExpressionFactor(h: forward, z: desiredEndEffectorPose, R: model);
72
73 // Create initial estimate
74 Values initial;
75 initial.insert(j: Q(j: 1), val: 0.1);
76 initial.insert(j: Q(j: 2), val: 0.2);
77 initial.insert(j: Q(j: 3), val: 0.3);
78 initial.print(str: "\nInitial Estimate:\n"); // print
79 GTSAM_PRINT(forward.value(initial));
80
81 // Optimize the initial values using a Levenberg-Marquardt nonlinear optimizer
82 LevenbergMarquardtParams params;
83 params.setlambdaInitial(1e6);
84 LevenbergMarquardtOptimizer optimizer(graph, initial, params);
85 Values result = optimizer.optimize();
86 result.print(str: "Final Result:\n");
87
88 GTSAM_PRINT(forward.value(result));
89
90 return 0;
91}
92