mtsVFController.h

Go to the documentation of this file.

/* -*- Mode: C++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*-    */
/* ex: set filetype=cpp softtabstop=4 shiftwidth=4 tabstop=4 cindent expandtab: */

/*
  Author(s):  Paul Wilkening
  Created on: 2014

 (C) Copyright 2014 Johns Hopkins University (JHU), All Rights Reserved.

 --- begin cisst license - do not edit ---

 This software is provided "as is" under an open source license, with
 no warranty.  The complete license can be found in license.txt and
 http://www.cisst.org/cisst/license.txt.

 --- end cisst license ---
 */

#ifndef _mtsVFController_h
#define _mtsVFController_h

#include <cisstNumerical/nmrConstraintOptimizer.h>

#include <sawConstraintController/mtsVFBase.h>
#include <sawConstraintController/mtsVFJointVel.h>
#include <sawConstraintController/mtsVFJointPos.h>
#include <sawConstraintController/mtsVFDataSensorCompliance.h>
#include <sawConstraintController/mtsVFSensorCompliance.h>
#include <sawConstraintController/prmKinematicsState.h>
#include <sawConstraintController/prmSensorState.h>
#include <sawConstraintController/prmOffsetState.h>
#include <sawConstraintController/mtsVFCartVel.h>
#include <sawConstraintController/mtsVFCartOrientationVel.h>
#include <sawConstraintController/mtsVFPlane.h>
#include <sawConstraintController/mtsVFFollow.h>
#include <typeinfo>

// Always include last!
#include <sawConstraintController/sawConstraintControllerExport.h>

class CISST_EXPORT mtsVFController: public cmnGenericObject
{
    CMN_DECLARE_SERVICES(CMN_NO_DYNAMIC_CREATION, CMN_LOG_LOD_RUN_VERBOSE);

protected:

    //map between string names and pointers to virtual fixtures
    std::map<std::string, mtsVFBase *> VFMap;

    //map between string names and pointers to kinematics objects
    std::map<std::string, prmKinematicsState *> Kinematics;

    //map between string names and pointers to sensor objects
    std::map<std::string, prmSensorState *> Sensors;

    //robot joint state
    prmJointState JointState;

    //control optimizer variables
    nmrConstraintOptimizer Optimizer;

    //Current mode of controller (what controllerOutput represents)
    //The possible values of MODE refer to:
    //1. Treating controllerOutput as an incremental joint position
    //2. Treating controllerOutput as an incremental cartesian position
    mtsVFBase::CONTROLLERMODE ControllerMode;

public:

    mtsVFController(){}

    mtsVFController(size_t num_joints, mtsVFBase::CONTROLLERMODE cm):
        Optimizer(num_joints)
    {
        ControllerMode = cm;
    }

    ~mtsVFController()
    {
        std::map<std::string,mtsVFBase *>::iterator itVF;
        for(itVF = VFMap.begin(); itVF != VFMap.end(); itVF++)
        {       
            delete itVF->second;
        }

        std::map<std::string,prmKinematicsState *>::iterator itKin;
        for(itKin = Kinematics.begin(); itKin != Kinematics.end(); itKin++)
        {       
            delete itKin->second;
        }

        std::map<std::string,prmSensorState *>::iterator itSens;
        for(itSens = Sensors.begin(); itSens != Sensors.end(); itSens++)
        {       
            delete itSens->second;
        }
    }

    nmrConstraintOptimizer GetOptimizer(){return Optimizer;}

protected:

    void AddVFJointVelocity(const mtsVFDataBase & vf);

    void AddVFJointPosition(const mtsVFDataBase & vf);

    void AddVFCartesianTranslation(const mtsVFDataBase & vf);

    void AddVFCartesianOrientation(const mtsVFDataBase & vf);

    void AddVFSensorCompliance(const mtsVFDataSensorCompliance & vf);

    void AddVFPlane(const mtsVFDataPlane &vf);        

    void AddVFFollowPath(const mtsVFDataBase & vf);

    void SetSensor(const prmSensorState & sen);

    void SetSensorOffset(const prmOffsetState & sen);

    void RemoveSensorFromMap(const std::string & senName);

    void SetMode(const mtsVFBase::CONTROLLERMODE & m);

    void LookupBaseData(void);

    void SetKinematics(const prmKinematicsState & kin);

    void RemoveKinematicsFromMap(const std::string & kinName);

    void UpdateOptimizer(double TickTime);

    nmrConstraintOptimizer::STATUS Solve(vctDoubleVec & dq);

    void IncrementUsers(const std::vector<std::string> kin_names, const std::vector<std::string> sensor_names);

    void DecrementUsers(const std::vector<std::string> kin_names, const std::vector<std::string> sensor_names);

    bool SetVFData(const mtsVFDataBase & data, const std::type_info & type);

    bool SetVFDataSensorCompliance(const mtsVFDataSensorCompliance & data, const std::type_info & type);

    bool SetVFDataPlane(const mtsVFDataPlane & data, const std::type_info & type);

};

CMN_DECLARE_SERVICES_INSTANTIATION(mtsVFController);

#endif // _mtsVFController_h