RL机器人人库使用简介

1. 主要模块介绍

     Hal : 硬件抽象层
Kin ：运动学算法库
Math : 数学算法库和数据结构
Mdl : 机器人模型库（模型管理和加载XML定义的机器人模型）
Plan : 运动规划库
Sg ：机器人模型可视化库（显示机器人3D模型和场景）
Std : 数据管理和内存管理库
Unit : 工具库（抽象操作系统接口和工具函数）
Xml ：xpath接口的xml文件库

2. 基本应用

2.1. 初始化

#define SCENEFILE "D:\\openCode\\robotlib\\rl-examples_urdf-model\\rlsg\\abb-irb-4600-205\\abb-irb-4600-205.xml"
#define MODELFILE "D:\\openCode\\robotlib\\rl-examples_urdf-model\\rlmdl\\abb-irb-4600-205.xml"SoQt::init(this);
SoDB::init();
SoGradientBackground::initClass();m_scene = std::make_shared<rl::sg::so::Scene>();
rl::sg::XmlFactory geometryFactory;
geometryFactory.load(SCENEFILE, m_scene.get());m_viewer = new SoQtExaminerViewer(ui.widget);
m_viewer->setSceneGraph(m_scene->root);
m_viewer->setTransparencyType(SoGLRenderAction::SORTED_OBJECT_BLEND);
m_viewer->viewAll();rl::mdl::XmlFactory kinematicFactory;
m_geometryModels = m_scene->getModel(0);//界面模型
m_dynamicModel= std::dynamic_pointer_cast<rl::mdl::Dynamic>(kinematicFactory.create(MODELFILE));//仿真模型

2.2 获取关节坐标

void QMainWidget::getJoinPos(QVector<double> &pos)
{pos.clear();rl::math::Vector values = m_dynamicModel->getPosition();Eigen::Matrix<rl::math::Units, Eigen::Dynamic, 1> units = m_dynamicModel->getPositionUnits();int n = values.rows();for(int i=0; i<n; i++){if (rl::math::Units::radian == units(i)){pos.append(values(i) * rl::math::constants::rad2deg);}else{pos.append(values(i));}}
}

2.3 获取世界坐标

void QMainWidget::getWorldPos(QVector<double> &pos)
{pos.clear();const rl::math::Transform::ConstTranslationPart& position = m_dynamicModel->getOperationalPosition(0).translation();rl::math::Vector3 orientation = m_dynamicModel->getOperationalPosition(0).rotation().eulerAngles(2, 1, 0).reverse();pos.append(position.x());pos.append(position.y());pos.append(position.z());pos.append(orientation.x() * rl::math::constants::rad2deg);pos.append(orientation.y() * rl::math::constants::rad2deg);pos.append(orientation.z() * rl::math::constants::rad2deg);
}

2.4 关节操作

这里更新第六关节，减去一个固定角度

m_dynamicModel->forwardDynamics();
rl::math::Vector values = m_dynamicModel->getPosition();
values[5] -= ANGLE_STEP;m_dynamicModel->setPosition(values);
m_dynamicModel->forwardPosition();
updatePosUI();
updataModelView();

2.5 世界坐标操作

机器人提供的基本操作是关节操作，使用世界坐标操作涉及机器人的逆变换，这里使用NloptInverseKinematics作为逆变换算法

 rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(m_dynamicModel.get());if (kinematic){rl::math::Transform x = kinematic->getOperationalPosition(0);//rl::math::Vector3 orientation = x.linear().eulerAngles(2, 1, 0).reverse();x.translation().x() = worldPos[0];x.translation().y() = worldPos[1];x.translation().z() = worldPos[2];x.linear() = (rl::math::AngleAxis(worldPos[5] * rl::math::constants::deg2rad, rl::math::Vector3::UnitZ()) *rl::math::AngleAxis(worldPos[4] * rl::math::constants::deg2rad, rl::math::Vector3::UnitY()) *rl::math::AngleAxis(worldPos[3] * rl::math::constants::deg2rad, rl::math::Vector3::UnitX())).toRotationMatrix();rl::math::Vector q = kinematic->getPosition();#ifdef RL_MDL_NLOPTrl::mdl::NloptInverseKinematics ik(kinematic);ik.seed(0);#elserl::mdl::JacobianInverseKinematics ik(kinematic);ik.setDelta(1);#endifik.clearGoals();ik.addGoal(x, 0);if(ik.solve()){kinematic->forwardPosition();}else{kinematic->setPosition(q);kinematic->forwardPosition();}

2.6 跟新界面显示

void QMainWidget::updataModelView()
{rl::mdl::Kinematic* kinematic = dynamic_cast<rl::mdl::Kinematic*>(m_dynamicModel.get());if (kinematic){for (std::size_t i = 0; i < m_geometryModels->getNumBodies(); ++i){m_geometryModels->getBody(i)->setFrame(kinematic->getBodyFrame(i));}}
}