From bd75185a8ff0862cebe9b97e5dc19fa6eb583d55 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 3 Mar 2016 15:26:17 -0500 Subject: [PATCH 1/2] Add more description on InverseKinematics::solve() --- dart/dynamics/HierarchicalIK.cpp | 1 + dart/dynamics/InverseKinematics.cpp | 1 + dart/dynamics/InverseKinematics.h | 40 ++++++++++++++++++++++++----- 3 files changed, 36 insertions(+), 6 deletions(-) diff --git a/dart/dynamics/HierarchicalIK.cpp b/dart/dynamics/HierarchicalIK.cpp index e96fe991c7e9e..2dfa618633f89 100644 --- a/dart/dynamics/HierarchicalIK.cpp +++ b/dart/dynamics/HierarchicalIK.cpp @@ -39,6 +39,7 @@ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/EndEffector.h" #include "dart/dynamics/Skeleton.h" +#include "dart/optimizer/GradientDescentSolver.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp index 03481d013985d..8685602a5c61a 100644 --- a/dart/dynamics/InverseKinematics.cpp +++ b/dart/dynamics/InverseKinematics.cpp @@ -38,6 +38,7 @@ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/DegreeOfFreedom.h" #include "dart/dynamics/SimpleFrame.h" +#include "dart/optimizer/GradientDescentSolver.h" namespace dart { namespace dynamics { diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index a0a41ad97324e..82e3b26eb08df 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -47,7 +47,6 @@ #include "dart/common/Subject.h" #include "dart/math/Geometry.h" #include "dart/optimizer/Solver.h" -#include "dart/optimizer/GradientDescentSolver.h" #include "dart/optimizer/Problem.h" #include "dart/optimizer/Function.h" #include "dart/dynamics/SmartPointer.h" @@ -67,7 +66,9 @@ const double DefaultIKLinearWeight = 1.0; /// The InverseKinematics class provides a convenient way of setting up an IK /// optimization problem. It generates constraint functions based on the /// specified InverseKinematics::ErrorMethod and -/// InverseKinematics::GradientMethod. +/// InverseKinematics::GradientMethod. The optimization problem is then solved +/// by any classes derived from optimizer::Solver class. The default solver is +/// optimizer::GradientDescentSolver. /// /// It also provides a convenient way of specifying a configuration space /// objective and a null space objective. It is also possible to fully customize @@ -91,10 +92,37 @@ class InverseKinematics : public common::Subject /// Virtual destructor virtual ~InverseKinematics(); - /// Solve the IK Problem. By default, the Skeleton itself will retain the - /// solved joint positions. If you pass in false for _applySolution, then the - /// joint positions will be returned to their original positions after the - /// problem is solved. + /// Solve the IK Problem. + /// + /// The initial guess for the IK optimization problem is the current joint + /// positions of the target system. If the iterative solver fails to find a + /// successive solution, it attempts more to solve the problem with other seed + /// configurations or random configurations if enough seed is not provided. + /// + /// Here is the pseudocode as described above: + /// + /// \code + /// attempts <- 0 + /// initial_guess <- current_joint_positions + /// while attempts <= max_attempts: + /// result <- solve(initial_guess) + /// if result = success: + /// return + /// else: + /// attempts <- attempts + 1 + /// if attempts <= num_seed: + /// initial_guess <- seed[attempts - 1] + /// else: + /// initial_guess <- random_configuration // within the bounds + /// \endcode + /// + /// By default, the max_attempts is 1 and the list of seed is empty, which can + /// be specified by acceccing optimizer::GradientDescentSolver calling + /// InverseKinematics::getSolver(). + /// + /// By default, the Skeleton itself will retain the solved joint positions. + /// If you pass in false for _applySolution, then the joint positions will be + /// returned to their original positions after the problem is solved. bool solve(bool _applySolution = true); /// Same as solve(bool), but the positions vector will be filled with the From a7a0b4e6cc7d161189ce2a60b26da6316c7f8b5b Mon Sep 17 00:00:00 2001 From: "M.X. Grey" Date: Sat, 16 Apr 2016 01:59:04 -0400 Subject: [PATCH 2/2] added more commentary to IK::solve() --- dart/dynamics/InverseKinematics.h | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/InverseKinematics.h b/dart/dynamics/InverseKinematics.h index 82e3b26eb08df..d68552987bb41 100644 --- a/dart/dynamics/InverseKinematics.h +++ b/dart/dynamics/InverseKinematics.h @@ -116,13 +116,24 @@ class InverseKinematics : public common::Subject /// initial_guess <- random_configuration // within the bounds /// \endcode /// - /// By default, the max_attempts is 1 and the list of seed is empty, which can - /// be specified by acceccing optimizer::GradientDescentSolver calling - /// InverseKinematics::getSolver(). + /// By default, the max_attempts is 1, but this can be changed by calling + /// InverseKinematics::getSolver() and casting the SolverPtr to an + /// optimizer::GradientDescentSolver (unless you have changed the Solver type) + /// and then calling GradientDescentSolver::setMaxAttempts(size_t). + /// + /// By default, the list of seeds is empty, but they can be added by calling + /// InverseKinematics::getProblem() and then using + /// Problem::addSeed(Eigen::VectorXd). /// /// By default, the Skeleton itself will retain the solved joint positions. /// If you pass in false for _applySolution, then the joint positions will be /// returned to their original positions after the problem is solved. + /// + /// Calling this function will automatically call Position::setLowerBounds(~) + /// and Position::setUpperBounds(~) with the lower/upper position bounds of + /// the corresponding Degrees Of Freedom. Problem::setDimension(~) will be + /// taken care of automatically, and Problem::setInitialGuess(~) will be + /// called with the current positions of the Degrees Of Freedom. bool solve(bool _applySolution = true); /// Same as solve(bool), but the positions vector will be filled with the