opspace/include/opspace/task_library.hpp

Go to the documentation of this file.

/*
 * Shared copyright notice and LGPLv3 license statement.
 *
 * Copyright (C) 2011 The Board of Trustees of The Leland Stanford Junior University. All rights reserved.
 * Copyright (C) 2011 University of Texas at Austin. All rights reserved.
 *
 * Authors: Roland Philippsen (Stanford) and Luis Sentis (UT Austin)
 *          http://cs.stanford.edu/group/manips/
 *          http://www.me.utexas.edu/~hcrl/
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this program.  If not, see
 * <http://www.gnu.org/licenses/>
 */

#ifndef OPSPACE_TASK_LIBRARY_HPP
#define OPSPACE_TASK_LIBRARY_HPP

#include <opspace/Task.hpp>

namespace opspace {
  
  class TypeIOTGCursor;
  

  class PDTask
    : public Task
  {
  public:
    virtual Status check(Vector const * param, Vector const & value) const;
    
    inline void quickSetup(Vector const & kp, Vector const & kd, Vector const & maxvel)
    {
      kp_ = kd;
      kd_ = kd;
      maxvel_ = maxvel;
    }
    
  protected:
    typedef enum {
      SATURATION_OFF,
      SATURATION_COMPONENT_WISE,
      SATURATION_MAX_COMPONENT,
      SATURATION_NORM
    } saturation_policy_t;
    
    PDTask(std::string const & name, saturation_policy_t saturation_policy);
    
    Status initPDTask(Vector const & initpos);
    
    Status computePDCommand(Vector const & curpos,
          Vector const & curvel,
          Vector & command);
    
    saturation_policy_t const saturation_policy_;
    bool initialized_;
    Vector goalpos_;
    Vector goalvel_;
    Vector errpos_;
    Vector errvel_;
    Vector kp_;
    Vector kd_;
    Vector maxvel_;
  };
  
  
  class DraftPIDTask
    : public PDTask
  {
  public:
    explicit DraftPIDTask(std::string const & name);
    
    virtual Status check(double const * param, double value) const;
    virtual Status check(Vector const * param, Vector const & value) const;
    
    virtual Status init(Model const & model);
    virtual Status update(Model const & model);
    
    virtual void dbg(std::ostream & os,
         std::string const & title,
         std::string const & prefix) const;
    
  protected:
    Status initDraftPIDTask(Vector const & initpos);
    
    Status computeDraftPIDCommand(Vector const & curpos,
          Vector const & curvel,
          Vector & command);
    
    double dt_seconds_;
    Vector ki_;
    Vector errsum_;
    Vector limitpos_;
    Vector limitvel_;
    Vector triggerpos_;
  };
  
  
  class CartPosTask
    : public PDTask
  {
  public:
    explicit CartPosTask(std::string const & name);
    
    virtual Status init(Model const & model);
    virtual Status update(Model const & model);
    virtual Status check(std::string const * param, std::string const & value) const;
    
    inline void quickSetup(Vector const & kp, Vector const & kd, Vector const & maxvel,
         std::string const & name, Vector const & control_point)
    {
      PDTask::quickSetup(kp, kd, maxvel);
      end_effector_name_ = name;
      control_point_ = control_point;
    }
    
  protected:
    std::string end_effector_name_;
    Vector control_point_;
    
    mutable taoDNode const * end_effector_node_;
    
    taoDNode const * updateActual(Model const & model);
  };
  
  
  class JPosTask
    : public PDTask
  {
  public:
    explicit JPosTask(std::string const & name);
    
    virtual Status init(Model const & model);
    virtual Status update(Model const & model);
  };
  
  
  class SelectedJointPostureTask
    : public Task
  {
  public:
    explicit SelectedJointPostureTask(std::string const & name);
    
    virtual Status init(Model const & model);
    virtual Status update(Model const & model);
    virtual Status check(double const * param, double value) const;
    virtual Status check(Vector const * param, Vector const & value) const;
    
  protected:
    Vector selection_;
    double kp_;
    double kd_;
    bool initialized_;
    std::vector<size_t> active_joints_;
  };
  
  
  class TrajectoryTask
    : public PDTask
  {
  public:
    virtual ~TrajectoryTask();
    
    virtual Status check(double const * param, double value) const;
    
    virtual Status check(Vector const * param, Vector const & value) const;
    
    virtual void dbg(std::ostream & os,
         std::string const & title,
         std::string const & prefix) const;
    
  protected:
    typedef PDTask::saturation_policy_t saturation_policy_t;
    
    TrajectoryTask(std::string const & name, saturation_policy_t saturation_policy);
    
    Status initTrajectoryTask(Vector const & initpos);
    
    Status computeTrajectoryCommand(Vector const & curpos,
            Vector const & curvel,
            Vector & command);
    
    TypeIOTGCursor * cursor_;
    double dt_seconds_;
    Vector trjgoal_;
    Vector maxacc_;
    mutable Vector qh_maxvel_;  // grr, cursor always needs multi-dim maxvel, even if saturation_norm
  };
  
  
  class CartPosTrjTask
    : public TrajectoryTask
  {
  public:
    explicit CartPosTrjTask(std::string const & name);
    
    virtual Status init(Model const & model);
    virtual Status update(Model const & model);
    
  protected:
    int end_effector_id_;
    Vector control_point_;
    
    taoDNode const * updateActual(Model const & model);
  };
  
  
  class JPosTrjTask
    : public TrajectoryTask
  {
  public:
    explicit JPosTrjTask(std::string const & name);
    
    virtual Status init(Model const & model);
    virtual Status update(Model const & model);
    
    void quickSetup(double dt_seconds, double kp, double kd, double maxvel, double maxacc);
  };


  class JointLimitTask
    : public Task
  {
  public:
    explicit JointLimitTask(std::string const & name);
    virtual ~JointLimitTask();
    
    virtual Status check(Vector const * param, Vector const & value) const;
    virtual Status check(double const * param, double const & value) const;
    virtual Status init(Model const & model);
    virtual Status update(Model const & model);
    
    virtual void dbg(std::ostream & os,
         std::string const & title,
         std::string const & prefix) const;
    
  protected:
    // parameters
    Vector upper_stop_deg_;
    Vector upper_trigger_deg_;
    Vector lower_stop_deg_;
    Vector lower_trigger_deg_;
    double dt_seconds_;
    Vector maxvel_;
    Vector maxacc_;
    Vector kp_;
    Vector kd_;
    
    // non-parameters
    Vector upper_stop_;
    Vector upper_trigger_;
    Vector lower_stop_;
    Vector lower_trigger_;
    
    std::vector<TypeIOTGCursor *> cursor_;
    Vector goal_;

    void updateState(Model const & model);
  };
  
  
  class OrientationTask
    : public Task
  {
  public:
    explicit OrientationTask(std::string const & name);
    
    virtual Status init(Model const & model);
    virtual Status update(Model const & model);
    
    virtual void dbg(std::ostream & os,
         std::string const & title,
         std::string const & prefix) const;
    
  protected:
    taoDNode const * updateActual(Model const & model);
    
    //    Vector goal_;   // direction cosines
    Eigen::Vector3d goal_x_;
    Eigen::Vector3d goal_y_;
    Eigen::Vector3d goal_z_;
    Eigen::Vector3d actual_x_;
    Eigen::Vector3d actual_y_;
    Eigen::Vector3d actual_z_;
    Vector velocity_;   // instantaneous omega
    Vector delta_;    // instantaneous angular difference
    int end_effector_id_;
    double kp_;
    double kd_;
    double maxvel_;
    Vector eepos_;    // just for logging...
  };

  
}

#endif // OPSPACE_TASK_LIBRARY_HPP

---