작업공간 어드미턴스 제어

시스템 및 제어기 구성

IndySDK에서 제공하는 위치 제어기, 어드미턴스 궤적 생성기, 필터 및 공유메모리 등을 활용해서 작업공간 임피던스 제어기를 구성한다.

본 예제를 통해서 다음을 살펴볼 수 있다.

• F/T센서의 좌표계 변환방법
• H-infinity 위치제어기 사용방법
• 명목 시스템 초기화 방법
• 정동역학 기반 위치제어기 사용방법
• 작접공간내 3차원 어드미턴스 궤적 생성기 사용방법
• 저역필터의 사용방법

예제 코드 작성

헤더 파일 작성

작업공간 제어 컴포넌트 (Task Control Component)를 생성한 후, 공유메모리와 필터등을 사용하기 위해서 다음의 해더파일을 추가하고 필요한 코드를 작성한다.

// for the use of shared memeory
#include "NRMKFramework/Indy/Sharedmemory/SharedData.h"
#include "NRMKFramework/Indy/Sharedmemory/SharedData.h"
NRMKFramework::ShmemManager * indyShm = NULL;
NRMKIndy::SharedData::RobotControlSharedData ctrData;

// for the use of lowpass filter and filtered derviative
#include <Util/Filter.h>
#include <math.h>

작업공간 어드미턴스 제어기를 설계하기 전에 생성된 TaskControlComponent 클래스에 다음과 같이 멤버함수와 타입 및 변수들을 새로 정의한다

// degree of freedom (6dof)
enum
{
  JOINT_DOF = AbstractRobot6D::JOINT_DOF
};

// new type definitaions
typedef typename ROBOT::JointVec JointVec;
typedef typename ROBOT::JointMat JointMat;
typedef NRMKFoundation::FilterLowPass<double> LowPassFilter;
typedef NRMKFoundation::FilterDerivative<double> FilteredDerivative;

// member functions
void readFTSensor(ROBOT & robot);
void initNominalSystem(ROBOT & robot);
void computeJDet(ROBOT & robot);
void setBias() {_ftBias = _ftSensor;}

private:
    // getting nominal robot dynamics
    JointMat            _Mn, _Cn;
    JointVec            _Gn;

private:
    // getting admittance trajectory
    TaskPosition        _tposPxy;
    TaskVelocity        _tvelPxy;
    TaskAcceleration    _taccPxy;

private:
    // with respect to ft sensor
    LieGroup::Wrench    _ftBias;
    LieGroup::Wrench    _ftSensor;
    LieGroup::Rotation  _ft_sensor_orientation;
    LieGroup::Wrench    _ftTransformed, _ftTransformed_filtered;
    LowPassFilter       _ftLowPass[6];

private:
    // reset control, instaneous time, control gains
    bool                _reset_ctrl;
    double              _delT;
    TaskVec             _kp, _kv, _ki;

다음으로 공유 메모리에서 F/T센서 정보를 획득하기 위해서 다음과 같이 생성자에서 포인터 변수에 메모리를 할당한다 (자세한 내용은 IndyFramework 또는 F/T센서 이용 참고).

TaskControlComponent::TaskControlComponent()
: AbstractController(USERNAME, EMAIL, SERIAL)
, _robotNom(NULL)
{
    indyShm = new NRMKFramework::ShmemManager(INDY_SHM_NAME, INDY_SHM_LEN);

    for(int i=0; i<6; i++) _ftBias[i] = 0;
}

종료 시 할당된 메모리를 해제하기 위해서 다음과 같이 공유메모리와 명목 로봇에 할당된 메모리를 해제한다.

TaskControlComponent::~TaskControlComponent()
{
    if (_robotNom != NULL) delete _robotNom;

    if (indyShm != NULL)
        delete indyShm;
}

소스 파일 작성

작업공간 어드미턴스 유닛에 의해서 생성된 어드미턴스 궤적을 API로 제공되는 위치제어기를 사용하여 로봇이 추종하도록 제어기를 설계한다. 본 예제에서는 API로 제공되는 위치제어기인 H-infinity 제어기와 Forward Dynamic 제어기를 각각 활용하여 어드미턴스 제어를 구현한다.

이를 위해서 먼저 initialize() 함수를 다음과 같이 작성한다.

• 로봇의 끝단에 배치된 센서의 좌표계를 기준 좌표계와 일치 시킬 수 있도록 한다.
• 사용하려고 하는 제어기를 초기화 한다.
• 샘플링 시간(delt)을 전역변수로 사용할 수 있도록 _delT에 업데이트한다.

void TaskControlComponent::initialize(ROBOT & robot, double delt)
{
  // create a nominal robot
    _robotNom = AbstractController::createRobotObj();

  // align ft sensor coordinates with robot's tool frame's coordinates
    _ft_sensor_orientation = LieGroup::Rotation(-1,0,0,0,-1,0,0,0,1);
    robot.setTFTSensor(_ft_sensor_orientation, LieGroup::Displacement::Zero());

  // initialize position controller
#if defined(HINF_CTR)
    robot.initHinfController(delt);
  robot.resetHinfController();
#else
    robot.initForwardDynController(delt);
  robot.resetForwardDynController();
#endif

  // update sampling time
  _delT = delt;

  //initialize task-space error
  robot.initTaskErr(delt);
}

제어이득 값을 전역에서 사용하기 위해서 다음과 같이 setGains() 함수에서 제어 이득을 전역 변수에 업데이트 한다.

void TaskControlComponent::setGains(TaskVec const & kp, TaskVec const & kv, TaskVec const & ki)
{
    _kp = kp;
    _kv = kv;
    _ki = ki;
}

작업이 끝날 때마다 초기화가를 수행하기 위해서 reset() 함수를 다음과 같이 작성한다. 본 예제에서는 작업이 끝날 때마다 F/T센서 값의 초기화와 제어기 내부 초기화용 변수를 초기화 한다.

void TaskControlComponent::reset()
{
    // get ft sensor values from shared memory
    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);
    for (int i=0; i<6; i++) _ftSensor[i] = ctrData.Fext[i];

    // compensate ft sensor bias
    setBias();

    for(int j=0; j<JOINT_DOF; j++){
        for(int i=0; i<2; i++){
            _ftLowPass[j].filtered(i) = 0.0;
            _ftLowPass[j].raw(i) = 0.0;
        }
    }

    // reset flag defined for controller reset
    _reset_ctrl = true;
}

어드미턴스 제어기는 다음과 같이 computeControlTorq() 함수에 작성된다.

int TaskControlComponent::computeControlTorq(ROBOT & robot, LieGroup::Vector3D const & gravDir, TaskPosition const & posDesired, TaskVelocity const & velDesired, TaskAcceleration const & accDesired, JointVec & torque)
{
    if (indyShm == NULL)
    {
        printf("Shared memory is missed\n");
    }

  // reset admittance trajectory and position controller
    if (_reset_ctrl)
    {
      printf("initialized\n");

      //reset nominal system "robotNom"; the function is defined below
      initNominalSystem(robot);
      //reset admittancd trajectory
      robot.resetAdmittanceTraj(_delT);
      //reset position controllers
#if defined(HINF_CTR)
      robot.resetHinfController();
#else
      robot.resetForwardDynController();
#endif
      _reset_ctrl = false;
    }

    // read ft sensor value; the function is defined below
    readFTSensor(robot);

    // change ft sensor direction x,y,z,u,v,w to u,v,w,x,y,z
    for (int i=0; i<3; i++) _ftTransformed[i] = _ftSensor[i+3];
    for (int i=0; i<3; i++) _ftTransformed[i+3] = _ftSensor[i];

    // lowpass filtering of ft sensor values
    LieGroup::Wrench _ftTransFiltered, _ftTransFilteredLow;
    for (int i=0; i<INDY_NUM_TASK_AXES; i++)
    {
      _ftLowPass[i].filter(_ftTransformed[i], _ftTransformed_filtered[i]);
    }

    // calculate admittance trajecotry
    // based on desired trajectories and ft sensor values
    robot.updateAdmittanceTraj(posDesired, velDesired, accDesired, _tposPxy,
    _tvelPxy, _taccPxy, _ftTransformed, _ftTransformed_filtered, -1);

    // set control gains and update admittance trajecotry to controllers
#if defined(HINF_CTR)
    robot.setHinfControlGain(_kp, _kv, _ki);
    robot.HinfController(gravDir, _tposPxy, _tvelPxy, _taccPxy);
#else
    robot.setForwardDynControlGain(_kp, _kv, _ki);
    robot.ForwardDynController(*_robotNom, _Mn, _Cn, _Gn, _tposPxy, _tvelPxy,
    gravDir);
#endif

    torque = robot.tau();

    return 0;   //return 1: Close to Singularity
}

다음으로 computeControlTorq() 에서 사용된 resetNominalSystem() 함수와 readFTSensor() 함수는 다음과 같이 각각 정의된다.

아래와 같이 resetNominalSystem() 함수를 정의하여 작업완료 후 재시작 시 명목 로봇 시스템 robotNom 을 실제 로봇 시스템 robot 의 값으로 업데이트 한다.

void TaskControlComponent::initNominalSystem(ROBOT & robot)
{
    _robotNom->setTtarget(robot.Ttarget().R(), robot.Ttarget().r());
    _robotNom->setTReference(robot.Tref().R(),robot.Tref().r());
    _robotNom->q() = robot.q();
    _robotNom->qdot() = robot.qdot();
    _robotNom->update();
    _robotNom->updateJacobian();
}

아래와 같이 readFTSensor() 함수를 정의하여 F/T 센서 값을 로봇의 베이스 좌표계와 일치하도록 변환 한다.

void TaskControlComponent::readFTSensor(ROBOT & robot)
{
    NRMKIndy::SharedData::getControlData(*indyShm, ctrData);
    TaskPosition _tposT;
    TaskVelocity _tvelT;
    LieGroup::Wrench _ftSensorTemp;
    _ftSensorTemp.setZero();

    robot.computeFK(_tposT, _tvelT);

    for (int i=0; i<6; i++) _ftSensorTemp[i] = ctrData.Fext[i];
    _ftSensorTemp = _ftSensorTemp - _ftBias;

    LieGroup::HTransform T_ft_target = robot.Ttarget().icascade(robot.Tft());
    LieGroup::Wrench _ftSensorTemp2 = T_ft_target.itransform(_ftSensorTemp);
    LieGroup::Wrench _ftSensorTransformed = LieGroup::Wrench(_tposT.R()*_ftSensorTemp2.f(), _ftSensorTemp2.n());

    for (int i=0; i<3; i++) _ftSensor[i] = _ftSensorTransformed[i+3];
    for (int i=0; i<3; i++) _ftSensor[i+3] = _ftSensorTransformed[i];
}