2022-2-20 rbdl floating base 正运动学计算

HI_Forrest

已于 2022-02-22 14:55:10 修改

阅读量1.5k

点赞数 2

分类专栏： rbdl 运动学 VREP 文章标签：运动学

于 2022-02-21 22:37:15 首次发布

本文链接：https://blog.csdn.net/HITORANGE/article/details/123031536

版权

VREP 同时被 3 个专栏收录

9 篇文章 3 订阅

订阅专栏

运动学

6 篇文章 1 订阅

订阅专栏

rbdl

5 篇文章 3 订阅

订阅专栏

rbdl库

rbdl是组里用得比较多的运动学动力学库了，还没怎么研究过，看需求用的函数，现在搞移动机器人搭载机械臂的，整体建模的复合机器人模型，直接用rbdl进行运动学求解尝试。

rbdl 库 GitHub源码

rigid body dynamics library 官网

在这里插入图片描述

安装

windows通过vcpkg安装：vcpkg install rbdl-orb:x64-windows
在这里插入图片描述

urdf文件

通常可以用：SolidWorks通过urdf插件导出 urdf格式的机器人结构模型，rbdl 有urdf reader可以读取urdf文件建立模型，但是urdf里面的部分参数有可能未被读取？？？，需要注意使用。urdf文件也可以手写修改，其内容参考：urdf学习笔记和 ROS wiki 关于urdf的介绍进行学习。
在这里插入图片描述

rbdl 源码test解读

urdf reader

https://github.com/HIT-orange/rbdl/blob/master/addons/urdfreader/urdfreader.cc

这个reader 读取urdf文件时根据函数参数

URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose)

floating_base 为TRUE时自动通过model.append 函数修改了baselink连杆和世界之间的结构，有一个JOINTTYPEFLOATINGBASE 的joint连接，以及一个虚拟连杆，导入后模型树结构如图
在这里插入图片描述

namespace RigidBodyDynamics {

namespace Addons {

using namespace Math;

typedef vector<LinkPtr> URDFLinkVector;
typedef vector<JointPtr> URDFJointVector;
typedef map<string, LinkPtr > URDFLinkMap;
typedef map<string, JointPtr > URDFJointMap;

bool construct_model (Model* rbdl_model, ModelPtr urdf_model, bool floating_base, bool verbose) {
  LinkPtr urdf_root_link;

  URDFLinkMap link_map;
  link_map = urdf_model->links_;

  URDFJointMap joint_map;
  joint_map = urdf_model->joints_;

  vector<string> joint_names;

  // Holds the links that we are processing in our depth first traversal with the top element being the current link.
  stack<LinkPtr > link_stack;
  // Holds the child joint index of the current link
  stack<int> joint_index_stack;

  // add the bodies in a depth-first order of the model tree
  link_stack.push (link_map[(urdf_model->getRoot()->name)]);

  // add the root body
  ConstLinkPtr& root = urdf_model->getRoot ();
  Vector3d root_inertial_rpy;
  Vector3d root_inertial_position;
  Matrix3d root_inertial_inertia;
  double root_inertial_mass;

  if (root->inertial) {
    root_inertial_mass = root->inertial->mass;

    root_inertial_position.set (
        root->inertial->origin.position.x,
        root->inertial->origin.position.y,
        root->inertial->origin.position.z);

    root_inertial_inertia(0,0) = root->inertial->ixx;
    root_inertial_inertia(0,1) = root->inertial->ixy;
    root_inertial_inertia(0,2) = root->inertial->ixz;

    root_inertial_inertia(1,0) = root->inertial->ixy;
    root_inertial_inertia(1,1) = root->inertial->iyy;
    root_inertial_inertia(1,2) = root->inertial->iyz;

    root_inertial_inertia(2,0) = root->inertial->ixz;
    root_inertial_inertia(2,1) = root->inertial->iyz;
    root_inertial_inertia(2,2) = root->inertial->izz;

    root->inertial->origin.rotation.getRPY (root_inertial_rpy[0], root_inertial_rpy[1], root_inertial_rpy[2]);

    Body root_link = Body (root_inertial_mass,
        root_inertial_position,
        root_inertial_inertia);

    Joint root_joint (JointTypeFixed);
    if (floating_base) {
      root_joint = JointTypeFloatingBase; 
      //这一句有点坑，只修改了rootjoint为浮动关节，自由度数不修改，因此默认还是0. 
    }

    SpatialTransform root_joint_frame = SpatialTransform ();

    if (verbose) {
      cout << "+ Adding Root Body " << endl;
      cout << "  joint frame: " << root_joint_frame << endl;
      if (floating_base) {
        cout << "  joint type : floating" << endl;
      } else {
        cout << "  joint type : fixed" << endl;
      }
      cout << "  body inertia: " << endl << root_link.mInertia << endl;
      cout << "  body mass   : " << root_link.mMass << endl;
      cout << "  body name   : " << root->name << endl;
    }

    rbdl_model->AppendBody(root_joint_frame,
        root_joint,
        root_link,
        root->name); //appendbody，默认添加为世界的 child，rootjoint为空？？
  }
	//深度优先遍历？关节和连杆用栈 stack 结构来存储
  // depth first traversal: push the first child onto our joint_index_stack
  joint_index_stack.push(0);

  while (link_stack.size() > 0) {
    LinkPtr cur_link = link_stack.top();

    unsigned int joint_idx = joint_index_stack.top();

    // Add any child bodies and increment current joint index if we still have child joints to process.
    //第n-1个关节时，总关节数为 n，到末端n关节后一个空关节时停止
    if (joint_idx < cur_link->child_joints.size()) {
      JointPtr cur_joint = cur_link->child_joints[joint_idx];

      // increment joint index
      //临时栈，用一下就踢掉了？
      joint_index_stack.pop();
      joint_index_stack.push (joint_idx + 1);

      link_stack.push (link_map[cur_joint->child_link_name]);
      joint_index_stack.push(0);

      if (verbose) {
        for (unsigned int i = 1; i < joint_index_stack.size() - 1; i++) {
          cout << "  ";
        }
        cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << "' type = " << cur_joint->type << endl;
      }

      joint_names.push_back(cur_joint->name);
    } else {
      link_stack.pop();
      joint_index_stack.pop();
    }
  }

  unsigned int j;
  for (j = 0; j < joint_names.size(); j++) {
    JointPtr urdf_joint = joint_map[joint_names[j]];
    LinkPtr urdf_parent = link_map[urdf_joint->parent_link_name];
    LinkPtr urdf_child = link_map[urdf_joint->child_link_name];

    // determine where to add the current joint and child body
    unsigned int rbdl_parent_id = 0;

    rbdl_parent_id = rbdl_model->GetBodyId (urdf_parent->name.c_str());

    if (rbdl_parent_id == std::numeric_limits<unsigned int>::max())
      cerr << "Error while processing joint '" << urdf_joint->name
        << "': parent link '" << urdf_parent->name
        << "' could not be found." << endl;

    // create the joint
    Joint rbdl_joint;
    if (urdf_joint->type == urdf::Joint::REVOLUTE || urdf_joint->type == urdf::Joint::CONTINUOUS) {
      Vector3d axis (urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);

      if (fabs(axis.dot(Vector3d (1., 0., 0.)) - 1.0) < std::numeric_limits<double>::epsilon()) {
        rbdl_joint = Joint(JointTypeRevoluteX);
      } else if (fabs(axis.dot(Vector3d (0., 1., 0.)) - 1.0) < std::numeric_limits<double>::epsilon()) {
        rbdl_joint = Joint(JointTypeRevoluteY);
      } else if (fabs(axis.dot(Vector3d (0., 0., 1.)) - 1.0) < std::numeric_limits<double>::epsilon()) {
        rbdl_joint = Joint(JointTypeRevoluteZ);
      } else {
        rbdl_joint = Joint(JointTypeRevolute, axis);
      }
    } else if (urdf_joint->type == urdf::Joint::PRISMATIC) {
      Vector3d axis (urdf_joint->axis.x, urdf_joint->axis.y, urdf_joint->axis.z);
      rbdl_joint = Joint (JointTypePrismatic, axis);
    } else if (urdf_joint->type == urdf::Joint::FIXED) {
      rbdl_joint = Joint (JointTypeFixed);
    } else if (urdf_joint->type == urdf::Joint::FLOATING) {
      // todo: what order of DoF should be used?
      rbdl_joint = Joint (
          SpatialVector (0., 0., 0., 1., 0., 0.),
          SpatialVector (0., 0., 0., 0., 1., 0.),
          SpatialVector (0., 0., 0., 0., 0., 1.),
          SpatialVector (1., 0., 0., 0., 0., 0.),
          SpatialVector (0., 1., 0., 0., 0., 0.),
          SpatialVector (0., 0., 1., 0., 0., 0.));
    } else if (urdf_joint->type == urdf::Joint::PLANAR) {
      // todo: which two directions should be used that are perpendicular
      // to the specified axis?
      cerr << "Error while processing joint '" << urdf_joint->name << "': planar joints not yet supported!" << endl;
      return false;
    }

    // compute the joint transformation
    Vector3d joint_rpy;
    Vector3d joint_translation;
    urdf_joint->parent_to_joint_origin_transform.rotation.getRPY (joint_rpy[0], joint_rpy[1], joint_rpy[2]);
    joint_translation.set (
        urdf_joint->parent_to_joint_origin_transform.position.x,
        urdf_joint->parent_to_joint_origin_transform.position.y,
        urdf_joint->parent_to_joint_origin_transform.position.z
        );
    SpatialTransform rbdl_joint_frame =
          Xrot (joint_rpy[0], Vector3d (1., 0., 0.))
        * Xrot (joint_rpy[1], Vector3d (0., 1., 0.))
        * Xrot (joint_rpy[2], Vector3d (0., 0., 1.))
        * Xtrans (Vector3d (
              joint_translation
              ));

    // assemble the body
    Vector3d link_inertial_position;
    Vector3d link_inertial_rpy;
    Matrix3d link_inertial_inertia = Matrix3d::Zero();
    double link_inertial_mass = 0.;

    // but only if we actually have inertial data
    if (urdf_child->inertial) {
      link_inertial_mass = urdf_child->inertial->mass;

      link_inertial_position.set (
          urdf_child->inertial->origin.position.x,
          urdf_child->inertial->origin.position.y,
          urdf_child->inertial->origin.position.z
          );
      urdf_child->inertial->origin.rotation.getRPY (link_inertial_rpy[0], link_inertial_rpy[1], link_inertial_rpy[2]);

      link_inertial_inertia(0,0) = urdf_child->inertial->ixx;
      link_inertial_inertia(0,1) = urdf_child->inertial->ixy;
      link_inertial_inertia(0,2) = urdf_child->inertial->ixz;

      link_inertial_inertia(1,0) = urdf_child->inertial->ixy;
      link_inertial_inertia(1,1) = urdf_child->inertial->iyy;
      link_inertial_inertia(1,2) = urdf_child->inertial->iyz;

      link_inertial_inertia(2,0) = urdf_child->inertial->ixz;
      link_inertial_inertia(2,1) = urdf_child->inertial->iyz;
      link_inertial_inertia(2,2) = urdf_child->inertial->izz;

      if (link_inertial_rpy != Vector3d (0., 0., 0.)) {
        cerr << "Error while processing body '" << urdf_child->name << "': rotation of body frames not yet supported. Please rotate the joint frame instead." << endl;
        return false;
      }
    }

    Body rbdl_body = Body (link_inertial_mass, link_inertial_position, link_inertial_inertia);

    if (verbose) {
      cout << "+ Adding Body: " << urdf_child->name << endl;
      cout << "  parent_id  : " << rbdl_parent_id << endl;
      cout << "  joint frame: " << rbdl_joint_frame << endl;
      cout << "  joint dofs : " << rbdl_joint.mDoFCount << endl;
      for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) {
        cout << "    " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl;
      }
      cout << "  body inertia: " << endl << rbdl_body.mInertia << endl;
      cout << "  body mass   : " << rbdl_body.mMass << endl;
      cout << "  body name   : " << urdf_child->name << endl;
    }

    if (urdf_joint->type == urdf::Joint::FLOATING) {
      Matrix3d zero_matrix = Matrix3d::Zero();
      Body null_body (0., Vector3d::Zero(3), zero_matrix);
      Joint joint_txtytz(JointTypeTranslationXYZ);
      string trans_body_name = urdf_child->name + "_Translate";
      rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, joint_txtytz, null_body, trans_body_name);

      Joint joint_euler_zyx (JointTypeEulerXYZ);
      rbdl_model->AppendBody (SpatialTransform(), joint_euler_zyx, rbdl_body, urdf_child->name);
    } else {
      rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child->name);
    }
  }

  return true;
}

RBDL_DLLAPI bool URDFReadFromFile (const char* filename, Model* model, bool floating_base, bool verbose) {
  ifstream model_file (filename);
  if (!model_file) {
    cerr << "Error opening file '" << filename << "'." << endl;
    abort();
  }
  
  // reserve memory for the contents of the file
  string model_xml_string;
  model_file.seekg(0, std::ios::end);
  model_xml_string.reserve(model_file.tellg());
  model_file.seekg(0, std::ios::beg);
  model_xml_string.assign((std::istreambuf_iterator<char>(model_file)), std::istreambuf_iterator<char>());

  model_file.close();

  return URDFReadFromString (model_xml_string.c_str(), model, floating_base, verbose);
}

RBDL_DLLAPI bool URDFReadFromString (const char* model_xml_string, Model* model, bool floating_base, bool verbose) {
  assert (model);

  ModelPtr urdf_model = urdf::parseURDF (model_xml_string);
 
  if (!construct_model (model, urdf_model, floating_base, verbose)) {
    cerr << "Error constructing model from urdf file." << endl;
    return false;
  }

  model->gravity.set (0., 0., -9.81);

  return true;
}

}

}

直接api 建模

参考：
https://github.com/HIT-orange/rbdl/blob/master/tests/FloatingBaseTests.cc

从new model 新建一个 floatingbase的过程如下：


  Model *model;
  Body base;
  unsigned int base_body_id;
  
  model = new Model;
  model->gravity = Vector3d (0., -9.81, 0.);
  base = Body (1., Vector3d (1., 0., 0.), Vector3d (1., 1., 1.));
    
  
base_body_id = model->AddBody (0, SpatialTransform(), 
      Joint (
        SpatialVector (0., 0., 0., 1., 0., 0.),
        SpatialVector (0., 0., 0., 0., 1., 0.),
        SpatialVector (0., 0., 0., 0., 0., 1.),
        SpatialVector (0., 0., 1., 0., 0., 0.),
        SpatialVector (0., 1., 0., 0., 0., 0.),
        SpatialVector (1., 0., 0., 0., 0., 0.)
        ),
      base);	
//初始化基座运动参数为0：
  q = VectorNd::Constant(model->dof_count, 0.);
  qdot = VectorNd::Constant(model->dof_count, 0.);
  qddot = VectorNd::Constant(model->dof_count, 0.);
  tau = VectorNd::Constant(model->dof_count, 0.);
//计算当前基座位姿
CalcBaseToBodyCoordinates (*model, q, base_body_id, Vector3d (0., 0., 0.), false);
  q[1] = 1.; //基座绕y轴平移1米
  //ForwardDynamics (*model, q, qdot, tau, qddot); 官方api，测试失败不知道为啥？
//计算基座运动后，基座在世界坐标系下的位姿  
  Vector3d test_point;
  test_point = CalcBaseToBodyCoordinates (*model, q, base_body_id, Vector3d (0., 0., 0.), false);

6轴机械臂浮动基座正运动学test

先读取urdf文件并测试初始关节角的正运动学结果
在这里插入图片描述

base坐标系和joint坐标系变换

模型中的 q 现有12自由度，前六个表示基座的XYZ平移以及绕ZYX旋转的角度（ZYX欧拉角）
测试从世界坐标系到各连杆关节坐标系
以及从关节坐标系到世界坐标系的变换
在这里插入图片描述

calcbodytobasecoordinates()

各关节坐标系的原点在世界坐标系的坐标为
在这里插入图片描述
基本符合vrep关节角变化后的关节位置

calcbasetobodycoordinates()

在这里插入图片描述

基座移动后

基座平移X+1

基座向X轴平移1米后，机械臂末端位姿为
在这里插入图片描述
对比初始值：

末端位姿只有x轴增加了1。

坐标变换 body to base

在这里插入图片描述

坐标变换 base to body

在这里插入图片描述

***浮动基座的model tree

可见浮动基座的改变发生在世界坐标系root 和虚拟body 之间。root 的id是0，虚拟body的link id 是1 ， baselink 的id 是2 ，（对比：固定基座的root是id 0，之后是link1 ，没有虚拟link和baselink）
浮动基：
在这里插入图片描述
固定基座：

多轴平移XYZ

浮动基座 {1,2,-3,0,0,0}结果:
在这里插入图片描述
参考初始：

body to base

在这里插入图片描述

基座转动

多自由度关节用四元数表旋转

根据源码：
在这里插入图片描述
浮动基座分为XYZ平移关节和球形关节，转动用四元数表示，q 中参数的顺序如下

读取urdf后输出 q 的大小，有一个floatingbase 即一个球形关节，四元数表三维旋转，最后加一个quaternion.w

绕z轴转动90°

转动之前：末端位姿以及各关节在世界坐标系位置
在这里插入图片描述

转动之后：末端位姿以及各关节在世界坐标系位置
在这里插入图片描述

复合运动

    rbdlmath::VectorNd basepose(7);
    rbdlmath::Matrix3d mat = rbdlmath::rotz(M_PI/2);
    rbdlmath::Quaternion quat = rbdlmath::Quaternion::fromMatrix(mat);
    cout << "get quaternion from mat .. "<< endl;
    
    float base[7]={1,2,-3,(float)quat.x(),(float)quat.y(),(float)quat.z(),(float)quat.w()};
    
    floattovector(base,basepose);
    m_dyn.setfloatbase(basepose);
    m_dyn.getEndPose(pos,ori);
    m_dyn.updatelinks();
    vrep_setrbdllinks();

浮动基座平移{1,2，-3}，同时绕z轴和x轴旋转90°后：
在这里插入图片描述

HI_Forrest

关注

2
点赞
踩
13

收藏

觉得还不错? 一键收藏
1
评论
2022-2-20 rbdl floating base 正运动学计算

RBDL处理浮动基座机械臂rbdl库urdf文件rbdl 源码test解读urdf reader直接api 建模rbdl库rbdl是组里用得比较多的运动学动力学库了，还没怎么研究过，看需求用的函数，现在搞移动机器人搭载机械臂的，整体建模的复合机器人模型，直接用rbdl进行运动学求解尝试。rbdl 库 GitHub源码rigid body dynamics library 官网urdf文件通常可以用：SolidWorks通过urdf插件导出 urdf格式的机器人结构模型，rbdl 有urdf
复制链接

扫一扫