diff --git a/cython/gtsam/examples/PlanarManipulatorExample.py b/cython/gtsam/examples/PlanarManipulatorExample.py index 73959b6c5..e42ae09d7 100644 --- a/cython/gtsam/examples/PlanarManipulatorExample.py +++ b/cython/gtsam/examples/PlanarManipulatorExample.py @@ -115,7 +115,7 @@ class ThreeLinkArm(object): l1Zl1 = expmap(0.0, 0.0, q[0]) l2Zl2 = expmap(0.0, self.L1, q[1]) - l3Zl3 = expmap(0.0, 7.0, q[2]) + l3Zl3 = expmap(0.0, self.L1+self.L2, q[2]) return compose(l1Zl1, l2Zl2, l3Zl3, self.sXt0) def ik(self, sTt_desired, e=1e-9): @@ -297,12 +297,18 @@ def run_example(): """ Use trajectory interpolation and then trajectory tracking a la Murray to move a 3-link arm on a straight line. """ + # Create arm arm = ThreeLinkArm() + + # Get initial pose using forward kinematics q = np.radians(vector3(30, -30, 45)) sTt_initial = arm.fk(q) + + # Create interpolated trajectory in task space to desired goal pose sTt_goal = Pose2(2.4, 4.3, math.radians(0)) poses = trajectory(sTt_initial, sTt_goal, 50) + # Setup figure and plot initial pose fignum = 0 fig = plt.figure(fignum) axes = fig.gca() @@ -310,6 +316,8 @@ def run_example(): axes.set_ylim(0, 10) gtsam_plot.plot_pose2(fignum, arm.fk(q)) + # For all poses in interpolated trajectory, calculate dq to move to next pose. + # We do this by calculating the local Jacobian J and doing dq = inv(J)*delta(sTt, pose). for pose in poses: sTt = arm.fk(q) error = delta(sTt, pose) diff --git a/examples/InverseKinematicsExampleExpressions.cpp b/examples/InverseKinematicsExampleExpressions.cpp new file mode 100644 index 000000000..9e86886e7 --- /dev/null +++ b/examples/InverseKinematicsExampleExpressions.cpp @@ -0,0 +1,92 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file InverseKinematicsExampleExpressions.cpp + * @brief Implement inverse kinematics on a three-link arm using expressions. + * @date April 15, 2019 + * @author Frank Dellaert + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; + +// Scalar multiplication of a vector, with derivatives. +inline Vector3 scalarMultiply(const double& s, const Vector3& v, + OptionalJacobian<3, 1> Hs, + OptionalJacobian<3, 3> Hv) { + if (Hs) *Hs = v; + if (Hv) *Hv = s * I_3x3; + return s * v; +} + +// Expression version of scalar product, using above function. +inline Vector3_ operator*(const Double_& s, const Vector3_& v) { + return Vector3_(&scalarMultiply, s, v); +} + +// Expression version of Pose2::Expmap +inline Pose2_ Expmap(const Vector3_& xi) { return Pose2_(&Pose2::Expmap, xi); } + +// Main function +int main(int argc, char** argv) { + // Three-link planar manipulator specification. + const double L1 = 3.5, L2 = 3.5, L3 = 2.5; // link lengths + const Pose2 sXt0(0, L1 + L2 + L3, M_PI / 2); // end-effector pose at rest + const Vector3 xi1(0, 0, 1), xi2(L1, 0, 1), + xi3(L1 + L2, 0, 1); // screw axes at rest + + // Create Expressions for unknowns + using symbol_shorthand::Q; + Double_ q1(Q(1)), q2(Q(2)), q3(Q(3)); + + // Forward kinematics expression as product of exponentials + Pose2_ l1Zl1 = Expmap(q1 * Vector3_(xi1)); + Pose2_ l2Zl2 = Expmap(q2 * Vector3_(xi2)); + Pose2_ l3Zl3 = Expmap(q3 * Vector3_(xi3)); + Pose2_ forward = compose(compose(l1Zl1, l2Zl2), compose(l3Zl3, Pose2_(sXt0))); + + // Create a factor graph with a a single expression factor. + ExpressionFactorGraph graph; + Pose2 desiredEndEffectorPose(3, 2, 0); + auto model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); + graph.addExpressionFactor(forward, desiredEndEffectorPose, model); + + // Create initial estimate + Values initial; + initial.insert(Q(1), 0.1); + initial.insert(Q(2), 0.2); + initial.insert(Q(3), 0.3); + initial.print("\nInitial Estimate:\n"); // print + GTSAM_PRINT(forward.value(initial)); + + // Optimize the initial values using a Levenberg-Marquardt nonlinear optimizer + LevenbergMarquardtParams params; + params.setlambdaInitial(1e6); + LevenbergMarquardtOptimizer optimizer(graph, initial, params); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + GTSAM_PRINT(forward.value(result)); + + return 0; +}