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);
85  Values result = optimizer.optimize();
86  result.print("Final Result:\n");
87 
88  GTSAM_PRINT(forward.value(result));
89 
90  return 0;
91 }
Pose2.h
2D Pose
gtsam::NonlinearOptimizer::optimize
virtual const Values & optimize()
Definition: NonlinearOptimizer.h:98
s
RealScalar s
Definition: level1_cplx_impl.h:126
screwPose2::xi
Vector xi
Definition: testPose2.cpp:148
LevenbergMarquardtOptimizer.h
A nonlinear optimizer that uses the Levenberg-Marquardt trust-region scheme.
initial
Values initial
Definition: OdometryOptimize.cpp:2
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
gtsam::Pose2_
Expression< Pose2 > Pose2_
Definition: slam/expressions.h:24
gtsam::ExpressionFactorGraph
Definition: ExpressionFactorGraph.h:29
vanilla::params
static const SmartProjectionParams params
Definition: smartFactorScenarios.h:69
result
Values result
Definition: OdometryOptimize.cpp:8
Q
Quaternion Q
Definition: testQuaternion.cpp:27
gtsam::Expression
Definition: Expression.h:47
Expmap
Pose2_ Expmap(const Vector3_ &xi)
Definition: InverseKinematicsExampleExpressions.cpp:47
expressions.h
Common expressions, both linear and non-linear.
BetweenFactor.h
ExpressionFactorGraph.h
Factor graph that supports adding ExpressionFactors directly.
gtsam::LevenbergMarquardtOptimizer
Definition: LevenbergMarquardtOptimizer.h:35
GTSAM_PRINT
#define GTSAM_PRINT(x)
Definition: Testable.h:43
main
int main(int argc, char **argv)
Definition: InverseKinematicsExampleExpressions.cpp:50
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
gtsam::Values::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
Definition: Values.cpp:66
gtsam
traits
Definition: SFMdata.h:40
Marginals.h
A class for computing marginals in a NonlinearFactorGraph.
forward
Definition: testScenarioRunner.cpp:79
gtsam::OptionalJacobian
Definition: OptionalJacobian.h:38
gtsam::Values
Definition: Values.h:65
expressions.h
Common expressions for solving geometry/slam/sfm problems.
gtsam::testing::compose
T compose(const T &t1, const T &t2)
Definition: lieProxies.h:39
std
Definition: BFloat16.h:88
v
Array< int, Dynamic, 1 > v
Definition: Array_initializer_list_vector_cxx11.cpp:1
gtsam::LevenbergMarquardtParams
Definition: LevenbergMarquardtParams.h:35
initial
Definition: testScenarioRunner.cpp:148
gtsam::Vector3_
Expression< Vector3 > Vector3_
Definition: nonlinear/expressions.h:31
M_PI
#define M_PI
Definition: mconf.h:117
graph
NonlinearFactorGraph graph
Definition: doc/Code/OdometryExample.cpp:2
scalarMultiply
Vector3 scalarMultiply(const double &s, const Vector3 &v, OptionalJacobian< 3, 1 > Hs, OptionalJacobian< 3, 3 > Hv)
Definition: InverseKinematicsExampleExpressions.cpp:33
operator*
Vector3_ operator*(const Double_ &s, const Vector3_ &v)
Definition: InverseKinematicsExampleExpressions.cpp:42
gtsam::NonlinearFactorGraph::addExpressionFactor
void addExpressionFactor(const SharedNoiseModel &R, const T &z, const Expression< T > &h)
Definition: NonlinearFactorGraph.h:187
gtsam::Pose2
Definition: Pose2.h:39


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:32:51