robot/sim/IKORTest

Go to the documentation of this file.

/* **-*-c++-*-**************************************************************
  Copyright (C)2003 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: IKORTest 1033 2004-02-11 20:47:52Z jungd $
  $Revision: 1.5 $
  $Date: 2004-02-11 15:47:52 -0500 (Wed, 11 Feb 2004) $
  $Author: jungd $
 
****************************************************************************/

#ifndef _ROBOT_SIM_IKORTEST_
#define _ROBOT_SIM_IKORTEST_

#include <robot/sim/sim>

#include <base/Externalizable>
#include <base/Named>
#include <base/Vector>
#include <base/Time>
#include <base/Trajectory>
#include <base/PathName>

#include <robot/KinematicChain>
#include <robot/sim/TestBasicEnvironment>
#include <robot/sim/SimulatedBasicEnvironment>
#include <robot/control/kinematics/InverseKinematicsSolver>
#include <robot/control/kinematics/FullSpaceSolver>
#include <robot/control/kinematics/BetaFormConstraints>
#include <robot/control/kinematics/IKOR>

#include <set>


namespace robot {
namespace sim {


/// Test case for the IKOR tester
/// Holds the description of a test to be run for IKOR (and optionally the results) -   
/// environment (robot, tools, obstacles); trajectory sequences etc.
/// \todo later, rather than instantiating a TestBasicEnvironment to externalize in, it should be
///       left to the tester to instantiate.  This will require an EnvironmentDescription heirarchy
///       to mirror the Environment classes (in robot::sim)
class IKORTest :  public base::Named, public base::Externalizable, public base::ReferencedObject
{
public:
  IKORTest(ref<base::VFile> testSpecification,
           ref<base::VFileSystem> fs, ref<base::Cache> cache);

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

  
  /// an individual test 
  /// IKORTest comprises a sequence of these Tests.  The environment is persistent
  ///  accross the tests
  class Test : public base::Named, public base::Externalizable
  {
  public:
    Test(const String& name = "");

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

    
    bool initialConfigSpecified; ///< if true, use q as the initial joint configuration, otherwise use the currrent configuration
    base::Vector initq;      ///< initial joint configuration
    base::Trajectory traj;   ///< trajectory/path for manipulator to follow

    bool toolAttached;
    String toolName;

    Robot::CoordFrame frame;      ///< reference frame of specified the trajectory
    bool timeIntervalSpecified;   ///< true if an overriding time interval was specified for the trajectory
    Vector timeInterval;

    bool maxdxSpecified;         ///< true if a maximum dx length was specified
    Real maxdx;                  ///< maximum value of dx length for any trajectory step

    bool orientationControl;

    bool jointWeightsSpecified;
    base::Vector jointWeights;   ///< joint params weights 
    
    
    enum SolutionMethod { PseudoInverse, FullSpace };
    
    SolutionMethod                                                               solutionMethod;
    robot::control::kinematics::InverseKinematicsSolver::OptimizationMethod      optMethod;
    robot::control::kinematics::InverseKinematicsSolver::OptimizationCriterion   optCriteria;
    robot::control::kinematics::InverseKinematicsSolver::OptimizationConstraints optConstraints; 

    bool resultsPresent; ///< are there test results recorded? (either via externalization input or generated by execution)
    
    // test output records - all indices correspond (except that qs, xs & times have an extra final element at the end)
    array<base::Vector> qs;    ///< test output; sequence of 'joint' parameter vectors
    array<base::Vector> xs;    ///< test output; sequence of end-effector positions/orientations
    array<base::Vector> dxs;   ///< test output; sequence of end-effector displacements
    array<base::Vector> dqs;   ///< test output; sequence of 'joint' parameter displacements
    array<base::Time> times;   ///< test output; sequence of times
    array<base::Matrix> Js;    ///< test output; sequence of J(q) jacobian matrices
    
    ManipulatorJointTrajectory jtraj; ///< test output trajectory in 'joint' space (dim is chain.dof() - so also includes any platform dofs)
    
    bool testCompleted; ///< true if the test completed, fasle if an error occured and only partial results are available
    String failureString; ///< if !testCompleted, provides the failure error message
    
    base::PathName outputFileName; ///< output file specified in testspec file
    base::PathName inputFilePath;  ///< path from which testspec was loaded

    
    // some display related flags (read/written by test result viewers)
    
    bool displayRangeSpecified; ///< true if a subrange of the result joint trajectory has been specified for display purposes
    Int displayStartIndex , displayEndIndex;
    
    
    
    
    void saveResult(ref<base::VFileSystem> filesystem, base::PathName alternateOutputFileName = base::PathName());
                           
    // Externalizable
    virtual bool formatSupported(const String format, Real version = 1.0, ExternalizationType type = IO) const
    { return ( (format=="xml") && (version==1.0) ); }
    
    virtual void externalize(base::Externalizer& e, String format = "", Real version = 1.0);

  };



  /// the environment in which tests are performed
  ref<BasicEnvironment> getEnvironment() { return env; } 
  ref<const BasicEnvironment> getEnvironment() const { return env; } 
  void setEnvironment(ref<BasicEnvironment> env);

  /// which robot is to be tested?
  ref<Robot> getRobot() { return narrow_ref<Robot>(env->getRobot(testRobotIndex)); }
  ref<const Robot> getRobot() const { return narrow_ref<const Robot>(env->getRobot(testRobotIndex)); }
  
  /// which of the robot's manipulators is to be tested?
  Int getManipulatorIndex() const { return testManipulatorIndex; }
  
  Int numTests() const { return tests.size(); }
  Test& getTest(Int index) { Assert(index < numTests()); return tests[index]; }
  
  /// the VFileSystem path from which the test specification was read
  base::PathName getTestInputPath() const { return inputPath; }
  
  /// save the test and any results 
  /// Externalizes the entire IKORTest specification with results (if present)
  /// Also, if saveTrajFiles is true, the result trajectory of each test is saved in
  ///  a seperate file (per test)
  void saveResults(bool saveTrajFiles = true, base::PathName alternateOutputFileName = base::PathName());
  

  // some display related flags (read/written by test result viewers)
  
  bool displayObstacles; ///< true if obstacles are selected for display
  bool displayAxes;      ///< true if coordinate axes selected for display
  bool displayPlatform;  ///< true if the platform should be displayed
  bool displayEEPath;    ///< true if the path of the manip/tool is selected for display
  Int  displayStepMod;   ///< only display every nth step (1 for all steps)

  /// camera parameters (as in gfx::LookAtCameraManipulator)
  base::Point3 lookAtTarget; 
  Real alpha, theta, d;
  
    
  // Externalizable
  virtual bool formatSupported(const String format, Real version = 1.0, ExternalizationType type = IO) const
    { return ( (format=="xml") && (version==1.0) ); }

  virtual void externalize(base::Externalizer& e, String format = "", Real version = 1.0);

protected:
  ref<base::VFileSystem> filesystem;
  ref<base::Cache> cache;
  ref<SimulatedBasicEnvironment> env;
  Int testRobotIndex;       ///< which robot from the environment is used for testing 
  Int testManipulatorIndex; ///< which manipulator (in case the robot has multiple) is used for testing


  array<Test> tests;
  base::PathName inputPath;

};


} // namespace sim
} // namespace robot

#endif

---