CRobot.hpp

/* This file is part of scl, a control and simulation library
for robots and biomechanical models.

scl 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.

Alternatively, 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.

scl 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.

You should have received a copy of the GNU Lesser General Public
License and a copy of the GNU General Public License along with
scl. If not, see <http://www.gnu.org/licenses/>.
*/
/* \file CRobot.hpp
 *
 *  Created on: Dec 27, 2010
 *
 *  Copyright (C) 2010
 *
 *  Author: Samir Menon <smenon@stanford.edu>
 */

#ifndef CROBOT_HPP_
#define CROBOT_HPP_

#include <scl/DataTypes.hpp>

#include <scl/Singletons.hpp>
#include <scl/robot/data_structs/SRobot.hpp>
#include <scl/dynamics/CDynamicsBase.hpp>
#include <scl/control/CControllerBase.hpp>

#include <sutil/CMappedList.hpp>

#include <string>
#include <vector>
#include <fstream>

namespace scl
{
  class CRobot
  {
  public:
    // **********************************************************************
    //                    Primary Computation functions
    // **********************************************************************

    void computeServo();

    void computeDynamics();

    void computeNonControlOperations();

    void integrateDynamics();


    // **********************************************************************
    //                       Initialization helper functions
    // **********************************************************************

    sBool initFromDb(std::string arg_robot_name,
        CDynamicsBase* arg_dynamics,
        CDynamicsBase* arg_integrator);

    sBool init(std::string arg_robot_name,
        CDynamicsBase* arg_dynamics,
        CDynamicsBase* arg_integrator,
        SRobotParsed *arg_robot,
        SRobotIO *arg_io_data,
        std::vector<SControllerBase*>& arg_ctrls);

    sBool hasBeenInit() { return data_.has_been_init_;  }

    // **********************************************************************
    //                       Robot helper functions
    //                Runtime overrides for config file settings
    // **********************************************************************

    SRobot* getData() { return &data_;  }

    void setFlagApplyDamping(sBool arg_flag)
    { data_.parsed_robot_data_->flag_apply_gc_damping_ = arg_flag;  }

    sBool setDamping(const Eigen::VectorXd& arg_d);

    void setFlagApplyGcPosLimits(sBool arg_flag)
    { data_.parsed_robot_data_->flag_apply_gc_pos_limits_ = arg_flag;  }

    sBool setGcPosLimits(const Eigen::VectorXd& arg_max,
        const Eigen::VectorXd& arg_min);

    void setFlagApplyActuatorForceLimits(sBool arg_flag)
    { data_.parsed_robot_data_->flag_apply_actuator_force_limits_ = arg_flag;  }

    sBool setActuatorForceLimits(const Eigen::VectorXd& arg_max,
        const Eigen::VectorXd& arg_min);

    void setFlagControllerOn(sBool arg_flag)
    { data_.parsed_robot_data_->flag_controller_on_ = arg_flag;  }

    // **********************************************************************
    //                       Robot state helper functions
    // **********************************************************************

    const Eigen::VectorXd& getGeneralizedCoordinates()
    { return data_.io_data_->sensors_.q_; }

    const Eigen::VectorXd& getGeneralizedVelocities()
    { return data_.io_data_->sensors_.dq_; }

    const Eigen::VectorXd& getGeneralizedAccelerations()
    { return data_.io_data_->sensors_.ddq_; }

    const Eigen::VectorXd& getGeneralizedForcesMeasured()
    { return data_.io_data_->sensors_.force_gc_measured_; }

    const Eigen::VectorXd& getGeneralizedForcesCommanded()
    { return data_.io_data_->actuators_.force_gc_commanded_; }

    void setGeneralizedCoordinates(const Eigen::VectorXd& arg_q)
    {  data_.io_data_->sensors_.q_ = arg_q; }

    void setGeneralizedVelocities(const Eigen::VectorXd& arg_dq)
    {  data_.io_data_->sensors_.dq_ = arg_dq; }

    void setGeneralizedAccelerations(const Eigen::VectorXd& arg_ddq)
    {  data_.io_data_->sensors_.ddq_ = arg_ddq; }

    void setGeneralizedForcesMeasured(const Eigen::VectorXd& arg_f)
    {  data_.io_data_->sensors_.force_gc_measured_ = arg_f; }

    void setGeneralizedForcesCommanded(const Eigen::VectorXd& arg_f)
    {  data_.io_data_->actuators_.force_gc_commanded_ = arg_f; }

    void setGeneralizedCoordinatesToZero()
    {  data_.io_data_->sensors_.q_.setZero(data_.parsed_robot_data_->dof_); }

    void setGeneralizedVelocitiesToZero()
    {  data_.io_data_->sensors_.dq_.setZero(data_.parsed_robot_data_->dof_); }

    void setGeneralizedAccelerationsToZero()
    {  data_.io_data_->sensors_.ddq_.setZero(data_.parsed_robot_data_->dof_); }

    void setGeneralizedForcesMeasuredToZero()
    {  data_.io_data_->sensors_.force_gc_measured_.setZero(data_.parsed_robot_data_->dof_); }

    void setGeneralizedForcesCommandedToZero()
    {  data_.io_data_->actuators_.force_gc_commanded_.setZero(data_.parsed_robot_data_->dof_); }

    // **********************************************************************
    //                       Controller helper functions
    // **********************************************************************

    sBool addController(SControllerBase* arg_ctrl_ds);

    CControllerBase* getControllerCurrent();

    sBool setControllerCurrent(std::string arg_ctrl_name);

    SControllerBase* getControllerDataStruct(const std::string& arg_ctrl_name);

    const CDynamicsBase* getControllerDynamics()
    { return static_cast<const CDynamicsBase*>(dynamics_);  }

    sBool getProportionalGain(Eigen::VectorXd& ret_gains,
        const std::string& arg_ctrl_name="",
        const std::string& arg_task_name="");

    sBool getDerivativeGain(Eigen::VectorXd& ret_gains,
        const std::string& arg_ctrl_name="",
        const std::string& arg_task_name="");

    sBool getIntegralGain(Eigen::VectorXd& ret_gains,
        const std::string& arg_ctrl_name="",
        const std::string& arg_task_name="");

    sBool setProportionalGain(const Eigen::VectorXd& arg_kp,
        const std::string& arg_ctrl_name="",
        const std::string& arg_task_name="");

    sBool setDerivativeGain(const Eigen::VectorXd& arg_kv,
        const std::string& arg_ctrl_name="",
        const std::string& arg_task_name="");

    sBool setIntegralGain(const Eigen::VectorXd& arg_ki,
        const std::string& arg_ctrl_name="",
        const std::string& arg_task_name="");

    // **********************************************************************
    //                       Logging functions
    // **********************************************************************

    sBool setLogFile(const std::string &arg_file);

    sBool logState(bool arg_log_gc=true, bool arg_log_gc_matrices=true,
        bool arg_log_task_matrices=false);

    // **********************************************************************
    //                       Constructors etc.
    // **********************************************************************

    CRobot();

    virtual ~CRobot();

  private:
    SRobot data_;

    SDatabase *db_;

    CDynamicsBase* dynamics_;

    CDynamicsBase* integrator_;

    sutil::CMappedList<std::string,CControllerBase*> ctrl_;

    CControllerBase* ctrl_current_;

    std::string log_file_name_;
    std::fstream log_file_;
    sBool logging_on_;
  };

}

#endif /* CROBOT_HPP_ */