InverseKinematicsExampleExpressions.cpp
Go to the documentation of this file.
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 
19 #include <gtsam/geometry/Pose2.h>
25 #include <gtsam/slam/expressions.h>
26 
27 #include <cmath>
28 
29 using namespace std;
30 using namespace gtsam;
31 
32 // Scalar multiplication of a vector, with derivatives.
33 inline Vector3 scalarMultiply(const double& s, const Vector3& v,
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.
42 inline Vector3_ operator*(const Double_& s, const Vector3_& v) {
43  return Vector3_(&scalarMultiply, s, v);
44 }
45 
46 // Expression version of Pose2::Expmap
47 inline Pose2_ Expmap(const Vector3_& xi) { return Pose2_(&Pose2::Expmap, xi); }
48 
49 // Main function
50 int 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(1)), q2(Q(2)), q3(Q(3));
60 
61  // Forward kinematics expression as product of exponentials
62  Pose2_ l1Zl1 = Expmap(q1 * Vector3_(xi1));
63  Pose2_ l2Zl2 = Expmap(q2 * Vector3_(xi2));
64  Pose2_ l3Zl3 = Expmap(q3 * Vector3_(xi3));
65  Pose2_ forward = compose(compose(l1Zl1, l2Zl2), compose(l3Zl3, Pose2_(sXt0)));
66 
67  // Create a factor graph with a a single expression factor.
69  Pose2 desiredEndEffectorPose(3, 2, 0);
70  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
71  graph.addExpressionFactor(forward, desiredEndEffectorPose, model);
72 
73  // Create initial estimate
75  initial.insert(Q(1), 0.1);
76  initial.insert(Q(2), 0.2);
77  initial.insert(Q(3), 0.3);
78  initial.print("\nInitial Estimate:\n"); // print
79  GTSAM_PRINT(forward.value(initial));
80 
81  // Optimize the initial values using a Levenberg-Marquardt nonlinear optimizer
83  params.setlambdaInitial(1e6);
84  LevenbergMarquardtOptimizer optimizer(graph, initial, params);
85  Values result = optimizer.optimize();
86  result.print("Final Result:\n");
87 
88  GTSAM_PRINT(forward.value(result));
89 
90  return 0;
91 }
Vector3_ operator*(const Double_ &s, const Vector3_ &v)
Point2 q2
Definition: testPose2.cpp:753
Quaternion Q
virtual const Values & optimize()
Point2 q1
Definition: testPose2.cpp:753
Eigen::Vector3d Vector3
Definition: Vector.h:43
noiseModel::Diagonal::shared_ptr model
void insert(Key j, const Value &val)
Definition: Values.cpp:140
ArrayXcf v
Definition: Cwise_arg.cpp:1
#define M_PI
Definition: main.h:78
Pose2_ Expmap(const Vector3_ &xi)
Values initial
Definition: Half.h:150
NonlinearFactorGraph graph
Expression< Pose2 > Pose2_
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
Values result
Factor graph that supports adding ExpressionFactors directly.
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
RealScalar s
Point2 q3
Definition: testPose2.cpp:753
static SmartStereoProjectionParams params
Vector xi
Definition: testPose2.cpp:150
traits
Definition: chartTesting.h:28
int main(int argc, char **argv)
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:77
#define GTSAM_PRINT(x)
Definition: Testable.h:41
T value(const Values &values, boost::optional< std::vector< Matrix > & > H=boost::none) const
Return value and optional derivatives, reverse AD version Notes: this is not terribly efficient...
void addExpressionFactor(const Expression< T > &h, const T &z, const SharedNoiseModel &R)
A class for computing marginals in a NonlinearFactorGraph.
2D Pose
Vector3 scalarMultiply(const double &s, const Vector3 &v, OptionalJacobian< 3, 1 > Hs, OptionalJacobian< 3, 3 > Hv)
Expression< Vector3 > Vector3_


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:16