robot/sim/SimulatedRobot

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: SimulatedRobot 1033 2004-02-11 20:47:52Z jungd $
  $Revision: 1.22 $
  $Date: 2004-02-11 15:47:52 -0500 (Wed, 11 Feb 2004) $
  $Author: jungd $
 
****************************************************************************/

#ifndef _ROBOT_SIM_SIMULATEDROBOT_
#define _ROBOT_SIM_SIMULATEDROBOT_

#include <robot/sim/sim>

#include <robot/Robot>

#include <base/VFile>
#include <base/Externalizable>
#include <base/Point3>
#include <base/Orient>

#include <physics/SolidSystem>
#include <physics/Spatial>
#include <physics/SpatialGroup>
#include <physics/CollidableProvider>

#include <robot/BasicControlInterface>
#include <robot/ToolDescription>
#include <robot/sim/SimulatedPlatform>
#include <robot/sim/SimulatedSerialManipulator>
#include <robot/sim/SimulatedTool>


namespace robot {
namespace sim {


/**
 * A concrete Robot class that instantiates a simulated robot
 *  according to a supplied specification.  Simulated by adding
 *  bodies, joints and motors to solidSystem.
 */
class SimulatedRobot : public Robot, public physics::Spatial, public physics::CollidableProvider, public base::Externalizable
{
public:
  SimulatedRobot(ref<base::VFile> robotSpecification,
                 const base::Point3& initialPosition, const base::Orient& initialOrientation,
                 ref<physics::SolidSystem> solidSystem, bool dynamic=true);

  SimulatedRobot(ref<const robot::RobotDescription> robotDescription,
                 const base::Point3& initialPosition, const base::Orient& initialOrientation,
                 ref<physics::SolidSystem> solidSystem, bool dynamic=true);
                 

  virtual String className() const { return String("SimulatedRobot"); }
  
  // factory
  virtual ref<PlatformDescription>    newPlatformDescription() const { return ref<PlatformDescription>(NewObj PlatformDescription()); }
  virtual ref<ManipulatorDescription> newManipulatorDescription() const { return ref<ManipulatorDescription>(NewObj ManipulatorDescription()); }


  virtual void setDynamic(bool enabled);
  
  virtual void setSolidSystem(ref<physics::SolidSystem> solidSystem)
    { this->solidSystem = solidSystem; }
    
    
  virtual array<std::pair<String,String> > controlInterfaces() const; 

  /**
   * SimulatedRobot provides the following ControlInterfaces (name:type):
   *
   *  platform:PlatformControl                        (only available when dynamic==true)
   *    - outputs control drive and stearing motors, depending on the specific platform
   *      (possibly none e.g. for a fixed platform)
   *      For the basic non-holonomic mobile platform, the first two inputs control
   *       the left & right drive motor torques and the third control the steering motor
   *       torque.  The inputs provide the angular velocity of the drive wheels and the
   *       steering angle.
   *
   *  platformVelocity:PlatformVelocityControl        (only available when dynamic==true)
   *    - As for PlatformControl, but the outputs control target velocity instead of
   *       torque.
   *
   *  platformPosition:PlatformPositionControl        (only available when dynamic==false)
   *    - outputs control the x,y and angle of the platform w.r.t to the world coordinate frame.
   *       The angle is clockwise relative to the x-axis (about z).
   *
   *  manipulatorN:JointForceControl                  (only available when dynamic==true)
   *     - where N is the manipulator number (from 1). 
   *       Outputs control joint force/torque and corresponding inputs give joint position.
   *
   *  manipulatorVelocityN:JointVelocityControl       (only available when dynamic==true)
   *     - where N is the manipulator number (from 1).  Outputs control joint velocity
   *       and corresponding inputs give joint position.
   *
   *  manipulatorPositionN:JointPositionControl       
   *    - outputs control the position of a manipulators joints (angle for revolute joints,
   *      extension for prismatic joints etc.)
   *
   *  manipulatorProximityN:LinkProximitySensors
   *     - where N is the manipulator number (from 1).
   *     - Inputs provide proximity sensor information for each link.  Each sequence of 5
   *        inputs provides the following information on the sensors of a single link:
   *        0: distance to closest object detected by proximity sensors on the link (or 0 if none detected)
   *        1-3: (x,y,z) direction vector to the closest object detected by proximity sensors on the link
   *        4: the distance along the link x-axis of the proximity sensor that detected the closest object to the link
   *       e.g. l*5 + 0  is the distance to the closest object detected by link l=[0..LastLink] 
   *     - There are no outputs
   *
   *  manipulatorToolGripN:ToolGripControl
   *     - single scalar input represents tool proximity to tool grip mechanism.
   *       0 => not in range to be grasped, 1 => in range to be grasped (or currently grasped)
   *       single scalar output controls tool gripper latch
   *       1 => grasp/latch tool, 0 => ungrasp/unlatch tool
   *
   *  toolN:JointForceControl                         (only available when dynamic==true)
   *     - joint force/torque output for tool joints (if any); and joint position inputs.
   *       Note that the inputSize() and outputSize()
   *       change dynamically depending on which tool is grasped (0 when no tool is grasped)
   *
   *  toolVelocityN:JointVelocityControl              (only available when dynamic==true)
   *     - joint velocity output for tool joints (if any); and joint position inputs.
   *       Note that the inputSize() and outputSize()
   *       change dynamically depending on which tool is grasped (0 when no tool is grasped)
   *
   *  toolPositionN:JointPositionControl              (only available when dynamic==false)
   *     - joint position for tool joints (if any).  Note that the inputSize() and outputSize()
   *       change dynamically depending on which tool is grasped (0 when no tool is grasped)
   *
   */
  virtual ref<ControlInterface> getControlInterface(String interfaceName="") throw(std::invalid_argument);

  virtual bool isDescriptionProvided() const { return true; }

  virtual ref<physics::Solid> getPlatformSolid() const { return platform->getPlatformSolid(); }

  
  // Spatial
  // NB: these are intended for static simulations.  Setting the configuration in a dynamic simulation
  //      will result in unrealistic motions - and possibly large forces/instability of system
  virtual void   setPosition(const Point3& pos);
  virtual Point3 getPosition() const;
  virtual void   setOrientation(const Orient& orient);
  virtual Orient getOrientation() const;
  virtual void   setConfiguration(const base::Transform& configuration);
  virtual base::Transform getConfiguration() const;

  
  /// test if tool is in position to be grasped by any end-effectors.
  bool checkProximity(ref<SimulatedTool> tool);

  /// 'magically' transport a tool to within grasping range of the end-effector
  void placeToolInProximity(ref<SimulatedTool> tool, Int manipIndex=0);

  /// gasp tool (if a suitable one is within grasping proximity of the end-effector)
  ///  returns true if a tool was grasped
  bool graspTool(Int manipIndex=0);

  /// release grasp
  void releaseGrasp(Int manipIndex=0); 

  
  // CollidableProvider
  virtual ref<physics::Collidable> createCollidable(CollidableFlags flags = 0);
  
  // Externalizable
  virtual bool formatSupported(String format, Real version = 1.0, ExternalizationType type = IO) const; ///< query if specific format is supported (for input, output or both)
  virtual void externalize(base::Externalizer& e, String format = "", Real version = 1.0); ///< read or write object state to Externalizer

protected:
  /// construct the physical robot from loaded spec by adding bodies, joints and motors into solidSystem
  void construct(const base::Point3& initialPosition, const base::Orient& initialOrientation);

  ref<SimulatedPlatform>              platform;     ///< simulated robot platform
  reflist<SimulatedSerialManipulator> manipulators; ///< simulated manipulators

  ref<physics::SolidSystem> solidSystem;  ///< SolidSystem into which the robot's physical parts will be added

  ref<physics::SpatialGroup> spatialGroup;   ///< a group containing the platform and manipulators

  /// if true, full physics is simulated; if false, the Solids that comprise the Robot only have a position/orientation - no velocity/acceleration
  ///  (and the position based ControlInterfaces can be used instead of the velocity & force/torque interfaces)
  bool dynamic; 
  

  /// ForceControl & VelControl require a dynamic simulation; PosControl will also function for
  ///  simple non-dynamic simulations
  enum ControlType { ForceControl, VelControl, PosControl };
  

  class ManipulatorControlInterface : public BasicControlInterface
  {
  public:
    ManipulatorControlInterface(const String& name, const String& type, 
                                ref<SimulatedSerialManipulator> sm, ControlType controlType = ForceControl)
      : BasicControlInterface(name, type,
                              sm->getManipulatorDescription()->getKinematicChain().dof(),
                              sm->getManipulatorDescription()->getKinematicChain().dof() ),
        sm(sm), controlType(controlType) {}

    ManipulatorControlInterface(const ManipulatorControlInterface& mci)
      : BasicControlInterface(mci), sm(mci.sm), controlType(mci.controlType) {}

    virtual String className() const { return String("ManipulatorControlInterface"); }
    virtual Object& clone() const { return *NewObj ManipulatorControlInterface(*this); } 

    virtual String        inputName(Int i) const
      { return String("jointPosition")+base::intToString(i); }

    virtual Real          getInput(Int i) const
      { 
        Assert(i < inputSize());
        return sm->getJointPos(i+1);
      }

    virtual String outputName(Int i) const
      { 
        if (controlType == ForceControl)
          return String("jointForce")+base::intToString(i); 
        else if (controlType == VelControl)
          return String("jointVelocity")+base::intToString(i); 
        else
          return String("jointPosition")+base::intToString(i); 
      }

    virtual void   setOutput(Int i, Real value)
      {
        Assert(i < outputSize());
        if (controlType == ForceControl)
          sm->setJointForce(i+1, value);
        else if (controlType == VelControl)
          sm->setJointVel(i+1, value);
        else
          sm->setJointPos(i+1, value);
      }

  protected:
    ref<SimulatedSerialManipulator> sm;
    ControlType controlType;
  };




  class ToolControlInterface : public BasicControlInterface
  {
  public:
    ToolControlInterface(const String& name, const String& type, ref<SimulatedSerialManipulator> sm, 
                         bool gripControl, ControlType controlType = ForceControl)
      : BasicControlInterface(name, type, gripControl?1:0, gripControl?1:0),
        gripControl(gripControl), sm(sm), controlType(controlType) {}
    
    ToolControlInterface(const ToolControlInterface& tci)
      : BasicControlInterface(tci), gripControl(tci.gripControl), 
        sm(tci.sm), controlType(tci.controlType) {}

    virtual String className() const { return String("ToolControlInterface"); }
    virtual Object& clone() const { return *NewObj ToolControlInterface(*this); }

    virtual Int           inputSize() const 
    {
      if (gripControl) return 1;
      if (sm->isToolGrasped()) return sm->getToolInProximity()->getToolDescription()->getKinematicChain().dof();
      return 0;
    }

    virtual String        inputName(Int i) const
      { return gripControl?String("toolProximity"):String("jointPosition")+base::intToString(i); }

    virtual Real          getInput(Int i) const
      { 
        Assert(i < inputSize());
        if (gripControl) {
          if (sm->isToolGrasped()) return 1;
          if (sm->getToolInProximity())
            return sm->checkProximity(sm->getToolInProximity())?1.0:0.0;
        }
        if (sm->isToolGrasped()) 
          return sm->getToolInProximity()->getJointPos(i+1);
        return 0;
      }

    virtual const Vector& getInputs() const 
      {
        Int s = inputSize();
        lastInputs.resize(s);
        for(Int i=0; i<s; i++)
          lastInputs[i] = getInput(i);
        return lastInputs;
      }

    virtual Int    outputSize() const 
      {
        if (gripControl) return 1;
        if (sm->isToolGrasped()) return sm->getToolInProximity()->getToolDescription()->getKinematicChain().dof();
        return 0;
      }

    virtual String outputName(Int i) const
      { 
        if (gripControl) return String("toolGrasp");
        if (controlType==ForceControl) return String("jointForce")+base::intToString(i);
        else if (controlType==VelControl) return String("jointVelocity")+base::intToString(i);
        else return String("jointPosition")+base::intToString(i);
      }

    virtual void   setOutput(Int i, Real value)
      {
        Assert(i < outputSize());
        if (gripControl) {
          if (Math::equals(value,0)) 
            sm->releaseGrasp();
          else
            sm->graspTool();
          return;
        }
        if (sm->isToolGrasped()) {
          if (controlType==ForceControl)
            sm->getToolInProximity()->setJointForce(i+1, value);
          else if (controlType==VelControl)
            sm->getToolInProximity()->setJointVel(i+1, value);
          else 
            sm->getToolInProximity()->setJointPos(i+1, value);
        }
      }

    virtual void   setOutputs(const Vector& values) 
      {
        numOutputs = outputSize();
        BasicControlInterface::setOutputs(values);
      }

  protected:
    bool gripControl; ///< is this a ToolGripControl? (or a JointPositionControl)
    ref<SimulatedSerialManipulator> sm;
    ControlType controlType;
   };




  class PlatformControlInterface : public BasicControlInterface
  {
  public:
    PlatformControlInterface(const String& name, const String& type, 
                             ref<SimulatedRobot> sr, ref<SimulatedPlatform> sp, 
                             ControlType controlType=ForceControl)
      : BasicControlInterface(name, type,
                              sp->getPlatformDescription()->isMobile()?3:0,
                              sp->getPlatformDescription()->isMobile()?3:0),
        sr(sr), sp(sp), controlType(controlType) {}

    PlatformControlInterface(const PlatformControlInterface& pci)
      : BasicControlInterface(pci), sr(pci.sr), sp(pci.sp), controlType(pci.controlType) {}

    virtual String className() const { return String("PlatformControlInterface"); }
    virtual Object& clone() const { return *NewObj PlatformControlInterface(*this); } 

    virtual String        inputName(Int i) const
    {
      if (controlType != PosControl)
        return (i==0)?String("leftDriveWheelVelocity"):( (i==1)?String("rightDriveWheelVelocity"):String("steeringAngle") );
      else
        return (i==0)?String("x"):( (i==1)?String("y"):String("theta") );
    }

    virtual Real          getInput(Int i) const
      { 
        Assert(i < inputSize());
        if (controlType != PosControl) {
          if (i==0) return sp->getLeftBackWheelVel();
          if (i==1) return sp->getRightBackWheelVel();
          return sp->getSteeringAngle();
        }
        else {
          if (i==0) return sp->getPosition2D().x;
          if (i==1) return sp->getPosition2D().y;
          return sp->getOrientation2D();
        }
      }

    virtual String outputName(Int i) const
      { 
        if (controlType==ForceControl) 
          return (i==0)?String("leftDriveWheelTorque"):( (i==1)?String("rightDriveWheelTorque"):String("steeringTorque") ); 
        else if (controlType==VelControl)
          return (i==0)?String("leftDriveWheelVelocity"):( (i==1)?String("rightDriveWheelVelocity"):String("steeringVelocity") );
        else
          return (i==0)?String("x"):( (i==1)?String("y"):String("theta") );
      }

    virtual void   setOutput(Int i, Real value)
      {
        Assert(i < outputSize());
        if (controlType==ForceControl) {
          if (i==0)
            sp->setLeftBackWheelTorque(value);
          else {
            if (i==1)
              sp->setRightBackWheelTorque(value);
            else
              sp->setSteeringTorque(value);
          }
        }
        else if (controlType==VelControl) {
          if (i==0)
            sp->setLeftBackWheelVel(value);
          else {
            if (i==1)
              sp->setRightBackWheelVel(value);
            else
              sp->setSteeringVel(value);
          }
        }
        else {
          // move the robot
          Point2 p( sr->getPosition2D() );
          Real theta = sr->getOrientation2D();
          if (i==0) 
            sr->setPosition2D( Point2(value, p.y), theta );
          else {
            if (i==1) 
              sr->setPosition2D( Point2(p.x, value), theta );
            else 
              sr->setPosition2D( p, value );
          }
        }
      }

  protected:
    ref<SimulatedRobot> sr;
    ref<SimulatedPlatform> sp;
    ControlType controlType;
  };

  

  class ProximitySensorInterface : public BasicControlInterface
  {
  public:
    ProximitySensorInterface(const String& name, const String& type,
                             ref<SimulatedSerialManipulator> sm)
      : BasicControlInterface(name, type, (sm->getManipulatorDescription()->getKinematicChain().size()+1)*5, 0),
        sm(sm) {}
      
   ProximitySensorInterface(const ProximitySensorInterface& psi)
     : BasicControlInterface(psi), sm(psi.sm) {}
  
    virtual String className() const { return String("ProximitySensorInterface"); }
    virtual Object& clone() const { return *NewObj ProximitySensorInterface(*this); }
    
    virtual String        inputName(Int i) const
      {
        Assert(i < inputSize() );
        Int linkNo = (i/5);
        Int dataIndex = i%5;
        String name(String("proximitySensor")+base::intToString(linkNo));
        switch (dataIndex) {
          case 0: name+=" distance"; break;
          case 1: name+=" normal.x"; break;
          case 2: name+=" normal.y"; break;
          case 3: name+=" normal.z"; break;
          case 4: name+=" sensor position"; break;
          default:;
        }
        return name;
      }
  
    virtual String outputName(Int i) const { Assertm(false,"interface has outputs"); return ""; }

    virtual Real          getInput(Int i) const
    {
      Assert(i < inputSize());
      Int linkNo = (i/5);
      Int dataIndex = i%5;
      switch (dataIndex) {
          case 0: return sm->getClosestObjectDistance(linkNo);
          case 1: return sm->getClosestObjectDirection(linkNo).x;
          case 2: return sm->getClosestObjectDirection(linkNo).y;
          case 3: return sm->getClosestObjectDirection(linkNo).z;
          case 4: return sm->getClosestObjectSensorPosition(linkNo);
          default:;
      }
      return 0;
    }
    
    
    virtual void   setOutput(Int i, Real value) { Assertm(false,"interface has outputs"); }
    
  protected:
    ref<SimulatedSerialManipulator> sm;
  };
    

};



}
} // robot::sim

#endif

---