ODE 引擎🎮入门
原文 Open Dynamics Engine v0.5 User Guide
刚体
从仿真的角度来看，一个刚体有各种属性。有些属性会随时间变化：
- 刚体的参考点的位置向量（x,y,z）。参考点必须对应于刚体的质量中心。
- 参考点的线性速度，一个矢量（vx,vy,vz）。
- 刚体的方向，用一个四元数（qs,qx,qy,qz）或旋转矩阵表示。
- 角速度矢量（wx,wy,wz），描述方向如何随时间变化。
其他刚体属性通常是随时间变化的：
- 刚体的质量。
- 质量中心相对于参考点的位置。在目前的实现中，质心和参考点必须重合。
- 惯性矩阵。这是一个3x3矩阵，描述刚体的质量如何围绕质量中心分布。
从概念上讲，每个体都有一个嵌入其中的 x-y-z 坐标框架，它随着体的移动和旋转，如图所示。
这个坐标框架的原点是刚体的参考点。ODE中的一些数值（向量、矩阵等）是相对于刚体坐标框架而言的，而其他数值则是相对于全局坐标框架而言的。
请注意，刚体的形状不是一个动态属性。只有碰撞检测才关心体的详细形状。
Island 和禁用刚体
刚体是通过关节相互连接的。一个 Island 是一个不能被拉开的刚体群体。换句话说，每个刚体都以某种方式与 Island 里的其他刚体相连。
仿真时，世界上的每个 Island 都被单独处理。这一点很有用，如果仿真中有 N 个类似的 Island，那么单步计算时间将是 O(N)。
每个刚体都可以被启用或禁用。被禁用的刚体实际上是关闭的，仿真时不被更新。当已知物体不运动或与仿真无关时，禁用刚体是节省计算时间的一种有效方法。
如果一个 Island 有任何启用的刚体，那么 Island 的每个刚体在下一个仿真步骤中都会被启用。因此，要想有效地禁用一个 Island，需要把 Island 的每个刚体都禁用。如果一个被禁用的 Island 被另一个被启用的刚体所接触，那么整个 Island 将被启用，因为一个接触关节将把被启用的刚体与 Island 连接起来。
积分器
通过时间对刚体系统进行仿真的过程称为积分。每个积分步骤都将当前时间向前推进一个给定的步长，为新的时间值调整所有刚体的状态。在使用任何积分器时，有两个主要问题需要考虑：
- 准确性如何？也就是说，仿真系统的行为与现实生活中发生的情况有多一致？
- 稳定性如何？也就是说，计算错误是否会导致仿真系统的完全非物理性行为？
ODE 目前的积分器是非常稳定的，但是除非步长很小，否则不是特别准确。对于ODE的大多数用途来说，这不是一个问题。ODE 的行为在几乎所有的情况下看起来都是完美的物理现象。然而，在这个精度问题在未来的版本中被解决之前，ODE 不应该被用于定量工程。😄
力累加器
在每个积分器步骤之间，用户可以调用函数来对刚体施加力。这些力被添加到刚体对象的力累加器中。当下一个积分器步骤发生时，所有应用的力的总和将被用来推动刚体。在每个积分器步骤之后，力累加器被设置为零。
关节与约束
在现实生活中，关节是类似铰链的东西，用来连接两个物体。在 ODE 中，关节是非常相似的：它是一种在两个刚体之间强制执行的关系，使它们只能有相对特定的位置和方向。这种关系被称为约束，关节和约束这两个词经常被互换使用。图中显示了三种不同的约束类型。
积分器每执行一个步长时，所有的关节都被允许对它们所影响的刚体施加约束力。这些力的计算是为了使物体的移动能够保持所有的关节关系。
每个关节都有一些控制其几何的参数。一个例子是球窝接头的球窝点的位置。设置关节参数的函数都采用全局坐标，而不是相对于刚体的坐标。这样做的一个结果是，在连接关节之前，关节所连接的刚体必须被正确定位。
关节组
关节组是一个特殊的容器，用来容纳世界中的关节。关节可以被添加到一个组中，然后当这些关节不再需要时，整个组的关节可以通过一个函数调用被快速销毁。然而，在整个组被清空之前，组内的单个关节是不能被销毁的。
这对于接触关节来说是最有用的，因为接触点在每个时间步长中都会被分组添加和删除。
关节误差与 ERP ｜ 硬约束
当一个关节连接两个刚体时，这些刚体被要求有特定的相对位置和方向。然而，刚体有可能处于不符合关节约束的位置。这种关节错误可能以两种方式发生：
- 如果用户设置了一个体的位置/方向而没有正确设置另一个体的位置/方向。
- 在仿真过程中，错误会悄悄出现，导致刚体偏离所需位置。
下图显示了一个球窝接头的误差例子（球窝没有对准）。
有一种机制可以减少关节的误差：在每个仿真步骤中，每个关节都会施加一个特殊的力，使其刚体回到正确的对位。这个力是由减少误差参数（error reduction parameter，ERP）控制的，它的值在 [0,1] 之间。
ERP 规定了在下一个仿真步骤中，关节误差的比例将被固定。如果 ERP=0 就不应用纠正力，随着仿真的进行，刚体最终会漂移开来。如果 ERP=1 仿真将试图在下一个时间步骤中修复所有关节误差。然而，不建议设置 ERP=1 因为由于各种内部近似，关节误差将不会被完全固定。推荐使用 ERP=[0.1, 0.8]（默认为 0.2）。
可以设置一个全局的 ERP 值，影响到仿真中的大多数关节。然而有些关节有局部的 ERP 值，控制关节的各个方面。
软约束与 CFM
大多数约束在本质上是硬的。这意味着约束条件代表了永远不会被违反的条件。在实践中，约束条件可能被无意中引入系统的错误所违反，但可以设置 ERP 来纠正这些错误。
不是所有的约束都是硬约束。一些软约束被设计成可以违反的。例如，防止碰撞物体穿透的接触约束在默认情况下是硬的，所以它的作用就像碰撞的表面是由钢铁制成的。但它可以被做成软约束，以仿真较软的材料，从而允许两个物体被迫在一起时有一些自然穿透。
有两个参数可以控制硬约束和软约束之间的区别。第一个是已经介绍过的 ERP。第二个是约束力混合（constraint force mixing，CFM）值。
Constraint Force Mixing (CFM)
下面是对 CFM 含义的一些技术描述。如果你只想知道它在实践中是如何使用的，那么请跳到下一节。传统上，每个关节的约束方程的形式为
J * v = c
其中 v 是刚体的速度矢量，J 是一个雅可比矩阵，每一个关节从系统中移除一个自由度就有一行，c 是一个右手定则矢量。在下一个时间步长中，计算出一个矢量 lambda，这样施加在刚体上的力就可以保持关节的约束
force = J^T * lambda
ODE 增加了一个新的 twist。ODE 的约束方程有如下形式
J v = c + CFM lambda
其中 CFM 是一个对角方阵。CFM 将产生的约束力与产生它的约束混合在一起。CFM 的非零正值允许原始约束方程被违反，违反量与 CFM 乘以执行约束所需的恢复力 lambda 成正比。求解 lambda 可以得到
(J M^-1 J^T + CFM/h) * lambda = c/h
因此，CFM 只是增加了原始系统矩阵的对角线。使用 CFM 的正值有一个额外的好处，就是使系统远离任何奇异点，从而提高分解器的精度。
LM?🤔️
如何使用 ERP和CFM
ERP和CFM 可以在许多关节处独立设置。它们可以设置在接触关节、关节限位和其他各种地方，以控制关节（或关节限位）的海绵度和弹力度。
如果 CFM 被设置为零，约束将是硬的。如果 CFM 被设置为正值，就有可能通过推压来违反约束（例如，对于接触约束，通过强迫两个接触物体在一起）。换句话说，该约束将是软的，而且软度将随着 CFM 的增加而增加。这里实际发生的情况是，约束被允许被违反的程度与 CFM 乘以执行约束所需的恢复力成正比。注意，将 CFM 设置为负值会产生不良的影响，如不稳定。不要这样做。
通过调整 ERP和CFM 的值，你可以实现各种效果。例如，你可以仿真有弹性的约束，两个体的摆动就像被弹簧连接一样。或者你可以仿真更多的海绵状约束，没有振荡。事实上，ERP和CFM 可以被选择为与任何所需的弹簧和阻尼器常数具有相同的效果。如果你有一个弹簧常数 kp 和阻尼常数 kd，那么相应的 ODE 常数
ERP = h kp / (h kp + kd)
CFM = 1 / (h * kp + kd)
其中 h 是步长。这些值将给出与用隐式一阶积分仿真的弹簧-阻尼器系统效果一样。
增加 CFM，特别是全局 CFM，可以减少仿真的数值误差。如果系统是接近奇异点，那么这可以明显地提高稳定性。事实上，如果系统表现不佳，首先要做的就是增加全局 CFM。
碰撞处理
刚体之间或刚体与静态环境之间的碰撞处理如下。
- 在每个仿真步骤之前，用户调用碰撞检测函数来确定什么东西在接触什么。这些函数返回一个接触点的列表。每个接触点指定一个空间位置、一个表面法向量和一个穿透深度。
- 为每个接触关节创建一个特殊的接触点。接触点被赋予关于接触的额外信息，例如接触面存在的摩擦力，它的弹力或柔软程度，以及其他各种属性。
- 接触点被放在一个接触关节组中，这使得它们可以被快速添加到系统中或从系统中移除。仿真速度会随着接触点数量的增加而下降，因此可以使用各种策略来限制接触点的数量。
- 执行一个仿真步长。
- 所有的接触点都从系统中移除。
请注意，不一定要使用内置的碰撞函数，只要提供正确种类的接触点信息，就可以使用其他碰撞检测库，如 FCL。
仿真流程
一个典型的仿真器将像这样执行：
- 创建一个动力学世界
- 在动力学世界中创建刚体
- 设置所有刚体的状态
- 在动力学世界中创建关节
- 将关节连接到刚体上
- 设置所有关节的参数
- 创建一个碰撞世界和碰撞几何对象
- 创建一个关节组来容纳接触的关节
- 循环大逻辑
- 必要时对刚体施加力
- 必要时调整关节参数
- 调用碰撞检测
- 为每个碰撞关节创建一个接触点，并把它放在接触关节组中
- 执行一个仿真步长
- 移除接触关节组中的所有关节
物理模型
这里讨论了 ODE 中使用的各种方法和近似。
摩擦力近似
库仑摩擦模型是一种简单而有效的方法来仿真接触点的摩擦。它是存在于接触点的法向力和切向力之间的一种简单关系。其规则是
abs(f_T) <= mu * abs(f_N)
其中 f_N 和 f_T 分别是法向力和切向力，mu 是摩擦系数（通常是 1.0 左右的数字）。这个方程定义了一个摩擦锥。想象一个以 f_N 为轴，接触点为顶点的锥体。如果总的摩擦力矢量在锥体内，那么接触就处于粘着模式，摩擦力足以阻止接触面相对于对方的移动。如果力矢量在圆锥体的表面上，那么接触就处于滑动模式，摩擦力通常不足以阻止接触面滑动。因此，参数 mu 规定了切向力与法向力的最大比率。
ODE 的摩擦模型是摩擦锥的近似值，是出于效率的考虑。目前有两个近似值可以选择。
- mu 的含义被改变了，所以它指定了在一个接触点上可能存在的最大摩擦（切向）力，在任何一个切向摩擦方向。这是相当非物理的，因为它与法向力无关，但它可能是有用的，而且是计算上最便宜的选择。请注意，在这种情况下，mu 是一个力的极限，必须选择适合于仿真的。
- 摩擦锥由一个与第一和第二摩擦方向对齐的摩擦金字塔来近似。一个进一步的近似是：首先 ODE 计算法向力，假设所有的接触都是无摩擦的。然后，它计算出摩擦（切向）力的最大极限 f_m，来自于
f_m = mu * abs(f_N)
然后用这些固定的极限值对整个系统进行求解（方式与上述近似值1 相似）。这与真正的摩擦金字塔不同，因为有效的 mu 并不完全固定。这种近似方法更容易使用，因为 mu 是一个无单位的比率，与正常的 Coloumb 摩擦系数相同，因此可以设置为 1.0 左右的恒定值而不考虑具体的仿真。
数据类型与约定
基本数据类型
- single 浮点类型
- double 浮点类型
数据类型
- dReal
- dVector3
- dVector4
- dMatrix3
- dMatrix4
- dQuaternion
对象和 ID
用户能创建的对象
| 对象 | 功能 |
|---|---|
| dWorld | 一个动态世界 |
| dSpace | 一个碰撞空间 |
| dBody | 一个刚体 |
| dGeom | 几何体(for 碰撞检测) |
| dJoint | 一个关节 |
| dJointGroup | 一组关节 |
处理这些对象的函数接受并返回对象的 ID。对象的 ID 类型有 dWorldID、dBodyID 等。
参数约定
set函数的所有 3 维向量 (x,y,z) 是作为单独的 x,y,z 参数给出的。get函数的所有 3 维向量返回结果是指向dReal数组的指针。- 较大的向量总是作为指向 dReal 数组的指针提供和返回。
- 除非另有规定，所有坐标都是在全局坐标系下。
C vs C++
ODE 库是用 C++ 编写的，但它的公共接口是由简单的 C 函数组成的，而不是类。这是为什么呢？
- 只使用 C 接口更简单，C++ 的特性对 ODE 没有什么帮助。
- 它可以防止 C++ 的纠缠和跨多个编译器的运行时间支持问题。
- 用户不需要熟悉 C++ 的怪癖来使用 ODE。
调试
ODE 库可以用 Debug 或 Release 模式编译。调试 Debug 模式比较慢，但是会对函数参数进行检查，并进行许多运行时测试以确保内部一致性。发布 Release 模式更快，但不做任何检查。
Note 以下内容源自 http://demura.net/english
ODE 入门第 1 讲——ODE 是什么
ODE（Open Dynamics Engine）是一个开源的物理引擎，从 2001 年开始由 Russell Smith 开发。特点是
- 自由
- 易用
- 快速
- 跨平台
- 支持 C/C++
ODE 入门第 2 讲——安装与编译
mkdir build
cmake
make
ODE 入门第 3 讲——Hello Physics World
ODE 仿真流程 Simulation flow
- Drawing setting function Drawstuff
- Setup a cameras
dsSetViewPoint()
- Setup a cameras
- Initialize ODE
dInitODE() - Create a world for dynamics
dWorldCreate() - Set gravity
dWorldSetGravity() - Create a rigid body
- Set mass
dBodySetMass() - Set position
dBodySetPosition() - Set orientation
dBodySetRotation()
- Set mass
- Simulation Loop
- Step a simulation world
dWorldStep() - Write simulation codes
- Step a simulation world
- Destroy the world
dWorldDestroy() - Close ODE
dCloseODE()
例子
// Sample3 by Kosei Demura
#include "ode/ode.h"
#include "drawstuff/drawstuff.h"
#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#endif
static dWorldID world; // a physics world
dBodyID ball; // an ball
const dReal radius = 0.2, mass = 1.0; // radius(m)、weight(kg) of an ball
// Simulation loop
static void simLoop (int pause)
{
const dReal *pos,*R; // position, rotation matrix
dWorldStep(world,0.05); // Step a simulation world, time step is 0.05 [s]
dsSetColor(1.0,0.0,0.0); // Set color (red, green, blue) value is from 0 to 1.0
pos = dBodyGetPosition(ball); // Get a position
R = dBodyGetRotation(ball); // Get a rotation
dsDrawSphere(pos,R,radius); // Draw a sphere
}
// Start function void start()
{
// Set a camera
static float xyz[3] = {0.0,-3.0,1.0}; // View position (x, y, z [m])
static float hpr[3] = {90.0,0.0,0.0}; // View direction （head, pitch, roll[°]）
dsSetViewpoint (xyz,hpr); // Set a view point
}
// main function
int main (int argc, char **argv)
{
dReal x0 = 0.0, y0 = 0.0, dReal z0 = 1.0; // initial position of an ball[m]
dMass m1; // mass parameter
// for drawstuff
dsFunctions fn; // drawstuff structure
fn.version = DS_VERSION; // the version of the drawstuff
fn.start = &start; // start function
fn.step = &simLoop; // step function
fn.command = NULL; // no command function for keyboard
fn.stop = NULL; // no stop function
fn.path_to_textures = "../../drawstuff/textures"; // path to the texture
dInitODE(); // Initialize ODE
world = dWorldCreate(); // Create a dynamic world
dWorldSetGravity(world,0,0,-0.001); // Set gravity（x, y, z)
// Create a ball
ball = dBodyCreate(world); // Crate a rigid body
dMassSetZero(&m1); // Initialize mass parameters
dMassSetSphereTotal(&m1,mass,radius); // Calculate a mass parameter
dBodySetMass(ball,&m1); // Set a mass parameter
dBodySetPosition(ball, x0, y0, z0); // Set a position(x, y, z)
// Simulation loop
// argc, argv are argument of main function. Window size is 352 x 288 [pixel]
// fn is a structure of drawstuff
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy(world); // Destroy the world
dCloseODE(); // Close ODE
return 0;
}
这是一个自由落体程序的红球。ODE仿真的流程如下，首先，dInitODE()初始化ODE。接下来，通过调用dWorldCreate()创建动态世界（World）。重力由dWorldSetGravity(world,0,0,-0.001)设置。在这种情况下，重力矢量被设置为(0, 0, -0.001)，使得物体下落非常缓慢。调用dBodyCreate来创建一个刚体，然后初始化一个质量参数，计算并设置它。通过dBodySetPosition()设置体的位置。
仿真是由simLoop函数执行的。这个函数在每个仿真步骤中都会被调用。dWorldStep(world, 0.05)步骤是一个单步。第二个参数是一个时间步长。在本例中，0.05秒是步长。dsDrawSphere()在你的电脑屏幕上显示一个球体。dWorldDestroy(world)销毁了世界，dCloseODE()关闭了ODE，在代码的最后，清理了仿真。
这个函数以小写的’d’开头，是ODE的API，并且，’ds’是Drawstuff的API。Drawstuff是ODE的演示程序的图形库（一个基于OpenGL的3D图形库）。
ODE 入门第 4 讲——3D 图形学
Drawstuff 是一个非常简单的3D图形库。它不适合商业游戏，但对于你自己的作品的仿真器来说，它可能是足够的。Drawstuff 的替代品是 Ogre3D 和 Irrlicht。
Drawstuff 是在 OpenGL 3D 图形库上实现的。Drawstuff 是非常简单的库，所以它可能对学习OpenGL有用。Drawstuff 的API以小字母 ds 开头，而 ODE 的 API 以小字母 d 开头。
#include "drawstuff/drawstuff.h"
#include "ode/ode.h"
#ifdef dDOUBLE
#define dsDrawSphere dsDrawSphereD
#endif
static dWorldID world;
// a physics world
dBodyID ball;
// an ball
const dReal radius = 0.2, mass = 1.0;
// radius(m)、weight(kg) of an ball
// Simulation loop
static void simLoop(int pause) {
const dReal *pos, *R;
// position, rotation matrix
dWorldStep(world, 0.05);
// Step a simulation world, time step is 0.05 [s]
dsSetColor(1.0, 0.0,
0.0); // Set color (red, green, blue) value is from 0 to 1.0
pos = dBodyGetPosition(ball);
// Get a position
R = dBodyGetRotation(ball);
// Get a rotation
dsDrawSphere(pos, R, radius);
// Draw a sphere
}
void start() {
// Set a camera
static float xyz[3] = {0.0, -3.0, 1.0};
// View position (x, y, z [m])
static float hpr[3] = {90.0, 0.0, 0.0};
// View direction （head, pitch, roll[°]）
dsSetViewpoint(xyz, hpr);
// Set a view point
}
// main function
int main(int argc, char **argv) {
dReal x0 = 0.0, y0 = 0.0;
dReal z0 = 1.0; // initial position of an ball[m]
dMass m1;
// mass parameter
// for drawstuff
dsFunctions fn;
// drawstuff structure
fn.version = DS_VERSION;
// the version of the drawstuff
fn.start = &start;
// start function
fn.step = &simLoop;
// step function
fn.command = NULL;
// no command function for keyboard
fn.stop = NULL;
// no stop function
fn.path_to_textures =
"/Users/zhangjixiang/Code/ode-0.16.2/drawstuff/textures";
// path to the texture
dInitODE(); // Initialize ODE
world = dWorldCreate();
// Create a dynamic world
dWorldSetGravity(world, 0, 0, -0.001);
// Set gravity（x, y, z)
// Create a ball
ball = dBodyCreate(world);
// Crate a rigid body
dMassSetZero(&m1);
// Initialize mass parameters
dMassSetSphereTotal(&m1, mass, radius);
// Calculate a mass parameter
dBodySetMass(ball, &m1);
// Set a mass parameter
dBodySetPosition(ball, x0, y0, z0);
// Set a position(x, y, z)
// Simulation loop
// argc, argv are argument of main function. Window size is 352 x 288
// [pixel] fn is a structure of drawstuff
dsSimulationLoop(argc, argv, 352, 288, &fn);
dWorldDestroy(world);
// Destroy the world
dCloseODE(); // Close ODE
return 0;
}
ODE 入门第 5 讲——刚体与几何体
我们来研究一个作为动力学目标的刚体和一个作为碰撞检测目标的几何体。
ODE 分别实现了动力学核心和碰撞检测库。它的优点是很容易使用其他碰撞库而不是 ODE。目前，ODE 使用 OPCODE 和 Gimpact。OPCODE 是默认的碰撞检测库，它比 Gimpact 快。然而，Gimpact 比 OPCODE 更准确。OpenHRP3 也使用了 OPCODE 进行碰撞检测。
该实现的另一个优点是，只有碰撞检测可以应用于物体。例如，筑波挑战赛，真实世界机器人挑战赛，你不必计算关于建筑物、树木和栅栏的动力学。只计算动力学就足以仿真环境了。
由于这种优势，ODE 为动力学计算创建了一个世界 World，并为碰撞检测创建了一个空间 Space。一个物体有两个属性，即用于动力学的刚体和用于碰撞检测的几何体。
ODE 入门第 6 讲——碰撞检测
在前面的教程中，你知道，动力学和碰撞检测是在 ODE 中单独实现的。为了计算动力学，首先通过 dWorldCreate() 创建一个世界，其次在这个世界中创建一个刚体，最后通过 dWorldStep() 计算动力学。
一个几何体是一个物体的形状。它被用于碰撞检测。同时，为了计算碰撞检测，首先通过 dHashSpaceCreate() 创建一个空间，其次在该空间中创建一个几何体，最后通过 dSpaceCollide() 检测碰撞。
在下面的示例程序中，在第97行，通过 dCreateSphere() 创建了一个球形几何体。在第98行，通过 dGeomSetBody() 将球的几何体 ball.geom 与刚体 ball.body 联系起来。一个 ODE 对象是由刚体和几何体组成的。这两个属性都必须被关联。
然后，dSpaceCollide() 在 simLoop 函数中被调用。nearCallback() 的参数 o1 和 o2 是有可能相互碰撞的几何体。你可以在 nearCallback() 函数中设置接触面的属性，如接触点的上限、摩擦系数、反弹系数、摩擦模型、柔软度参数等等。
此外，你必须注意参数 o1 和 o2 可能会发生碰撞，有时这些参数并不相互碰撞。为了了解碰撞情况，可以检查 dCollide() 的返回值。如果有碰撞，返回值会大于 1。
如第88行，首先通过 dJointGroupCreate() 创建一个接触组，这是一个碰撞点的容器，然后像第53行那样，每仿真一步都通过 dJointGroupEmpty() 清除这个容器。
// sample6.cpp
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
static dWorldID world;
static dSpaceID space;
static dGeomID ground;
static dJointGroupID contactgroup;
static int flag = 0;
dsFunctions fn;
const dReal radius = 0.2;
const dReal mass = 1.0;
typedef struct {
dBodyID body;
dGeomID geom;
} MyObject;
MyObject ball;
static void nearCallback(void *data, dGeomID o1, dGeomID o2)
{
const int N = 10;
dContact contact[N];
int isGround = ((ground == o1) || (ground == o2));
int n = dCollide(o1,o2,N,&contact[0].geom,sizeof(dContact));
if (isGround) {
if (n >= 1) flag = 1;
else flag = 0;
for (int i = 0; i < n; i++) {
contact[i].surface.mode = dContactBounce;
contact[i].surface.mu = dInfinity;
contact[i].surface.bounce = 0.0; // (0.0~1.0) restitution parameter
contact[i].surface.bounce_vel = 0.0; // minimum incoming velocity for bounce
dJointID c = dJointCreateContact(world,contactgroup,&contact[i]);
dJointAttach (c,dGeomGetBody(contact[i].geom.g1),dGeomGetBody(contact[i].geom.g2));
}
}
}
static void simLoop (int pause)
{
const dReal *pos,*R;
flag = 0;
dSpaceCollide(space,0,&nearCallback);
dWorldStep(world,0.01);
dJointGroupEmpty(contactgroup);
if (flag == 0) dsSetColor(1.0, 0.0, 0.0);
else dsSetColor(0.0, 0.0, 1.0);
pos = dBodyGetPosition(ball.body);
R = dBodyGetRotation(ball.body);
dsDrawSphere(pos,R,radius);
}
void start()
{
static float xyz[3] = {0.0,-3.0,1.0};
static float hpr[3] = {90.0,0.0,0.0};
dsSetViewpoint (xyz,hpr);
}
void prepDrawStuff() {
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../../drawstuff/textures";
}
int main (int argc, char *argv[])
{
dReal x0 = 0.0, y0 = 0.0, z0 = 2.0;
dMass m1;
prepDrawStuff();
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dWorldSetGravity(world,0,0,-0.5);
// Create a ground
ground = dCreatePlane(space,0,0,1,0);
// Create a ball
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1,mass,radius);
dBodySetMass(ball.body,&m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space,radius);
dGeomSetBody(ball.geom,ball.body);
dsSimulationLoop (argc,argv,352,288,&fn);
dWorldDestroy (world);
dCloseODE();
return 0;
}
在代码中，一个刚体和一个几何体是 MyObject 结构的成员。
碰撞检测函数 dSpaceCollide() 在 simLoop 函数中被调用。你必须在 dWorldStep() 之前调用它。dSpaceCollide() 找到接触点，它们是动力学计算的约束条件。
dSpaceCollide() 调用回调函数 nearCallback()。dSpaceCollide() 将两个可能发生碰撞的几何体 o1 和 o2 传递给 nearCallback()。dCollide() 的返回值是第28行的接触点的数量。
ODE 入门第 7 讲——关节即约束
我们周围有很多关节。例如，手机的铰链，门的铰链等等。一个关节连接两个刚体。换句话说，关节是一个限制两个刚体运动的约束条件。ODE 使用的关节与约束条件的含义相同。
关节的用法
- Create a joint
- dJointCreate***()
- connect bodies with the joint
- dJointAttach()
- Set an anchor of the joint
- dJointSet***Anchor()
- Set an axis of the joint
- dJointSet***Axis()
关节的配置
- Set range of motion
- dJointSetHingeParam (dJointID, dParamLoStop, the minimum range of motion);
- dJointSetHingeParam (dJointID, dParamHiStop, the maximum range of motion);
- Set angular velocity and maximum torque
- dJointSetHingeParam (dJointID, dParamVel, target angular velocity);
- dJointSetHingeParam (dJointID, dParamFMax, maximum torque);
// sample7.cpp by Kosei Demura in 2005-2009
// My web site is http://demura.net
// This program uses the Open Dynamics Engine (ODE) by Russell Smith.
// The ODE web site is http://ode.org/
// Change logs
// 2009-03-07: Courtesy of Richard Kooijiman
// change mass parameters for the sphere and the capsule
// delete prepDrawStuff(), because it is not used in this source code.
#include <drawstuff/drawstuff.h>
#include <ode/ode.h>
#ifdef dDOUBLE // adaptation for single and double precision
#define dsDrawSphere dsDrawSphereD
#define dsDrawCapsule dsDrawCapsuleD
#endif
static dWorldID world;
static dSpaceID space;
static dGeomID ground;
static dJointGroupID contactgroup;
dsFunctions fn;
typedef struct {
dBodyID body;
dGeomID geom;
dReal radius;
dReal length;
dReal mass;
} myLink;
myLink ball, pole;
dJointID joint;
static void nearCallback(void *data, dGeomID o1, dGeomID o2) {
const int N = 10;
dContact contact[N];
int isGround = ((ground == o1) || (ground == o2));
int n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact));
if (isGround) {
for (int i = 0; i < n; i++) {
contact[i].surface.mode = dContactBounce;
contact[i].surface.mu = dInfinity;
contact[i].surface.bounce = 1.0; // (0.0~1.0)
contact[i].surface.bounce_vel = 0.01;
dJointID c = dJointCreateContact(world, contactgroup, &contact[i]);
dJointAttach(c, dGeomGetBody(contact[i].geom.g1),
dGeomGetBody(contact[i].geom.g2));
}
}
}
static void simLoop(int pause) {
const dReal *pos1, *R1, *pos2, *R2;
dSpaceCollide(space, 0, &nearCallback);
dWorldStep(world, 0.01);
dJointGroupEmpty(contactgroup);
dsSetColor(1.0, 0.0, 0.0);
// draw a ball
pos1 = dBodyGetPosition(ball.body);
R1 = dBodyGetRotation(ball.body);
dsDrawSphere(pos1, R1, ball.radius);
// draw a capsule
pos2 = dBodyGetPosition(pole.body);
R2 = dBodyGetRotation(pole.body);
dsDrawCapsule(pos2, R2, pole.length, pole.radius);
}
void start() {
static float xyz[3] = {0.0, -3.0, 1.0};
static float hpr[3] = {90.0, 0.0, 0.0};
dsSetViewpoint(xyz, hpr);
}
// Create a ball and a pole
void createBallandPole() {
dMass m1, m2;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
// ball
ball.radius = 0.2;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1, ball.mass, ball.radius);
dBodySetMass(ball.body, &m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space, ball.radius);
dGeomSetBody(ball.geom, ball.body);
// pole
pole.radius = 0.025;
pole.length = 1.0;
pole.mass = 1.0;
pole.body = dBodyCreate(world);
dMassSetZero(&m2);
dMassSetCapsule(&m2, pole.mass, 3, pole.radius, pole.length);
dBodySetMass(pole.body, &m2);
dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);
pole.geom = dCreateCapsule(space, pole.radius, pole.length);
dGeomSetBody(pole.geom, pole.body);
// hinge joint
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, ball.body, pole.body);
dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
dJointSetHingeAxis(joint, 1, 0, 0);
}
void setDrawStuff() {
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures =
"/Users/zhangjixiang/Code/ode-0.16.2/drawstuff/textures";
}
int main(int argc, char **argv) {
setDrawStuff();
dInitODE();
world = dWorldCreate();
space = dHashSpaceCreate(0);
contactgroup = dJointGroupCreate(0);
dWorldSetGravity(world, 0, 0, -9.8);
// Create a ground
ground = dCreatePlane(space, 0, 0, 1, 0);
// create an object
createBallandPole();
dsSimulationLoop(argc, argv, 352, 288, &fn);
dWorldDestroy(world);
dCloseODE();
return 0;
}
ODE 入门第 8 讲——ERP 与 CFM
我们使用 ODE 时需牢记参数 ERP (Error Reduction Parameter) 和 CFM (Error Reduction Parameter)。
ERP
ERP 是一个纠正关节误差的参数。随着仿真的重复进行，由于各种内部近似的原因，关节误差会增加。ERP 可以修正这个误差。其值必须设置在 0 和 1 之间。0 不能修正误差，1 可以通过下一步修正误差。典型值在 0.1 到 0.8 之间，默认值是 0.2。 不建议将 ERP 设置为 1。如果你想设置 ERP，请使用以下 API。这个 API 会影响整个世界的关节。
dWorldSetERP (dWorldID, dReal erp)
CFM
接下来，解释一下 CFM，这是 ODE 的另一个特定参数，它改变了关节的刚度。有两种类型的约束，硬约束和软约束。硬约束是必须遵守的约束。同时，软约束是不那么硬的约束。可以允许某些违反。为了控制约束，ODE 使用 CFM 参数。如果你把 CFM 设置为 0，就会应用硬约束，CFM 的值越大，更倾向软约束。
此外，ODE 使用与约束相同含义的关节。硬约束意味着该关节是一个完美的关节。也就是说，一个锚的位置和一个关节的轴线是不会被违反的。换句话说，这个关节是硬的。软约束意味着关节不是完美的。换句话说，这个关节是灵活的。
你可以仿真一个弹簧和一个阻尼器系统来使用 ERP 和 CFM。弹簧和阻尼器系统经常被使用，如一个人形脚和地面之间的模型。请阅读 ODE 的用户手册。
接下来解释如何使用 CFM。如果你想设置 CFM。请使用下面的 API。然而，这个 API 适用于世界上的所有关节。在 single 精度的情况下，CFM 的默认值是 1E-5 (10^-5)，在 double 精度的情况下，其值是 1E-10 (10^-10)。
dWorldSetCFM (dWorldID, dReal cfm)
此外，CFM 的作用是稳定仿真。所以，在系统不稳定的情况下，增加 CFM。但是，关节会变软。推荐值是不大于 1E-3 (10^-3)。
ODE 入门第 9 讲—位置R3与姿态SO3
单位系统
ODE 并不介意任何单位制。角度默认采用[弧度]。在本教程中，使用的是 SI 单位系统。这对我们来说是非常熟悉的。长度是[m]，重量是[kg]，时间是[s]。
坐标系统
如上图所示，ODE的坐标系是直角坐标系，并且是右手坐标系的。它在物理学和数学中被广泛使用如上图所示，有九个小金字塔。 坐标系的原点是金字塔的中心，X轴是从中心到红色金字塔的方向，y轴为从中心到蓝色金字塔的方向，z轴是指从中心到天空的方向。教程采用 SI 单位系统，所以金字塔之间的距离是 1[m]。
设置位置和姿态
我们必须设置每个刚体的位置和方向。以下是用于该目的的 API。
- void dBodySetPosition (dBodyID，dReal x，dReal y，dReal z)
- void dRFromAxisAndAngle (dMatrix3 R, dReal ax, dReal ay, dReal az, dReal angle)
- void dBodySetRotation (dBodyID, const dMatrix3 R)
此外，四元数也可以在 ODE 中使用。其 API 是 dBodySetQuaternion (dBodyID, const dQuaternion q)。
获取位置和姿态
- const dReal *dBodyGetPosition (dBodyID)
- const dReal *dBodyGetRotation (dBodyID)
ODE 入门第 10 讲——速度与加速度
速度与角速度
dBodySetLinearVel() 设置一个线速度，而 dBodySetAngularVel() 设置一个体的角速度。速度是重心的速度，角速度是围绕重心的速度。你可以在初始状态下设置速度和角速度。 不建议在每个模拟步骤中设置这些。它滥用了你的物理学模型。
如果你想控制一个物体的速度。请使用一个关节电机。
- void dBodySetLinearVel (dBodyID body，dReal x，dReal y，dReal z)
- void dBodySetAngularVel (dBodyID body, dReal x, dReal y, dReal z)
- const dReal *dBodyGetLinearVel (dBodyID body)
- const dReal *dBodyGetAngularVel (dBodyID body)
加速度与角加速度
不幸的是，没有 API 来获取加速度和角加速度。如果你想要它们，用速度的变化除以步长，时间步长是 dWorldStep(dWorldID, dReal stepsize) 的第二个参数。stepsize 是一个时间步长，在数值积分中使用。
值得注意的是，dBodyGetLinearVel() 与 dBodyGetAngularVel() 返回一个由三个元素组成的数组指针。
ODE 入门第 11 讲——力和力矩
刚体的力和力矩
- const dReal *dBodyGetForce (dBodyID body)
- const dReal *dBodyGetTorque (dBodyID body)
- void dBodySetForce (dBodyID body, dReal x, dReal y, dReal z)
- void dBodySetTorque (dBodyID body, dReal x, dReal y, dReal z)
关节的力和力矩
获取力和力矩
接下来，让我们研究一下如何获得一个关节的力和力矩。为了获得力和力矩，首先必须调用 dJointSetFeedback() 来指定一个关节，其次，调用 dJointGetFeedback() 来获得信息。这是为了提高性能。你并不总是需要所有关节的力和扭矩信息。
设置功率和力矩
根据关节的类型，将设置力或力矩。换句话说，如一个旋转关节，一个铰链逛街，必须施加力矩。对于一个滑块关节，必须应用力。
- dJointAddHingeTorque (dJointID joint, dReal torque)
- dJointAddSliderForce (dJointID joint, dReal force)
ODE 入门第 12 讲——摩擦
摩擦力模型
abs(f_T) <= μ abs(f_N)
其中 f_N 是法线方向，f_T 是切向力矢量，μ 是摩擦系数。该方程表示摩擦锥，f_N 矢量是锥体的轴，顶点是接触点。如果 f_T 和 f_N 的组成矢量在摩擦锥内，则在接触点没有滑动。如果组成的矢量在摩擦锥体之外，就会出现滑移。
ODE 采用了库仑摩擦模型的近似值。ODE 有两种模型：恒定力极限模型和摩擦金字塔近似模型。默认的模型是恒力极限模型。如果物体和地面之间的碰撞对模拟非常重要，请选择摩擦金字塔近似模型。
恒定力极限模型
摩擦系数 μ 是按照最大摩擦力模型来模拟的。这不是精确的物理模型，但是计算成本是最低的。
摩擦金字塔近似
摩擦锥被建模为摩擦金字塔。这个模型比恒力极限更准确。如果你想使用这个模型，你必须设置 dContactApprox1 标志。
ODE 入门第 13 讲——DIY 机械臂🦾
我们做一个简单的 3 关节机械臂，用到目前为止的所学知识。使用 ODE 只需要 100 行就可以做出这样的模拟器。为了简化例子程序，省略了碰撞检测。该机器人可以通过键盘操作。第一个关节可以通过 j（增加）或f（减少）键移动，d和k键移动第二个关节，而l和s键移动第三个关节。
要移动关节，使用一个简单的控制，即 P控制，它控制关节与角速度的当前值和目标速度的误差成正比。关节角度的当前值与目标值相差甚远，如果你的角速度与角速度的目标值相同，则停止下一个关节。另外，由于目标为当前的最佳值，你将以一个负号进行反转，而当前值则接近于目标值。
控制功能实现。dParamVel 是目标角速度，dParamFMax 是实现角速度的最大扭矩。如果 dParamFMax 被设置为 0，就不能控制关节，因为没有向关节提供扭矩。
ODE 入门第 14 讲——加速仿真
关闭可视化 Drawstuff 功能：不在主函数中调用 dsSimulationLoop 和与绘图有关的函数。
ODE 入门第 15 讲——键盘交互
