2022-4-15 rbdl urdfreader 模型读取为floatingbase ？

最新推荐文章于 2024-07-14 12:30:49 发布
最新推荐文章于 2024-07-14 12:30:49 发布
阅读量625 收藏 2
点赞数
分类专栏： rbdl 学习笔记 文章标签： c++
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/HITORANGE/article/details/124186892
版权

之前测试的正常版，使用源码改urdfreader
在这里插入图片描述
使用rbdl库函数版：
在这里插入图片描述
id正确，但是结构树父子关系未建立，浮动基座与机械臂未连接。

之前改过rbdl的urdfreader所以好用了，未记录修改的地方

修正版urdfreader.h:
.pro 里面加上./rbdl-orb-3.0.0/addons/urdfreader/thirdparty\

#ifndef URDFREADER_H
#define URDFREADER_H

#include <rbdl/rbdl.h>
#include <rbdl/rbdl_utils.h>

#include "rbdl/rbdl_mathutils.h"
#include "rbdl/Logging.h"

#include "rbdl/Model.h"
#include "rbdl/Kinematics.h"
#include "rbdl/Dynamics.h"
#include <assert.h>
#include <iostream>
#include <fstream>
#include <map>
#include <stack>

#include <rbdl/rbdl_config.h>
#include <addons/urdfreader/thirdparty/urdf/urdfdom_headers/urdf_model/include/urdf_model/model.h>
#include <addons/urdfreader/thirdparty/urdf/urdfdom/urdf_parser/include/urdf_parser/urdf_parser.h>
using namespace RigidBodyDynamics;
using namespace RigidBodyDynamics::Math;
using namespace std;

typedef my_shared_ptr<urdf::Link> LinkPtr;
typedef const my_shared_ptr<const urdf::Link> ConstLinkPtr;
typedef my_shared_ptr<urdf::Joint> JointPtr;
typedef my_shared_ptr<urdf::ModelInterface> ModelPtr;


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

bool urdfmodeling (Model* rbdl_model, ModelPtr urdf_model, bool floating_base, bool verbose=false);
bool urdfreadfromfile (const char* filename, Model* model, bool floating_base, bool verbose=false);
bool urdfreadfromstring (const char* model_xml_string, Model* model, bool floating_base, bool verbose=false);



bool urdfmodeling (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)]);
  cout<<"link stack : " << link_stack.top()->name <<endl;

  // add the root body
  cout << "get root body " <<endl;
  ConstLinkPtr& root = urdf_model->getRoot ();
  cout << "root link's name: " << root->name<<endl;
  Vector3d root_inertial_rpy;
  Vector3d root_inertial_position;
  Matrix3d root_inertial_inertia;
  double root_inertial_mass;

  cout<<"set root body inertial..."<<endl;
  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]);
    cout << "set root body as baselink ..."<<endl;
    Body root_link = Body (root_inertial_mass,
        root_inertial_position,
        root_inertial_inertia);

    cout << "get root joint ..."<<endl;
    Joint root_joint (JointTypeFixed);

    if (floating_base) {
      root_joint = JointTypeFloatingBase;
      cout <<endl<<endl<<"-------------   "<< "setting the robot base link as floating base "<<endl;
    }

    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);
  }

  // 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.
    if (joint_idx < cur_link->child_joints.size()) {
      JointPtr cur_joint = cur_link->child_joints[joint_idx];
      cout << "traversal to joint index : " << cur_link->child_joints.size() <<endl;

      // 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;
}

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();
    cout<<" urdf read from file... " <<endl;
  return urdfreadfromstring (model_xml_string.c_str(), model, floating_base, verbose);
}

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

  ModelPtr urdf_model = urdf::parseURDF (model_xml_string);
  cout << " urdf read from string ...." <<endl;
  if (!urdfmodeling (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;
}












#endif // URDFREADER_H
HI_Forrest
关注
专栏目录
六轴机器臂URDF文件
12-23
6轴机器臂URDF文件，通过NX11建模，然后导入到SolidWorks2014中，通过URDF插件导出，可在linux下ROS中的rviz和moveit显示，经测过可以使用，具体方法请参考“ROS学习笔记（1）自定义6轴机器臂的URDF文件的生成并用rviz和moveit显示”内容
使用C++编辑urdf文件
南北粉面馆
01-25 769
因为我想在代码里修改并保存urdf文件，参考2个开源代码，实现待更新 https://github.com/ilidar/urdf_editor https://github.com/rhololkeolke/robot_editor
【RBDL库的构建与编译】
友善的橘子
04-22 2276
RBDL（Rigid Body Dynamics Library，刚体动力学库）是一个开源的动力学库，用于建模和模拟多体动力学系统，例如机器人，人体运动和其他机械系统。RBDL提供了基于牛顿-欧拉方法的运动学和动力学分析，可以计算机器人或多体系统的位置，速度，加速度和力矩等动力学变量。RBDL还提供了静态和动态力学分析工具，可以帮助研究人员评估系统的能量和力矩。RBDL可以在C++和MATLAB中使用，并且易于扩展和自定义。这是个人搭建RBDL开发环境的开发记录，也是本系列文章的第一篇。
【ROS2】中级:URDF-添加物理和碰撞属性
最新发布
cxyhjl的博客
07-14 861
目标：学习如何为连杆（link-被翻译为链接）添加碰撞和惯性属性，以及如何为关节添加关节动力学。教程级别：中级时间：10 分钟目录碰撞物理性质惯性接触系数关节动力学其他标签下一步在本教程中，我们将学习如何为您的 URDF 模型添加一些基本的物理属性以及如何指定其碰撞属性。碰撞到目前为止，我们只用一个子元素visual指定了我们的链接，这个子元素定义了机器人看起来的样子（这并...
ubuntu16.04下ROS操作系统学习笔记（四 ）机器人系组成、URDF机器人建模、xacro模型优化
小小何先生的学习之旅
11-11 2723
机器人的定义与组成 机器人系统构建 机器人系统的构建主要有以下几点： 执行机构的实现：轮子、电机、舵机等。 驱动系统的实现：电源子系统、电机驱动子系统、传感器接口等。 内部传感系统的实现：机器人里程计： 通过电机带动光删计数，实现对机器人运动速度的求取，然后计算积分得到里程。惯性测量单元(IMU):测量物体速度姿态，主要包括三轴陀螺仪、三轴加速...
(ROS C++系列 NO.2) 机器人模型URDF文件学习整理
fly_dzy666的博客
10-13 365
前言 ROS系列学习均主要参考《ROS机器人编程 原理与应用》这本书，作者为Wyatt S. Newman。 0. 需要理解的点 (1) 每一个连杆都有一个参考坐标系，随着连杆在空间中移动而移动（注：不一定在连杆主体中） (2) URDF中每个连杆都有唯一的父杆，引入关节坐标系描述两者的相对运动。 (3) origin的xyz描述（相对父参考坐标系）位置，rpy描述（相对父参考坐标系）方向。 (4) axis描述该关节的关节轴与所定义的关节坐标系的对齐情况。 (5) 定义关节后，子杆的参考坐标系隐式跟随。T
Nginx越界读取缓存漏洞 CVE-2017-7529
03-13
而如果我构造了两个负的位置，如(-600, -9223372036854774591)，将可能读取到负位置的数据。如果这次请求又命中了缓存文件，则可能就可以读取到缓存文件中位于“HTTP返回包体”前的“文件头”、“HTTP返回包头”等...
TVP-VAR模型的MATLAB代码
05-18
这些代码可能包括数据读取、模型设定、参数估计、IRF和方差分解等部分。用户可以直接运行这些代码，通过修改变量名和数据文件路径，适应自己的研究需求。 **使用注意事项** 1. 确保MATLAB版本兼容：不同的MATLAB...
S7-200_时间设定与读取.zip西门子PLC编程实例程序源码下载
04-19
S7-200_时间设定与读取.zip西门子PLC编程实例程序源码下载S7-200_时间设定与读取.zip西门子PLC编程实例程序源码下载S7-200_时间设定与读取.zip西门子PLC编程实例程序源码下载S7-200_时间设定与读取.zip西门子PLC编程...
rddata.m -- MIT-BIH ECG 信号的数据读取Matlab程序
06-21
MIT-BIH ECG 信号的数据读取Matlab程序，能够读取MIT-BIH数据库 .atr、.dat、.hea三种文件的数据，根据这些数据计算出实际的心电信号值，并绘制出信号波形。
c#读取DLT645-2007电力协议，项目源码
10-04
4. 报文解析：DLT645-2007协议中的数据是以报文的形式传输的，每个报文包含地址域、控制域、数据域和校验域。解析报文时，需要正确识别每个域的含义，例如地址域标识电表，控制域指示操作类型，数据域存储测量值，...
C++读取STL模型文件
热门推荐
在云端
01-01 2万+
数据格式二进制的数据格式：二进制STL文件用固定的字节数来给出三角面片的几何信息。 【80】文件起始的80个字节是文件头，用于存贮文件名 【4】紧接着用 4 个字节的整数来描述模型的三角面片个数(小端存储) 【50*n】后面逐个给出每个三角面片的几何信息。每个三角面片占用固定的50个字节(小端存储)，依次是: 【12】3个4字节浮点数(角面片的法矢量) 【12】3个4字节浮点数(1个顶点的坐标)
windows-rbdl-vs2019
HITORANGE的博客
11-05 687
根据源码提示，在项目属性-c+±预处理器-预处理定义中加入定义 #ifndef RBDL_BUILD_ADDON_URDFREADER #error "Error: RBDL addon URDFReader not enabled." #endif 将源码中需要的文件夹路径加入c++常规中的附加包含目录 D:\vcpkg-master\buildtrees\rbdl\src\v2.6.0-4ce2ac26ed.clean\addons\urdfreader ...
2022-2-20 rbdl floating base 正运动学计算
HITORANGE的博客
02-21 1756
RBDL处理浮动基座机械臂rbdl库urdf文件rbdl 源码test解读urdf reader直接api 建模 rbdl库 rbdl是组里用得比较多的运动学动力学库了，还没怎么研究过，看需求用的函数，现在搞移动机器人搭载机械臂的，整体建模的复合机器人模型，直接用rbdl进行运动学求解尝试。 rbdl 库 GitHub源码 rigid body dynamics library 官网 urdf文件 通常可以用：SolidWorks通过urdf插件导出 urdf格式的机器人结构模型，rbdl 有urdf
弹出浮层基类 BaseFloating 封装
ldhj1993的博客
08-11 733
之前在一个技术群里面尤为同学请教大家这种弹出窗怎么做（如下图所示），他是用的PopWindow实现的，但是PopWindow有很多地方不符合他的要求，比如弹出的起始位置，弹出的动画效果。我们项目中也会有这种弹出浮层的需求，举个例子，下面是仿淘宝选择规格的弹出浮层效果，不仅仅是这一处，整个项目中也会有很多类似的弹出浮层效果，为了方便，我们就需要有一个弹出浮层基类，来简化我们的代码。
1.1 ROS中新建机器人模型（urdf And xacro并用rviz显示
u010807272的博客
10-25 1072
1.1 ROS中新建机器人模型（urdf And xacro并用rviz显示新建workspace 和 packageurdf 篇xacro篇 基于以下博客修改并批注：可参考 https://blog.csdn.net/ktigerhero3/article/details/64439815 https://blog.csdn.net/ktigerhero3/article/details/649...
2022-2-22 rbdl 浮动基座 正运动学计算 姿态变换
HITORANGE的博客
02-22 689
vrep里的姿态表示欧拉角规则 右手规则规则！！！！！！！！！！！！！！！！！！！！！ eigen 库函数matrix3d.eularangle 四元数转旋转矩阵后 转欧拉角可能两个解 eularangle(0,1,2) 左手规则旋转！！！！！！！！！！！！！！！ 四元数转欧拉角2π内 eigen库旋转的变换 左手变换转右手变换 旋转矩阵的转置 的意义 手推的正运动学姿态变换： ////* forward kinematics *//// if(floatingbase) {
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值