# 加载模型
## Python
\include overview-urdf.py
## C++
\include overview-urdf.cpp
# 几何模型
## Python
\include geometry-models.py
## C++
\include geometry-models.cpp
# 示例
# 使用GepettoViewer显示模型
\include gepetto-viewer.py
# 碰撞检测与距离
## Python
\include collisions.py
## C++
\include collisions.cpp
# 逆运动学（clik）
此示例展示了如何将机械手机器人的末端执行器定位到给定的位姿（位置和方向）。
该示例采用了一种简单的基于雅可比矩阵的迭代算法，称为闭环逆运动学（CLIK）。
## Python
\includelineno inverse-kinematics.py
### 代码解释
首先，我们导入必要的库并创建`Model`和`Data`对象：
\until data
末端执行器对应于第6个关节
\skipline JOINT
其期望位姿如下：
\skipline oMdes
接下来，我们定义一个初始配置
\skipline q
这是算法的起点。先验地说，任何有效的配置都可以。
我们决定使用由`neutral`算法返回的机器人中性配置。
对于像本例中这样的简单机械手，它简单地对应于一个全零向量，
但使用这种方法可以很好地推广到更复杂类型的机器人，确保有效性。
接下来，我们设置一些与计算相关的值
\until damp
对应于期望的位置精度（我们稍后会看到它的确切含义）、
最大迭代次数（以避免在位置不可达的情况下无限循环）、
一个定义收敛速率的正“时间步长”，
以及用于伪逆的固定阻尼因子（见下文）。
然后，我们开始迭代过程。
在每次迭代中，我们首先计算正向运动学：
\skipline forwardKinematics
接下来，我们计算期望位姿`d`与当前位姿`i`之间的误差，该误差以当前关节坐标系`i`表示：
\skip iMd
\until err
这里，`data.oMi[JOINT_ID]`对应于第六个关节的位姿（之前由`forwardKinematics`计算得出），
`dMi`对应于期望位姿与当前位姿之间的变换，而`err`是误差。
为了计算误差，我们使用了`log`：这是一个`Motion`对象，它是在\(SO(3)\)中计算误差的一种方式，将其表示为一个六维向量。
如果误差的范数低于之前定义的阈值，我们就找到了解决方案并跳出循环
\until break
请注意，严格来说，空间速度的范数在物理上没有意义，因为它混合了线性和角量。
更严格的实现应该分别处理线性部分和角部分。
然而，在这个例子中，为了保持简单，我们选择稍微滥用一下符号。
如果我们已经达到了最大迭代次数，这意味着没有找到解决方案。我们记录下这个失败情况，然后也跳出循环
\until break
否则，我们尝试寻找另一个配置以减小误差。
我们首先在关节坐标系中计算雅可比矩阵：
\skipline J
为了得到我们任务的正确雅可比矩阵，我们应该调用\ref Jlog6函数（例如，参见[此笔记](https://scaron.info/robotics/jacobian-of-a-kinematic-task-and-derivatives-on-manifolds.html)了解推导细节）：
\skipline J
接下来，我们可以通过求解逆运动学来计算配置的演变。
为了避免在奇点处出现问题，我们使用阻尼伪逆：
\(v = - J^T (J J^T + \lambda I)^{-1} e\)
将该方程实现为：
\skipline v
请注意，选择这种计算阻尼伪逆的方法主要是因为其实现简单。
它不一定是最好或最快的方法，
并且使用固定的阻尼因子\(\lambda\)不一定是最佳做法。
最后，我们可以将得到的切向量添加到当前配置中
\skipline q
在我们的例子中，`integrate`相当于简单的求和。得到的误差将在下一次迭代中进行验证。
在循环结束时，我们显示结果：
\skip success
\until final
## C++
等效的C++实现如下
\includelineno inverse-kinematics.cpp
### 代码解释
代码遵循与Python完全相同的步骤。
除了Python和C++之间通常的语法差异外，我们可以识别出两个主要区别。
第一个区别涉及雅可比矩阵的计算。在C++中，你需要预先分配其内存空间，将其设置为零，并将其作为输入传递
\skip J(6,model.nv)
\until setZero
\skip computeJointJacobian
\line computeJointJacobian
这允许始终使用相同的内存空间，避免重新分配，从而实现更高的效率。
第二个区别在于速度的计算方式
\skip JJt
\until solve
这段代码比Python版本长，但与之等效。注意，我们显式地使用了`ldlt`分解。
# 使用RobotWrapper显示模型
`RobotWrapper`是一个用Python编写的包装类，用于持有Pinocchio模型和数据。
它还包含一些常用方法的包装函数。
`RobotWrapper`可以与不同的可视化工具交互。
下面是一个示例，展示了如何使用`RobotWrapper`加载模型以及如何将其与你选择的可视化工具一起使用。
\include robot-wrapper-viewer.py
# 加载URDF后更新模型
此示例展示了如何在从URDF描述中加载机器人模型后对其进行更新。
## Python
\include update-model-after-urdf.py
# 构建简化模型
此示例展示了如何通过将所需的关节固定在指定位置，从现有的URDF模型构建简化的机器人模型。
## Python
\include build-reduced-model.py
## C++
\include build-reduced-model.cpp
# 使用Meshcat显示模型
\include meshcat-viewer.py
# 加载并显示模型
Pinocchio没有内置的可视化工具，你必须依赖外部库。
然而，Pinocchio为一些外部工具提供了Python支持，以便于显示给定的模型。
下面你将看到不同选项的列表。
# 加载模型
## Python
\include overview-urdf.py
## C++
\include overview-urdf.cpp
# 几何模型
## Python
\include geometry-models.py
## C++
\include geometry-models.cpp
# 加载并显示模型
Pinocchio没有内置的可视化工具，你必须依赖外部库。
然而，Pinocchio为一些外部工具提供了Python支持，以便于显示给定的模型。
下面你将看到不同选项的列表。
# 碰撞检测与距离
## Python
\include collisions.py
## C++
\include collisions.cpp
# 逆运动学（clik）
此示例展示了如何将机械手机器人的末端执行器定位到给定的位姿（位置和方向）。
该示例采用了一种简单的基于雅可比矩阵的迭代算法，称为闭环逆运动学（CLIK）。
## Python
\includelineno inverse-kinematics.py
### 代码解释
首先，我们导入必要的库并创建`Model`和`Data`对象：
\until data
末端执行器对应于第6个关节
\skipline JOINT
其期望位姿如下：
\skipline oMdes
接下来，我们定义一个初始配置
\skipline q
这是算法的起点。先验地说，任何有效的配置都可以。
我们决定使用由`neutral`算法返回的机器人中性配置。
对于像本例中这样的简单机械手，它简单地对应于一个全零向量，
但使用这种方法可以很好地推广到更复杂类型的机器人，确保有效性。
接下来，我们设置一些与计算相关的值
\until damp
对应于期望的位置精度（我们稍后会看到它的确切含义）、
最大迭代次数（以避免在位置不可达的情况下无限循环）、
一个定义收敛速率的正“时间步长”，
以及用于伪逆的固定阻尼因子（见下文）。
然后，我们开始迭代过程。
在每次迭代中，我们首先计算正向运动学：
\skipline forwardKinematics
接下来，我们计算期望位姿`d`与当前位姿`i`之间的误差，该误差以当前关节坐标系`i`表示：
\skip iMd
\until err
这里，`data.oMi[JOINT_ID]`对应于第六个关节的位姿（之前由`forwardKinematics`计算得出），
`dMi`对应于期望位姿与当前位姿之间的变换，而`err`是误差。
为了计算误差，我们使用了`log`：这是一个`Motion`对象，它是在\(SO(3)\)中计算误差的一种方式，将其表示为一个六维向量。
如果误差的范数低于之前定义的阈值，我们就找到了解决方案并跳出循环
\until break
请注意，严格来说，空间速度的范数在物理上没有意义，因为它混合了线性和角量。
更严格的实现应该分别处理线性部分和角部分。
然而，在这个例子中，为了保持简单，我们选择稍微滥用一下符号。
如果我们已经达到了最大迭代次数，这意味着没有找到解决方案。我们记录下这个失败情况，然后也跳出循环
\until break
否则，我们尝试寻找另一个配置以减小误差。
我们首先在关节坐标系中计算雅可比矩阵：
\skipline J
为了得到我们任务的正确雅可比矩阵，我们应该调用\ref Jlog6函数（例如，参见[此笔记](https://scaron.info/robotics/jacobian-of-a-kinematic-task-and-derivatives-on-manifolds.html)了解推导细节）：
\skipline J
接下来，我们可以通过求解逆运动学来计算配置的演变。
为了避免在奇点处出现问题，我们使用阻尼伪逆：
\(v = - J^T (J J^T + \lambda I)^{-1} e\)
将该方程实现为：
\skipline v
请注意，选择这种计算阻尼伪逆的方法主要是因为其实现简单。
它不一定是最好或最快的方法，
并且使用固定的阻尼因子\(\lambda\)不一定是最佳做法。
最后，我们可以将得到的切向量添加到当前配置中
\skipline q
在我们的例子中，`integrate`相当于简单的求和。得到的误差将在下一次迭代中进行验证。
在循环结束时，我们显示结果：
\skip success
\until final
## C++
等效的C++实现如下
\includelineno inverse-kinematics.cpp
### 代码解释
代码遵循与Python完全相同的步骤。
除了Python和C++之间通常的语法差异外，我们可以识别出两个主要区别。
第一个区别涉及雅可比矩阵的计算。在C++中，你需要预先分配其内存空间，将其设置为零，并将其作为输入传递
\skip J(6,model.nv)
\until setZero
\skip computeJointJacobian
\line computeJointJacobian
这允许始终使用相同的内存空间，避免重新分配，从而实现更高的效率。
第二个区别在于速度的计算方式
\skip JJt
\until solve
这段代码比Python版本长，但与之等效。注意，我们显式地使用了`ldlt`分解。
# 加载URDF后更新模型
此示例展示了如何在从URDF描述中加载机器人模型后对其进行更新。
## Python
\include update-model-after-urdf.py
# 构建简化模型
此示例展示了如何通过将所需的关节固定在指定位置，从现有的URDF模型构建简化的机器人模型。
## Python
\include build-reduced-model.py
## C++
\include build-reduced-model.cpp
# 示例
# 使用GepettoViewer显示模型
\include gepetto-viewer.py
# 使用Meshcat显示模型
\include meshcat-viewer.py
# 使用RobotWrapper显示模型
`RobotWrapper`是一个用Python编写的包装类，用于持有Pinocchio模型和数据。
它还包含一些常用方法的包装函数。
`RobotWrapper`可以与不同的可视化工具交互。
下面是一个示例，展示了如何使用`RobotWrapper`加载模型以及如何将其与你选择的可视化工具一起使用。
\include robot-wrapper-viewer.py
扫一扫
7823
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
