Endüstriyel robotlar, ISO 8373 standardına göre endüstriyel uygulamalarda kullanılan, üç veya daha fazla programlanabilir ekseni olan, otomatik kontrollü, yeniden programlanabilir, çok amaçlı, uzayda sabitlenmiş veya hareketli manipülatörlerdir. Bu robotlar, uç işlevci olarak takılmış yardımcı ekipmanlar ile birlikte, yüksek dayanıklılık, hız ve hassasiyet ile kaynak, boyama, montaj ve ürün denetimi ile testlerde kullanılmaktadır.
Robot kinematik modeli, robot kontrolü, dinamik analiz, yörünge planlama gibi konuları temel olarak etkilemektedir ve bir robotun eklemlerindeki açı veya pozisyon değişimleri ile uç işlevcisi arasındaki ilişkiyi tanımlamaktadır. Bu yazımızda, robotikte önemli bir yeri olan ileri ve ters kinematik analizin ROS kullanılarak nasıl uygulanacağını ele alacağız. KDL ve MoveIt! kütüphaneleri kullanılarak uygulanan kinematik çözücüler gösterilerek, KUKA KR6R900 endüstriyel robotu ile demo uygulamalarını göstereceğiz.
ROS ile Kinematik Çözücü Uygulaması
Unified Robot Description File (URDF)
URDF, bir robot modelini tanımlamak için kullanılan bir XML formatıdır. ROS çerçevesinde geliştirilen programlar, bir robotu tanımlamak için URDF formatını kullanmaktadır. Aşağıda, KUKA KR6R900 endüstriyel robot manipülatörün URDF modeli gösterilmiştir.
MoveIt!
MoveIt! güncel hareket planlama, manipülasyon, 3D algılama, kinematik ve kontrol algoritmalarını içeren bir paket yazılımdır. ROS çerçevesinde endüstriyel robot manipülatörler ile hareket planlaması uygulamalarında kullanılmaktadır. MoveIt! eklenti olarak birkaç farklı kinematik çözücü kullanmaktadır. Bunlardan biri OROCOS KDL kütüphanesidir. OROCOS KDL projesi, ileri yön ve ters kinematik çözücüleri ve dinamik çözücüleri içeren bir C++ kütüphanesidir ve python eklentileri de bulunmaktadır.
MoveIt! Paketi Oluşturma
MoveIt! robot konfigürasyonu oluşturmak için kullanılan "setup_asistant" adında bir arayüz içermektedir. Bu asistan yardımıyla robotun URDF modeli tanıtılarak, robota özel MoveIt! paketi oluşturulabilmektedir. Robotun uzuvları arasındaki çarpışma analizleri, planlanacak eklem gruplarının oluşturulması gibi adımlar bu arayüzde yapılmaktadır. Aşağıda, KUKA KR6R900 robotu tanıtılmış arayüzden bir kesit gösterilmiştir. Burada KDL eklentisi kinematik çözücü olarak seçilmiştir.
Robota özgü MoveIt! konfigürasyon paketi oluşturulduktan sonra, paket içindeki "demo.launch" dosyası çalıştırılarak MoveIt! paketi sanal eklem kontrolcüleri yardımıyla test edilebilir. Aşağıda, KUKA KR6R900 robot için oluşturulmuş paketin, farklı hedef noktalar için kinematik çözüm üretip üretmediği test edilmektedir.
İleri Yön Kinematik
İleri yön kinematiği, robotun verilen eklem konfigürasyonuna göre uç işlevcinin kartezyen uzaydaki pozunu bulmak için kullanılmaktadır. KDL paketi, robot ileri yön kinematiği için kinematik çözücüleri içermektedir. Bu çözücüler, robot URDF modelinden kinematik zincir yapısı oluşturmakta ve analitik olarak robotun ileri yön kinematiğini hesaplamaktadır.
KDL kütüphanesi kullanılarak KUKA KR6R900 robotu için ileri yön kinematiğini bulalım. Projenin tam haline buradan ulaşılabilir.
ForwardKinematicSolver::ForwardKinematicSolver(const ros::NodeHandle& nh) :
nh_(nh)
{
if(!nh_.getParam("robot_description", robot_desc_string_))
{
throw std::runtime_error("Robot description parameter couldn't find in the parameter server!");
}
nh_.param<std::string>("/base_link", base_link_, "base_link");
nh_.param<std::string>("/tip_link", tip_link_, "link_6");
KDL::Tree tree;
if(!kdl_parser::treeFromString(robot_desc_string_, tree))
{
throw std::runtime_error("Failed to construct KDL tree");
}
KDL::SegmentMap segments = tree.getSegments();
for(auto segment = segments.begin(); segment != segments.end(); segment++)
{
std::cout << segment->first << std::endl;
}
const int num_joints = tree.getNrOfJoints();
ROS_INFO("%d numbers of joints are used", num_joints);
if(!tree.getChain(base_link_, tip_link_, chain_))
{
throw std::runtime_error("Could not get chain!");
}
fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
}
Robot URDF modeli, kdl_parser::treeFromString() fonksiyonu ile kinematik zincir nesnesine dönüştürülmektedir. Robotun ileri yön kinematiğini çözmek için, bu kinematik zincir girdi olarak verilerek KDL::ChainFkSolverPos_recursive nesnesi oluşturulmaktadır.
geometry_msgs::PoseStamped ForwardKinematicSolver::calcFK(const std::vector<double> &joint_arr)
{
geometry_msgs::PoseStamped result;
KDL::JntArray q(joint_arr.size());
printf("joint_arr size:%zu\n", joint_arr.size());
for(int i = 0; i < joint_arr.size(); i++)
{
q.data[i] = joint_arr[i];
}
KDL::Frame result_f;
fk_solver_->JntToCart(q, result_f);
double roll, pitch, yaw;
result_f.M.GetRPY(roll, pitch, yaw);
double qx, qy, qz, qw;
result_f.M.GetQuaternion(qx, qy, qz, qw);
std::cout << "Position: " << result_f.p.data[0] << " " << result_f.p.data[1] << " " << result_f.p.data[2] << std::endl;
std::cout << "Orientation: " << roll << " " << pitch << " " << yaw << std::endl;
std::cout << "Quaternion: " << qx << ", " << qy << ", " << qz << ", " << qw << std::endl;
std::cout << "---------------------\n";
result.pose.position.x = result_f.p.data[0];
result.pose.position.y = result_f.p.data[1];
result.pose.position.z = result_f.p.data[2];
result.pose.orientation.x = qx;
result.pose.orientation.y = qy;
result.pose.orientation.z = qz;
result.pose.orientation.w = qw;
return result;
}
ChainFkSolverPos_recursive nesnesi JntToCart() fonksiyonu ile eklem değişkenlerini girdi olarak alarak uç işlevcinin kartezyen pozunu hesaplamaktadır. Program çalıştırıldığında aşağıdaki gibi uç işlevci pozunu hesaplamaktadır.
Ters Kinematik
Ters kinematik, robotun kartezyen uzayda istenen bir uç işlevci pozu için gerekli olan eklem konfigürasyonunun bulunmasında kullanılmaktadır. KDL kütüphanesi, ters kinematik çözümü için nümerik çözücüler içermektedir. Bunlardan biri Newton-Raphson yöntemini kullanan ChainIkSolverPos_NR kinematik çözücüsüdür. Bu yöntemde robot kinematik modelinin Jacobian matrisi kullanılarak hedef poza ulaşan eklem konfigürasyonu iteratif olarak aranmaktadır. Her iterasyonda eklem değişkenleri Jacobian matrisi kullanılarak ilerlemekte ve ileri yön kinematiği çözülerek hedef poza ulaşma hatası hesaplanmaktadır. Bu hata bir epsilon değerine ulaştığında çözüm bulunmaktadır.
KDL kütüphanesi kullanılarak KUKA KR6R900 robotu için ters kinematiğini bulalım. Projenin tam haline buradan ulaşılabilir.
InverseKinematicSolver::InverseKinematicSolver(const ros::NodeHandle& nh) :
nh_(nh)
{
if(!nh_.getParam("robot_description", robot_desc_string_))
{
throw std::runtime_error("Robot description parameter couldn't find in the parameter server!");
}
KDL::Tree tree;
if(!kdl_parser::treeFromString(robot_desc_string_, tree))
{
throw std::runtime_error("Failed to construct KDL tree");
}
KDL::SegmentMap segments = tree.getSegments();
for(auto segment = segments.begin(); segment != segments.end(); segment++)
{
std::cout << segment->first << std::endl;
}
const int num_joints = tree.getNrOfJoints();
ROS_INFO("%d numbers of joints are used", num_joints);
if(!tree.getChain("base_link", "link_6", chain_))
{
throw std::runtime_error("Could not get chain!");
}
// Set solver parameters
int maxIterations=100000; // solver max loops
double epsilon=0.01; // loop finish condition
double lambda=0.05; // used in WDLS algorithm;
auto Mx = Eigen::MatrixXd::Identity(6,1);
ik_solver_vel_.reset(new KDL::ChainIkSolverVel_wdls(chain_, epsilon, maxIterations));
ik_solver_vel_->setLambda(lambda);
ik_solver_vel_->setWeightTS(Mx);
fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
ik_solver_.reset(new KDL::ChainIkSolverPos_NR(chain_, *fk_solver_, *ik_solver_vel_, maxIterations, epsilon));
}
İleri yön kinematik çözümünde kullanıldığı gibi robot URDF modelinden kinematik zincir nesnesi elde edilmektedir. Robotun ters kinematiğini çözmek için, bu kinematik zincir girdi olarak verilerek KDL::ChainIkSolverPos_NR nesnesi oluşturulur. Bu nesne oluşturulurken, algoritmanın kullanması için ileri yön kinematik çözücü ve hız kinematiği çözücüler de girdi olarak verilmektedir. Algoritmanın maksimum iterasyon sayısı ve çözümün ulaşması gereken maksimum hata değeri epsilon da nesne oluşturulurken girdi olarak verilmektedir.
bool InverseKinematicSolver::calcIK(const geometry_msgs::PoseStamped &request,
std::vector<double> &result)
{
req_.M.Quaternion(request.pose.orientation.x,
request.pose.orientation.y,
request.pose.orientation.z,
request.pose.orientation.w);
req_.p = KDL::Vector(request.pose.position.x,
request.pose.position.y,
request.pose.position.z);
result.clear();
KDL::JntArray q_init(chain_.getNrOfJoints());
for(int i = 0; i < chain_.getNrOfJoints(); i++)
{
q_init.data[i] = 0.0;
}
q_init.data << 1.0, -0.1, 0.3, 0.4, 1.0, -1.40;
KDL::JntArray q_out(chain_.getNrOfJoints());
int id = ik_solver_->CartToJnt(q_init, req_, q_out);
if(id < 0)
{
ROS_ERROR("Error ik_solver:%d",id);
return false;
}
ROS_INFO("Result of inverse kinematic:");
for(int i = 0; i < chain_.getNrOfJoints(); i++)
{
std::cout << q_out.data[i] << std::endl;
result.push_back(q_out.data[i]);
}
return true;
}
ChainIkSolverPos_NR nesnesi, CartToJnt() fonksiyonu ile başlangıçta tahmin edilen eklem değişkenlerini ve hedef uç işlevci pozunu alarak, eklem değişkenlerini hesaplamaktadır. Program çalıştırıldığında verilen uç işlevci pozu için gerekli olan eklem açıları aşağıdaki gibi hesaplanmaktadır.
Ters kinematik birden fazla çözüm içermektedir ve yukarıda da görüldüğü gibi robotun eklem limitleri aşılmıştır. Bu limitlerin kontrolünün yapılması ve programatik olarak çözümün bulunması sağlanmalıdır. Bu işlemler MoveIt! içinde hazırlanmış KDL eklentisinde uygulanmıştır. Eklem limitlerine de uygun çözümler elde etmek için ters kinematiğini bir kez de MoveIt! paketi aracılığıyla KDL eklentisini kullanarak yapalım.
MoveIt! ile Ters Kinematik
MoveitPlanner::MoveitPlanner(const std::string& planning_group)
: planning_group_(planning_group)
{
spinner_.reset(new ros::AsyncSpinner(1));
spinner_->start();
robot_model_loader_ = RobotModelLoaderPtr(new robot_model_loader::RobotModelLoader("robot_description"));
robot_model_ = robot_model_loader_->getModel();
move_group_ = MoveGroupPtr(new moveit::planning_interface::MoveGroupInterface(planning_group_));
planning_frame_ = move_group_->getPlanningFrame();
eef_frame_ = move_group_->getEndEffectorLink();
ROS_INFO("Planning Frame: %s", move_group_->getPlanningFrame().c_str());
ROS_INFO("End effector link: %s", move_group_->getEndEffectorLink().c_str());
planning_scene_.reset(new planning_scene::PlanningScene(robot_model_));
ros::NodeHandle nh;
// Get planning scene object
planning_scene_interface_.reset(new moveit::planning_interface::PlanningSceneInterface(nh.getNamespace()));
joint_model_group_ = move_group_->getCurrentState()->getJointModelGroup(planning_group_);
robot_state_ = move_group_->getCurrentState();
}
Kuka KR6R900 robot için MoveIt! paketi oluştururken kullanılan planlanacak grup ismi verilerek MoveGroupInterface objesi oluşturulmaktadır. Bu obje ile robotun anlık konfigürasyonunu veren RobotState nesnesine erişim sağlanmaktadır. RobotState nesnesi ters kinematiği çözen fonksiyonları içermektedir. Projenin tam haline buradan ulaşılabilir.
bool MoveitPlanner::calcIK(const geometry_msgs::PoseStamped &request,
std::vector<double> &result)
{
bool stat = robot_state_->setFromIK(joint_model_group_, request.pose, move_group_->getEndEffectorLink(), 1, 10.0);
if(stat)
{
robot_state_->copyJointGroupPositions(planning_group_, result);
ROS_INFO("Inverse kinematics solution:");
for(auto j : result)
std::cout << j << std::endl;
}
else
ROS_ERROR("Inverse kinematics solution could not found");
return stat;
}
RobotState nesnesinin setFromIK() fonksiyonu kullanılarak istenen uç işlevci pozuna göre eklem değişkenleri hesaplanmaktadır. Bir önceki aynı hedef uç işlevci pozisyonu kinematik çözücüye verildiğinde, aşağıdaki gibi eklem limitlerini dikkate alan çözüm elde edilmektedir.
Özet
Bu yazımızda, endüstriyel robot manipülatörler için ileri yön ve ters kinematik analizlerinin ROS ile nasıl yapılacağına değindik. KDL kütüphanesindeki çözücüler kullanılarak KUKA KR6R900 endüstriyel robot manipülatörün ileri ve ters kinematiklerini inceledik. Bunun yanında hareket planlama paket yazılımı olan MoveIt! üzerinden KDL eklentisini kullanarak ters kinematik çözümü gerçekleştirdik.
Blogda kullanılan kodlar github üzerinden (https://github.com/AytacKahveci/kinematic_analysis) paylaşılmıştır.
