Using orocos KDL library

Introduction

로봇암 Controller를 작성하다 보면 kinematics, dynamics library가 필요한 경우가 있다. kinematics는 링크 길이나 관절 구조에 따라서 조인트 각과 엔드이펙터 위치/자세의 관계를 나타낸 것이고, dynamics는 관성, 중력, Coriolis, 마찰력 등의 힘과 링크 사이의 관계를 계산하는 것이다. kinematics는 제어기에서 주로 역기구학을 이용해서 task space에서의 명령을 joint space로 옮겨서 joint 위치 명령을 보낼 때 사용하고, dynamics는 joint에 토크 명령을 주려고 할 때 미리 필요할 것 같은 토크를 계산해서 feedforward로 보상해주거나 feedback linearization을 할 때 사용한다.

kinematics, dynamics의 원리, 수식, 알고리즘은 로보틱스 텍스트북에 잘 나와 있어서 직접 구현할 수도 있겠지만, 여기서는 유럽 쪽에서 orocos라는 ros와 비슷한 middleware에서 지원해주는 KDL library를 사용하는 법을 알아보자. KDL library는 ros에도 포팅되어 있으며 urdf 모델을 parsing할 수 있는 kdl_parser도 있어서 kinematics, dynamics를 계산할 때 필요한 링크 길이, Inertia 정보 등을 다시 작성할 필요없이 urdf 모델에서 불러올 수 있어서 매우 편리하다.

이번 페이지에서는 간단한 중력 보상 제어기 예제를 통해서 kdl library를 ros에서 사용하는 법을 알아보자.

중력 보상 제어기

중력 보상 제어기란 중력에 의해서 각 관절이 받는 힘을 계산해서 이 힘을 보상해주고 피드백 제어기를 적용하는 방법이다. 로봇암은 자세에 따라서 각 관절이 중력을 견뎌야 하는 토크가 달라지게 마련이고 모터 입장에서는 가만히 있으려고 해도 어떨 때는 1만큼의 힘이 필요한데, 어떨 대는 또 2만큼의 힘이 필요하게 되는 것이기 때문에 꽤 큰 외란이 된다. 이것을 피드백으로만 잡으려고 하면 오차가 이미 생겨야 복원하려는 힘이 생기므로 반응이 늦어지고 오차만으로 제어를 해야하기 때문에 게인이 커지게 된다. 따라서 중력에 의한 힘을 미리 계산해서 이를 보상해주고 그 나머지를 피드백 제어기가 담당하게 해서 부담을 줄여주는 것이다.

식으로 표현해보자. 로봇암의 동역학 식은 가속도에 의한 관성질량 term, Coriolis term, 중력 term 의 합이 Joint에 가하는 torque와 일치한다.

이 때, 제어 입력인 Joint torque에 중력과 피드백 제어 입력을 주면 중력 term은 상쇄되고 나머지만 외란으로 작용하고, 이것을 피드백 제어기가 잡아주는 것이다.

KDL

kdl을 ros로 포팅해주는 package는 kdl_parser이다. 현재 lunar까지 지원되는 것 같고, 테스트는 kinetic으로 해 보았다. kdl은 링크, 조인트 구조를 chain이라는 개념으로 받아들이는데 전체 로봇 urdf 모델 중에서 내가 제어할 때 필요한 segment만을 chain으로 입력받아서 그 chain의 root부터 tip까지 kinematics와 dynamics를 계산해준다. kdl은 vector, homogeneous transform 등 로보틱스 계산에 필요한 math 도 지원해주고, joint를 다룰 때 유용한 class 들도 제공해준다. 하지만, 문서가 좀 빈약해서 doxygen page를 참조하는 것이 좋다.

Dependency

의존성은 kdl_parser만 참조해주면 해결된다. package.xml에 kdl_parser를 포함해주고,

  <build_depend>kdl_parser</build_depend>
  <run_depend>kdl_parser</run_depend>

CMakeLists.txt에도 kdl_parser를 포함해준다.

find_package(catkin REQUIRED COMPONENTS
  ...
  kdl_parser
)

include_directories(include ${catkin_INCLUDE_DIRS})

catkin_package(
  CATKIN_DEPENDS
    ...
    kdl_parser
  LIBRARIES ${PROJECT_NAME}
)

소스 파일

필요한 파일들을 include해 준다. tree.hpp, kdl.hpp, chain.hpp, kdl_parser.hpp는 기본적으로 들어가고 chaindynparam.hpp는 kinematics를 풀지, dynamics를 풀지 또는 어떤 solver를 쓸지에 따라 달라진다.

#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl_parser/kdl_parser.hpp>

필요한 변수들은 KDL::Tree, KDL::Chain가 기본이고, solver는 필요에 따라 선택한다. 여기서는 Gravity torque vector를 계산해야 하므로 KDL::ChainDynParam을 solver로 선택했다. 그 외에 필요한 변수를 KDL::JntArray 를 이용해서 벡터 길이를 가변적으로 사용할 수 있다.

	// kdl
	KDL::Tree 	kdl_tree_;
	KDL::Chain	kdl_chain_;
	boost::scoped_ptr<KDL::ChainDynParam> id_solver_;	// inverse dynamics solver
	KDL::JntArray G_;					// gravity torque vector
	KDL::Vector gravity_;

	// pid gain
	KDL::JntArray Kp_, Ki_, Kd_;						// p,i,d gain
	KDL::JntArray q_error_integral_;
	double q_integral_min_;
	double q_integral_max_;

	// cmd, state
	KDL::JntArray tau_cmd_;
	KDL::JntArray q_cmd_, qdot_cmd_, qddot_cmd_;
	KDL::JntArray q_cmd_sp_;
	KDL::JntArray q_, qdot_;

	// limit
	KDL::JntArray q_limit_min_, q_limit_max_, qdot_limit_, qddot_limit_;

여기서는 사용하지 않았지만 KDL::JntArrayAcc 같은 class는 Joint pos, vel, acc를 모두 포함하는 자료형으로서 각각 선언해서 사용하기 번거로울 때 사용하기 좋다.

제어기 init 함수에서 urdf를 읽고 joint handle 을 읽어올 때, kdl chain도 만들어준다. treeFromUrdfModel, getChain 함수를 이용하면 몇 줄로 chain을 만들 수 있고, 이 때 들어가는 링크 이름이 root와 tip으로 그 사이가 모두 계산하는 구간이 된다. 마지막으로 solver는 chain과 중력 벡터를 입력으로 주고 생성한다.

	// kdl parser
	if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_)){
		ROS_ERROR("Failed to construct kdl tree");
		return false;
	}

	// kdl chain
	std::string root_name = "world";
	std::string tip_name = "elfin_link6";
	if(!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
	{
                return false;
	}	

	gravity_ = KDL::Vector::Zero();
	gravity_(2) = -9.81;
	G_.resize(n_joints_);	
			
	// inverse dynamics solver
	id_solver_.reset( new KDL::ChainDynParam(kdl_chain_, gravity_) );

제어기 로직을 계산하는 update 함수에서는 실제 중력 보상 제어기를 구현한다. JntToGravity함수에 현재 조인트값을 주면 중력 토크를 계산해주고 나머지 피드백 제어기를 pid로 구현하였다. KDL::JntArray는 eigen의 VectorXd형 data를 가지고 있어서 JntArray 자체에서 지원되지 않는 벡터 연산은 data에 직접 접근해서 사용하면 된다.

	// compute gravity torque
	id_solver_->JntToGravity(q_, G_);

	// error
	KDL::JntArray q_error;
	q_error.data = q_cmd_.data - q_.data;	// [to do] shortest distance using angle
	q_error_integral_.data += q_error.data;
			
	// integral saturation
	for (size_t i; i<n_joints_; i++)
	{
		if (q_error_integral_(i) > q_integral_max_)
			q_error_integral_(i) = q_integral_max_;
		else if (q_error_integral_(i) < -q_integral_max_)
			q_error_integral_(i) = -q_integral_max_;
	}

	// torque command
	tau_cmd_.data = G_.data + Kp_.data.cwiseProduct(q_error.data) - Kd_.data.cwiseProduct(qdot_.data) + Ki_.data.cwiseProduct(q_error_integral_.data);

	for(int i=0; i<n_joints_; i++)
	{
		joints_[i].setCommand(tau_cmd_(i));
	}