【Pinocchio】加载和处理一个URDF模型文件，并进行正向运动学计算

最新推荐文章于 2025-04-13 00:54:30 发布

十年一梦实验室

最新推荐文章于 2025-04-13 00:54:30 发布

阅读量604

点赞数 9

文章标签： java 前端数据库服务器 linux

本文链接：https://blog.csdn.net/cxyhjl/article/details/145695383

版权

Python

这段代码主要使用 pinocchio 库来加载和处理一个URDF模型文件，并进行正向运动学计算。代码的主要步骤包括：

导入必要的模块和库，如 pathlib、sys 和 pinocchio。
定义一个路径，指向 Pinocchio 模型所在的目录。
根据传入的参数或默认路径，设置 URDF 文件的路径。
加载URDF模型，并打印模型名称。
创建一个数据对象，用于存储算法需要的数据。
生成一个随机配置，并打印该配置。
对运动学树进行正向运动学计算。
打印运动学树中每个关节的位置。

这样做的目的是为了在模型中生成并计算机器人各个关节的位置和姿态。

from pathlib import Path  # 从pathlib模块中导入Path类from sys import argv  # 从sys模块中导入argv变量
import pinocchio  # 导入pinocchio库
# 这个路径指向Pinocchio源代码目录，但你可以在这里定义你自己的目录pinocchio_model_dir = Path(__file__).parent.parent / "models"
# 你应该在这里设置你自己的URDF文件，或者将其作为这个例子的参数传递urdf_filename = (    pinocchio_model_dir / "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf"    if len(argv) < 2  # 如果argv长度小于2    else argv[1]  # 否则使用传入的参数)
# 加载urdf模型model = pinocchio.buildModelFromUrdf(urdf_filename)print("model name: " + model.name)  # 打印模型名称
# 创建算法所需的数据data = model.createData()
# 生成一个随机配置q = pinocchio.randomConfiguration(model)print(f"q: {q.T}")  # 打印随机配置
# 在运动学树上进行正向运动学pinocchio.forwardKinematics(model, data, q)
# 打印运动学树中每个关节的位置for name, oMi in zip(model.names, data.oMi):    print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat))

C++

这段代码主要使用 pinocchio 库来加载和处理一个URDF模型文件，并进行正向运动学计算。代码的主要步骤包括：

导入必要的头文件，如 pinocchio 的URDF解析器、关节配置算法和运动学算法，以及标准输入输出流。
定义一个路径，指向 Pinocchio 模型所在的目录。如果未定义路径，则使用默认路径。
根据传入的参数或默认路径，设置 URDF 文件的路径。
加载URDF模型，并打印模型名称。
创建一个数据对象，用于存储算法需要的数据。
生成一个随机配置，并打印该配置。
对运动学树进行正向运动学计算。
打印运动学树中每个关节的位置。

这样做的目的是为了在模型中生成并计算机器人各个关节的位置和姿态。

#include "pinocchio/parsers/urdf.hpp"  // 包含pinocchio的urdf解析器头文件
#include "pinocchio/algorithm/joint-configuration.hpp"  // 包含关节配置算法头文件#include "pinocchio/algorithm/kinematics.hpp"  // 包含运动学算法头文件
#include <iostream>  // 包含标准输入输出流头文件
// PINOCCHIO_MODEL_DIR 是由CMake定义的，但你可以在这里定义你自己的目录#ifndef PINOCCHIO_MODEL_DIR  #define PINOCCHIO_MODEL_DIR "path_to_the_model_dir"  // 如果未定义PINOCCHIO_MODEL_DIR，则定义其路径#endif
int main(int argc, char ** argv)  // 主函数，argc和argv用于传递命令行参数{  using namespace pinocchio;  // 使用pinocchio命名空间
  // 你应该在这里设置你自己的URDF文件，或者将其作为这个例子的参数传递  const std::string urdf_filename =    (argc <= 1) ? PINOCCHIO_MODEL_DIR  // 如果命令行参数个数小于等于1                    + std::string("/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf")  // 使用默认的URDF文件路径                : argv[1];  // 否则使用传入的第一个参数作为URDF文件路径
  // 加载urdf模型  Model model;  pinocchio::urdf::buildModel(urdf_filename, model);  // 从URDF文件中构建模型  std::cout << "model name: " << model.name << std::endl;  // 输出模型名称
  // 创建算法所需的数据  Data data(model);
  // 生成一个随机配置  Eigen::VectorXd q = randomConfiguration(model);  // 随机生成一个关节配置  std::cout << "q: " << q.transpose() << std::endl;  // 输出关节配置
  // 在运动学树上进行正向运动学  forwardKinematics(model, data, q);
  // 打印运动学树中每个关节的位置  for (JointIndex joint_id = 0; joint_id < (JointIndex)model.njoints; ++joint_id)    std::cout << std::setw(24) << std::left << model.names[joint_id] << ": " << std::fixed              << std::setprecision(2) << data.oMi[joint_id].translation().transpose() << std::endl;}