00001 /* **-*-c++-*-************************************************************** 00002 Copyright (C)2002 David Jung <opensim@pobox.com> 00003 00004 This program/file is free software; you can redistribute it and/or modify 00005 it under the terms of the GNU General Public License as published by 00006 the Free Software Foundation; either version 2 of the License, or 00007 (at your option) any later version. 00008 00009 This program is distributed in the hope that it will be useful, 00010 but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 GNU General Public License for more details. (http://www.gnu.org) 00013 00014 You should have received a copy of the GNU General Public License 00015 along with this program; if not, write to the Free Software 00016 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00017 00018 $Id: OldIKOR 1036 2004-02-11 20:48:55Z jungd $ 00019 $Revision: 1.5 $ 00020 $Date: 2004-02-11 15:48:55 -0500 (Wed, 11 Feb 2004) $ 00021 $Author: jungd $ 00022 00023 ****************************************************************************/ 00024 00025 #ifndef _ROBOT_CONTROL_KINEMATICS_OLDIKOR_ 00026 #define _ROBOT_CONTROL_KINEMATICS_OLDIKOR_ 00027 00028 #include <robot/control/kinematics/kinematics> 00029 00030 #include <robot/control/kinematics/InverseKinematicsSolver> 00031 #include <robot/KinematicChain> 00032 00033 00034 namespace robot { 00035 namespace control { 00036 namespace kinematics { 00037 00038 00039 /// wrapper for old IKORv2 inverse kinematics code. 00040 class OldIKOR : public InverseKinematicsSolver 00041 { 00042 public: 00043 OldIKOR(const robot::KinematicChain& chain, bool platformActive=false); 00044 00045 virtual String className() const { return String("OldIKOR"); } 00046 00047 /// Solve inverse kinematics. 00048 /** 00049 * (see InverseKinematicsSolver for parameters) 00050 * @param solver ignored. The old IKORv2 FSP solver is (implicitly) used. 00051 */ 00052 virtual Vector solve(const Vector& dx, const Vector& x, const Vector& q, 00053 const base::Matrix& J, 00054 OptimizationMethod optMethod = Lagrangian, 00055 OptimizationCriterion optCriterion = LeastNorm, 00056 OptimizationConstraints optConstraints = OptimizationConstraints(JointLimits), 00057 base::Orient::Representation orientationRepresentation = base::Orient::EulerRPY); 00058 00059 /// Set implementation specific parameters. Unknown names will throw a std::invald_argument exception 00060 /** Valid parameters: name value description 00061 * none currently 00062 */ 00063 virtual void setParameter(const String& name, Real value); 00064 00065 protected: 00066 robot::KinematicChain chain; 00067 bool platformActive; 00068 00069 }; 00070 00071 00072 } // namespace kinematics 00073 } // namespace control 00074 } // namespace robot 00075 00076 #endif