17-python3-new-forcefield.pyImplementation of a RestShapeForceField in Python import Sofa import SofaRuntime import numpy as np # # 自定义 Python ForceField # class RestShapeForceField(Sofa.Core.ForceFieldVec3d): def __init__(self, ks1.0, kd1.0, *args, **kwargs): Sofa.Core.ForceFieldVec3d.__init__( self, *args, **kwargs ) # 弹簧刚度 self.addData( ks, typefloat, valueks, helpThe stiffness spring, groupSprings Properties ) # 阻尼系数 self.addData( kd, typefloat, valuekd, helpThe damping spring, groupSprings Properties ) # # 初始化 # def init(self): # 获取当前节点中的 MechanicalObject mstate self.getContext().mechanical # 保存初始位置 self.initpos ( mstate.position.array().copy() ) self.k np.zeros((1, 1)) self.f [] self.d 0.5 # # 计算力 # def addForce( self, m, out_force, pos, vel ): with out_force.writeableArray() as wa: # 弹簧恢复力 # # Fspring ks * (x0 - x) # # 阻尼力 # # Fdamping -kd * v wa[:] ( (self.initpos - pos.value) * self.ks.value - vel.value * self.kd.value ) # # 力的导数 # # 新版 SofaPython3 的函数参数形式 # def addDForce( self, m, dforce, dx ): pass # # 创建场景 # def createScene(root): root.gravity [0.0, -9.81, 0.0] root.name root root.dt 0.01 root.addObject( RequiredPlugin, nameloadSOFAModules, pluginName[ Sofa.Component.AnimationLoop, Sofa.Component.LinearSolver.Iterative, Sofa.Component.Mass, Sofa.Component.ODESolver.Backward, Sofa.Component.StateContainer, ], ) # 改成 True root.addObject( DefaultAnimationLoop, computeBoundingBoxTrue ) # 给这个很小的场景指定合适的显示范围 root.bbox [ [-1.0, -1.0, -1.0], [2.0, 1.0, 1.0] ] objectNode root.addChild( Object ) objectNode.addObject( EulerImplicitSolver ) objectNode.addObject( CGLinearSolver, iterations200, tolerance1e-12, threshold1e-12 ) # 两个三维点 # # P0 (0, 0, 0) # P1 (1, 0, 0) MO objectNode.addObject( MechanicalObject, templateVec3d, namemechanical, position[ 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 ], showObjectTrue, # 关键修改 drawMode2, showObjectScale0.15, showColor[ 1.0, 0.0, 0.0, 1.0 ] ) objectNode.addObject( UniformMass, namemass, totalMass0.1 ) objectNode.addObject( RestShapeForceField( namePythonRestShapeForceField, ks2.0, kd0.1 ) ) return root def main(): import Sofa.Gui # -------------------------------------------------------- # 新版 GUISofaImGui # -------------------------------------------------------- SofaRuntime.importPlugin( SofaImGui ) # -------------------------------------------------------- # 创建 root # -------------------------------------------------------- root Sofa.Core.Node( root ) # -------------------------------------------------------- # 创建 Scene Graph # -------------------------------------------------------- createScene(root) # -------------------------------------------------------- # 初始化整个场景 # # 新版使用 initRoot # -------------------------------------------------------- Sofa.Simulation.initRoot( root ) # -------------------------------------------------------- # 查看当前支持的 GUI # -------------------------------------------------------- print( Supported GUIs:, Sofa.Gui.GUIManager.ListSupportedGUI() ) # -------------------------------------------------------- # 使用 ImGui # -------------------------------------------------------- Sofa.Gui.GUIManager.Init( myscene, imgui ) Sofa.Gui.GUIManager.createGUI( root, __file__ ) Sofa.Gui.GUIManager.SetDimension( 1080, 800 ) Sofa.Gui.GUIManager.MainLoop( root ) Sofa.Gui.GUIManager.closeGUI() print( End of simulation. ) # # 程序入口 # if __name__ __main__: main()代码16主要展示Python Controller→ 读取 Data→ 修改 Data→ 动态修改 Scene Graph而代码17进一步展示Python 不只是操作 SOFA 已经存在的组件还可以自己实现新的物理模型。这里自己实现的是一个 ForceField力场也就是class RestShapeForceField( Sofa.Core.ForceFieldVec3d ):1.这个场景的力学结构1.1.重力root.gravity [0.0, -9.81, 0.0]表示 g(0,−9.81,0) ,因此物体受到向下的重力。1.2.AnimationLooproot.addObject( DefaultAnimationLoop, computeBoundingBoxTrue )它负责组织一个时间步中各个仿真计算步骤的执行。root.bbox [ [-1.0, -1.0, -1.0], [2.0, 1.0, 1.0] ]因为场景只有两个距离为 1 的小点手动规定一个合适的显示范围避免在 GUI 中看起来几乎什么都没有。1.3.创建 Object 子节点objectNode root.addChild( Object )场景图结构root└── Object后面的求解器、MechanicalObject、质量、自定义 ForceField 都放在这个Object节点下面。1.4.积分器和线性求解器时间积分器objectNode.addObject( EulerImplicitSolver )线性求解器objectNode.addObject( CGLinearSolver, iterations200, tolerance1e-12, threshold1e-12 )2.MechanicalObjectMO objectNode.addObject( MechanicalObject, templateVec3d, namemechanical, position[ 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 ], showObjectTrue, drawMode2, showObjectScale0.15, showColor[ 1.0, 0.0, 0.0, 1.0 ] )2.1.position 不是一个六维点position[ 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 ]实际上代表两个Vec3d点。第一个第二个因此场景里有两个三维自由度点,不是一个六维刚体。2.2.显示两个点showObjectTrue drawMode2 showObjectScale0.15 showColor[1.0, 0.0, 0.0, 1.0]drawMode2 用于把自由度明显画成球。showObjectScale0.15 控制球的显示大小。这里是为了让两个 MechanicalObject 自由度在 SOFA 中看清楚不参与物理计算。2.3.两个点的质量objectNode.addObject( UniformMass, namemass, totalMass0.1 )整个 MechanicalObject 的总质量为3.如果去掉 Python ForceField会发生什么现在场景最后还有objectNode.addObject( RestShapeForceField( namePythonRestShapeForceField, ks2.0, kd0.1 ) )我们先暂时去掉这一部分运行场景。这样场景只剩MechanicalObject UniformMass Gravity EulerImplicitSolver CGLinearSolver因为 root.gravity [0.0, -9.81, 0.0] 所以两个点受到重力而没有其他力把它们拉住。然后再重新加入自定义 ForceField对比新的物理行为没有 RestShapeForceField→ 自由下落加入 RestShapeForceField→ 出现恢复力和阻尼力4.RestShapeForceField 不是普通 SOFA 组件普通内置组件通常这样加objectNode.addObject( UniformMass, ... )这里 UniformMass 是 SOFA 已经存在的组件名。但是自定义 ForceFieldobjectNode.addObject( RestShapeForceField( namePythonRestShapeForceField, ks2.0, kd0.1 ) )不是字符串。它是在 Python 中创建 RestShapeForceField(...) 这个类的一个实例。类定义class RestShapeForceField( Sofa.Core.ForceFieldVec3d ):继承关系Sofa.Core.ForceFieldVec3d↑│ 继承│RestShapeForceField为什么要继承 ForceFieldVec3d——因为当前 MechanicalObject 使用 templateVec3d也就是三维点自由度。ForceField 的任务就是根据当前位置、速度等状态计算力并把这些力加入 MechanicalObject 的总力。5.构造函数ks 和 kddef __init__(self, ks1.0, kd1.0, *args, **kwargs):先初始化父类Sofa.Core.ForceFieldVec3d.__init__( self, *args, **kwargs )然后添加两个自己的 SOFA Dataks、kd5.1.构造函数构造函数是创建对象时自动执行的特殊方法用来给对象初始化。class Particle: def __init__(self, x, y): # 这就是构造函数 self.x x # 给新对象设置初始x坐标 self.y y # 给新对象设置初始y坐标 self.velocity 0 # 初始速度为0名字固定叫__init__前后各两个下划线第一个参数固定是self代表正在创建的那个对象不能手动调用它创建对象时自动触发5.2.ks弹簧刚度self.addData( ks, typefloat, valueks, helpThe stiffness spring, groupSprings Properties )ks表示spring stiffness 弹簧刚度它会出现在恢复力中5.3.kd阻尼系数self.addData( kd, typefloat, valuekd, helpThe damping spring, groupSprings Properties )kd是damping coefficient 阻尼系数弹簧刚度Stiffness抵抗 位移 的力 —— 形变大 → 回复力大阻尼系数Damping抵抗 速度 的力 —— 运动快 → 阻力大经典弹簧 - 阻尼 - 质点系统的合力F -k·x - c·v6.init() 保存物体初始位置def init(self): mstate self.getContext().mechanical self.initpos ( mstate.position.array().copy() ) self.k np.zeros((1, 1)) self.f [] self.d 0.56.1.获取当前节点中的 MechanicalObjectmstate self.getContext().mechanicalRestShapeForceField位于 Object 节点中。同一节点里还有MechanicalObject( namemechanical )因此 self.getContext().mechanical 就是取得这个 MechanicalObject。RestShapeForceField↓找到自己所在 Object 节点↓找到其中的 mechanical state↓读取 MechanicalObject6.2.保存初始位置self.initpos ( mstate.position.array().copy() )初始 MechanicalObject 位置[ [0, 0, 0], [1, 0, 0] ]所以 self.initpos 保存的是也就是每个点最开始的位置。为什么必须保存因为后面恢复力需要也就是初始位置 - 当前位置7.addForce()addForce() 是代码17的核心def addForce( self, m, out_force, pos, vel ): with out_force.writeableArray() as wa: wa[:] ( (self.initpos - pos.value) * self.ks.value - vel.value * self.kd.value )7.1.pos是当前位置pos.value 就是 MechanicalObject 当前时刻的位置self.initpos 是所以self.initpos - pos.value 表示也就是当前点相对于自己的初始位置发生了多少位移以及应该往哪个方向返回。7.2.乘上 ks 得到恢复力( self.initpos - pos.value ) * self.ks.value对应等价于这就是弹簧恢复力。7.3.弹簧连接在哪里这里不是粒子A弹簧粒子B而是每一个点都和自己的初始位置建立一个恢复关系。可以理解成初始位置 x₀●│/\/\/\/│● 当前位置 x如果点偏离初始位置就会产生把它往初始位置拉。7.4.RestShapeForceField 和 FixedConstraint 的区别FixedConstraint→ 直接约束自由度→ 不允许它运动RestShapeForceField→ 不锁死自由度→ 允许运动→ 只是产生恢复力把它拉回来7.5.out_force.writeableArray()with out_force.writeableArray() as wa:和前面代码16中的writeableArray()是同一个核心概念。它表示获得当前力 Data 的可写数组。wa[:] ...注意这里是 而不是 。因此不是把其他力覆盖掉而是将这个 ForceField 计算出的力加入已有总力。最终7.6.加入阻尼最开始只有这是一个纯弹簧。但纯弹簧会产生持续振荡因此接下来加入阻尼。vel.value 表示当前 MechanicalObject 的速度假设参考速度于是因此阻尼力阻尼力始终倾向于阻碍当前运动。最终成为弹簧—阻尼系统spring-damper systemPython 不只是控制 SOFAPython 还可以扩展 SOFA 的物理模型。8.代码17完整逻辑createScene()↓创建两个 Vec3d 自由度P0(0,0,0)P1(1,0,0)↓添加质量和重力↓加入自定义RestShapeForceField↓init()↓保存初始位置 x₀↓每个时间步 SOFA 调用 addForce()↓读取当前位置 x读取当前速度 v↓计算F ks(x₀-x) - kd·v↓通过 writeableArray()加入 out_force↓EulerImplicitSolverCGLinearSolver↓计算新的运动状态