从零上手机器人动力学库pinocchio（一）pinocchio简介

carrot1128

已于 2023-12-25 19:14:31 修改

阅读量4.3k

点赞数 20

分类专栏： pinocchio pybullet 文章标签：笔记

于 2023-12-18 16:00:23 首次发布

本文链接：https://blog.csdn.net/carrot1128/article/details/132796851

版权

pinocchio 同时被 2 个专栏收录

1 篇文章 2 订阅

订阅专栏

pybullet

1 篇文章 0 订阅

订阅专栏

一、安装

二、Pinocchio中实现的主要算法

三、官方指南

四、pinocchio与pybullet联调

GitHub - stack-of-tasks/pinocchio: A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivativesA fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives - GitHub - stack-of-tasks/pinocchio: A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivativeshttps://github.com/stack-of-tasks/pinocchio

一、安装

github明确给出了安装步骤和指令，非常简单，根据需求二选一即可。

系统：ubutntu 20.04

安装指令：

conda安装：

conda install pinocchio -c conda-forge

pip 安装：

pip install pin  # 只适用于Linux

部署在ROS上，通过以下指令可以安装具有 HPP-FCL 支持和 Python 绑定的 Pinocchio：

sudo apt install ros-$ROS_DISTRO-pinocchio

详细步骤请参考上方github链接。

官方配套可视化工具：

pinocchio支持许多开源可视化工具，根据需求安装适合自己的即可，我装的是gepetto viewer。

可视化工具
Gepetto Viewer	conda安装指令：conda install gepetto-viewer gepetto-viewer-corba -c conda-forge 与pinocchio一起使用时需要先在终端打开gepetto： gepetto-gui 基于FCL方法实现了运动树的碰撞和距离算法。
Panda3d	pip安装：pip install panda3d_viewer
Meshcat	pip安装：pip install meshcat 跟Panda3d一样，通过嵌入式浏览器实现交互
RViz	可以与其他ROS包交互

二、Pinocchio中实现的主要算法

Pinocchio提供基于revisited Roy Featherstone's algorithms的多关节系统刚体算法，提供主要刚体算法的解析导数（例如：Recurisive Newton-Euler Algorithm或Articulated-Body Algorithm），详见官方论文。

The Pinocchio C++ library : A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives | IEEE Conference Publication | IEEE Xplorehttps://ieeexplore.ieee.org/abstract/document/8700380?casa_token=iavz9sD7eccAAAAA:Bo6MXDNsxCOKO6NxCX5SEn_8qpGNsYXhoO3GfYqGlfGOFkNE8j22aBf4J0PfLcQ1e0dBV9xX实现的主要算法：

正向运动学（forward kinematics）：给定机器人构型，计算每个关节的空间位置并将其存储为坐标变换。如果给定速度或加速度，将计算每个关节的空间速度（以局部坐标系表示）。
运动学雅可比矩阵（kinematic jacobian）：在机械臂运动学中用来计算机械臂末端执行器的速度与各个关节运动速度之间的关系。
逆动力学（inverse dynamics）：采用Recursive Newton-Euler Algorithm (RNEA) 计算逆动力学。即给定所需的机器人构型、速度、加速度，计算并存储执行该运动所需的扭矩。
关节空间惯性矩阵（Joint space inertia matrix）：采用Compostie rigid body algortihm (CRBA)算法，计算关节空间惯性矩阵。
前向动力学（forward dynamics）：采用Articulated Body Algorithm（ABA）计算无约束的前向动力学。即给定机器人构型、速度、扭矩和外力的情况下，可以计算出由此产生的关节加速度。
其他算法：其他算法包括约束正运动学，脉冲动力学，逆关节空间惯性矩阵，向心动力学。

三、官方指南

pinocchio: Overviewhttps://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/index.html#OverviewConclu

四、pinocchio与pybullet联调

参考项目：github repo：

GitHub - KanishkNavale/Robot-Physics-Engine: This repository implements an optimization strategy for singularity free trajectory generation for the serial robots.This repository implements an optimization strategy for singularity free trajectory generation for the serial robots. - GitHub - KanishkNavale/Robot-Physics-Engine: This repository implements an optimization strategy for singularity free trajectory generation for the serial robots.https://github.com/KanishkNavale/Robot-Physics-Engine GitHub - nyu-legged-group/pinocchio_bulletContribute to nyu-legged-group/pinocchio_bullet development by creating an account on GitHub.https://github.com/nyu-legged-group/pinocchio_bullet

https://github.com/machines-in-motion/mim_robotshttps://github.com/machines-in-motion/mim_robots———————————————— 分割线 2023.12.18——————————————————

pinocchio运动学计算及参数更新机制：

1. 导入URDF模型，创建数据模型。注意，model为刚体的模型结构，data存储了刚体系统的数据结构，当调用forwardKinematics()函数计算正向运动学时，函数根据当前关节位置、速度和加速度，更新data中存储的关节位置、空间速度和空间加速度。

import pinocchio
from sys import argv
from os.path import dirname, join, abspath
 
# This path refers to Pinocchio source code but you can define your own directory here.
pinocchio_model_dir = join(dirname(dirname(str(abspath(__file__)))), "models")
 
# You should change here to set up your own URDF file or just pass it as an argument of this example.
urdf_filename = pinocchio_model_dir + '/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf' if len(argv)<2 else argv[1]
 
# Load the urdf model
model = pinocchio.buildModelFromUrdf(urdf_filename)
print('model name: ' + model.name)
 
# Create data required by the algorithms
data = model.createData()
 
# Sample a random configuration
q = pinocchio.randomConfiguration(model)
print('q: %s' % q.T)
 
# Perform the forward kinematics over the kinematic tree
pinocchio.forwardKinematics(model,data,q)
 
# Print out the placement of each joint of the kinematic tree
for name, oMi in zip(model.names, data.oMi):
    print(("{:<24} : {: .2f} {: .2f} {: .2f}"
          .format( name, *oMi.translation.T.flat)))

——————————————————分割线 2023.12.23————————————————

2. Pinocchio与Pybullet联调：pinocchio_bullet_wrapper.py

这个python文件是纽约大学团队开发的，为需要pybullet物理引擎仿真和pinocchio库计算设计的，使用这个接口一是为了用pinocchio，另一个原因是bullet可以集成到OpenAI Gym框架中，方便开发和比较强化学习算法。

（仿真在 PyBullet中进行，动力学库 Pinocchio 用于更新机器人的动态项，如惯性矩阵和雅可比矩阵）

包中关键函数是根据bullet中状态更新pinocchio参数：

    def get_state_update_pinocchio(self):
        """Get state from pybullet and update pinocchio robot internals.
        This gets the state from the pybullet simulator and forwards
        the kinematics, jacobians, centroidal moments on the pinocchio robot
        (see forward_pinocchio for details on computed quantities). """
        q, dq, ef = self.get_state()
        self.update_pinocchio(q, dq)
        return q, dq, ef

cons_cost_func：增广奖励函数，在有约束的MPPI中使用

cost_func：奖励函数，在env中调用，用于step函数中获取当前状态的reward，并且存储在mem中，用于NAF网络训练。