robot/control/kinematics/AnalyticLagrangianFSBetaOptimizer

Go to the documentation of this file.

/* **-*-c++-*-**************************************************************
  Copyright (C)2002 David Jung <opensim@pobox.com>

  This program/file is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.
  
  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details. (http://www.gnu.org)
  
  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  
  $Id: AnalyticLagrangianFSBetaOptimizer 1036 2004-02-11 20:48:55Z jungd $
  $Revision: 1.7 $
  $Date: 2004-02-11 15:48:55 -0500 (Wed, 11 Feb 2004) $
  $Author: jungd $
 
****************************************************************************/

#ifndef _ROBOT_CONTROL_KINEMATICS_ANALYTICLAGRANGIANFSBETAOPTIMIZER_
#define _ROBOT_CONTROL_KINEMATICS_ANALYTICLAGRANGIANFSBETAOPTIMIZER_

#include <robot/control/kinematics/kinematics>

#include <robot/control/kinematics/LagrangianOptimizer>
#include <robot/control/kinematics/BetaFormConstraints>


namespace robot {
namespace control {
namespace kinematics {


/// A constrained optimizer that uses an analytic Lagrangian solution technique
/**
 * Analytic Lagrangian optimizer in Full-Space with Beta form constraints.
 * The paper below details a optimizaiton method that provides an analytic solution
 *  for a Lagrangian-type problem, given the objective function and constraints are
 *  of a particular form.
 * The objective form is represented via the ReferenceOpVectorFormObjective class and 
 * the form of the constraints via the BetaFormConstraints class.
 *
 * see 1. "Resolving Kinematic Redundancy with Constraints 
 * Using the FSP (Full Space Parameterization) Approach", Francois G. Pin & Faithlyn A. Tulloch,
 * Proceedings of the 1996 IEEE International Conference on Robotics and Automation.
 *
 * for the formulation of the non-holonomic constraint solution, 
 * see 2. "Motion Planning for Mobile Manipulators with a Non-Holonomic Constraint
 * Using the FSP (Full Space Parameterization) Method", Francois G. Pin, Kristi A. Morgansen,
 * Faithlyn A. Tulloch, Charles J. Hacker and Kathryn B. Gower,
 * Journal of Robotic Systems 13(11), 723-736 (1996).
 */
class AnalyticLagrangianFSBetaOptimizer : public LagrangianOptimizer
{
public:
  AnalyticLagrangianFSBetaOptimizer() 
    : gs(0), nullSpace(false) {}

  virtual String className() const { return String("AnalyticLagrangianFSBetaOptimizer"); }

  /// set the gs (Matrix of column vectors that span the full-space)
  virtual void setGs(const Matrix& gs) { this->gs = &gs; }

  /// indicate if a solution is sought in the null-space
  void setSolveForNullspace(bool nullSpace) { this->nullSpace=nullSpace; }

  /// optimize the objective with the given constraints
  virtual Vector optimize(ref<const Objective> objective, ref<const Constraints> constraints) const;

protected:
  Matrix const *gs; ///< matrix of column vectors that span the solition space of dq's (from a full-space solver)
  bool nullSpace;

  mutable Vector o; // convenient zero Vector
  mutable Vector e; // convenient Vector of 1s

  /// compute t Vector
  Vector calct(const Matrix& Ginv, const Vector& H, ref<const BetaFormConstraints> betaConstraints, bool nullSpace) const;

};


} // namespace kinematics
} // namespace control
} // namespace robot

#endif

---