2. 跟做实例
跟做上一篇的实例过程中意外关机了，重启后那个实例相关的代码都无法运行了，自己写的和源代码都不行。后来莫名其妙又能运行了，愁死个人。
这一篇的代码解读比较粗糙，因为很多是直接从sofa的api里调用模块，没什么好分析的，会用就行。
相比前一篇，能学到的东西比较少，因此看起来很枯燥，更建议使用时自己对照仿真阅读源代码。
注：我用的sofa都是python3版本的，官方文档说要3.7及以后。
2.3 SoftFingerDesign
这部分要实现的效果：刚软混合手指结构，刚体部分的电机做旋转驱动，软体部分被动变形。
我需要从这个实例中了解的：
刚性结构的导入；
刚软混合的连接与固定方法；
闭环结构的实现方式。
刚性结构的驱动。
这个项目的程序和模型结构图大概如图所示，其中漏掉一些模型，不过不重要，读代码时会发现的。主函数是gripper_simulation。
下图是这个仿真的模型的树形结构图，框出来的是刚才看文件时比较眼熟的，可以仔细分析其中的主从关系：
2.3.1 模型的导入与构建
一步步来，首先导入电机模型。
构建电机模型
先在sofa里观察电机模型和其他部分的连接形式：
下面是代码，代码主要是设置电机的参数，如果觉得烦，可以不去探究，只在使用时直接调用这个电机，参照例子修改参数。
import os
import Sofa
from stlib3.scene import Scene
dirPath = os.path.dirname(os.path.abspath(__file__)) + '/'
class ServoMotor(Sofa.Prefab):
"""
该预制件实现了S90伺服电机。
电机链接（不太重要，要vpn，打不开就算了）https://servodatabase.com/servo/towerpro/sg90
预制伺服电机由以下部件组成:
- 视觉模型
- 由两个刚体组成的机械模型。一个刚性用于电机主体，而另一个刚性实现伺服旋转轮。
该预设有以下参数:
- translation 电机的位置 (默认 [0.0,0.0,0.0])
- rotation 电机的姿态 (默认 [0.0,0.0,0.0,1])
- scale 电机大小尺度（多少倍，大概） (默认 1)
- showServo 是否添加视觉模型 (默认 True)
- showWheel 是否显示电机转轴的连体坐标系 (默认 False)
该预设有以下属性
- angle 指定伺服电机的旋转角度
- angleLimits 设置伺服角度旋转的最小值和最大值
- position 指定伺服电机的位置
在sofa场景中使用这个电机模型的例子:
def addScene(root):
...
servo = ServoMotor(root)
## Direct access to the components
servo.angle.value = 1.0
"""
# 注意一下下面定义的这俩列表可能是在初始化时传入Prefab模块的
prefabParameters = [
{'name': 'rotation', 'type': 'Vec3d', 'help': 'Rotation', 'default': [0.0, 0.0, 0.0]},
{'name': 'translation', 'type': 'Vec3d', 'help': 'Translation', 'default': [0.0, 0.0, 0.0]},
{'name': 'scale3d', 'type': 'Vec3d', 'help': 'Scale 3d', 'default': [1.0e-3, 1.0e-3, 1.0e-3]}]
prefabData = [
{'name': 'minAngle', 'help': 'min angle of rotation (in radians)', 'type': 'float', 'default': -100},
{'name': 'maxAngle', 'help': 'max angle of rotation (in radians)', 'type': 'float', 'default': 100},
{'name': 'angleIn', 'help': 'angle of rotation (in radians)', 'type': 'float', 'default': 0},
{'name': 'angleOut', 'help': 'angle of rotation (in degree)', 'type': 'float', 'default': 0}
]
def __init__(self, *args, **kwargs):
# 会根据继承的父类Sofa.Prefab初始化
"""
调用时的用法：
ServoMotor(name="ServoMotor", translation=self.translation.value, rotation=self.rotation.value)
"""
Sofa.Prefab.__init__(self, *args, **kwargs)
def init(self):
# 在self节点下创建子节点Servo body， self的节点应该是在初始化时根据传入参数创建的
servoBody = self.addChild('ServoBody')
# 这个dof在软件开始仿真后可以读取一些变化信息，比如力、位移等
servoBody.addObject('MechanicalObject', name='dofs', template='Rigid3', position=[[0., 0., 0., 0., 0., 0., 1.]],
translation=list(self.translation.value), rotation=list(self.rotation.value),
scale3d=list(self.scale3d.value))
# 固定约束
servoBody.addObject('FixedConstraint', indices=0)
# 均匀质量
servoBody.addObject('UniformMass', totalMass=0.01)
# visual这部分如果不写，也能跑，只是不显示电机模型。
visual = servoBody.addChild('VisualModel')
visual.addObject('MeshSTLLoader', name='loader', filename='Data/SG90_servomotor_finger.stl', scale=1e-3,
rotation=[0.0, -90.0, 0.0], translation=[-12.0e-3, -5.0e-3, 0.0])
visual.addObject('MeshTopology', src='@loader')
visual.addObject('OglModel', color=[0.15, 0.45, 0.75, 0.7], writeZTransparent=True)
visual.addObject('RigidMapping', index=0)
# Servo wheel 伺服轮（可以理解为与轴连接的坐标系）
angle = self.addChild('Articulation')
angle.addObject('MechanicalObject', name='dofs', template='Vec1', position=[[self.getData('angleIn')]],
rest_position=self.getData('angleIn').getLinkPath())
angle.addObject('RestShapeSpringsForceField', points=0, stiffness=1e9)
angle.addObject('UniformMass', totalMass=0.01)
# 电机的dofs和它的伺服轮的dofs应该就是在这一句里进行的绑定
servoWheel = angle.addChild('ServoWheel')
servoWheel.addObject('MechanicalObject', name='dofs', template='Rigid3',
position=[[0., 0., 0., 0., 0., 0., 1.], [0., 0., 0., 0., 0., 0., 1.]],
translation=list(self.translation.value), rotation=list(self.rotation.value),
scale3d=list(self.scale3d.value))
servoWheel.addObject('ArticulatedSystemMapping', input1="@../dofs", input2="@../../ServoBody/dofs",
output="@./")
articulationCenter = angle.addChild('ArticulationCenter')
articulationCenter.addObject('ArticulationCenter', parentIndex=0, childIndex=1, posOnParent=[0., 0., 0.],
posOnChild=[0., 0., 0.])
articulation = articulationCenter.addChild('Articulations')
articulation.addObject('Articulation', translation=False, rotation=True, rotationAxis=[1, 0, 0],
articulationIndex=0)
angle.addObject('ArticulatedHierarchyContainer', printLog=False)
# 将伺服电机的输出以dofs的position输出
self.angleOut.setParent(angle.dofs.position)
def createScene(rootNode):
import math
from splib3.animation import animate
def animation(target, factor):
target.angleIn.value = math.sin(factor * 2 * math.pi)
scene = Scene(rootNode, plugins=['SofaConstraint', 'SofaGeneralRigid', 'SofaOpenglVisual', 'SofaRigid'],
iterative=False)
scene.addMainHeader()
scene.addObject('DefaultVisualManagerLoop')
scene.addObject('FreeMotionAnimationLoop')
# 通用约束求解器
scene.addObject('GenericConstraintSolver', maxIterations=1e3, tolerance=1e-5)
# 通用约束校正
scene.Simulation.addObject('GenericConstraintCorrection')
scene.dt = 0.01
scene.gravity = [0., -9810., 0.]
scene.Simulation.addChild(ServoMotor(name="ServoMotor"))
animate(animation, {'target': scene.Simulation.ServoMotor}, duration=5., mode='loop')
scene.Simulation.ServoMotor.Articulation.ServoWheel.dofs.showObject = True
scene.Simulation.ServoMotor.Articulation.ServoWheel.dofs.showObjectScale = 2e-2
return scene这部分代码写完后在sofa中运行结果如图所示：，其中一个坐标系在一定角度范围内往复转动。
这个实例的驱动是这种伺服电机，不知道能不能自由定义电机，我想定义其他型号的电机。。。。
构建伺服摆臂模型的
分为两步：1. 导入摆动臂； 2. 将电机和摆动臂装配起来。
摆动臂：
代码：
# -*- coding: utf-8 -*-
"""
本部分包含以下两个类:
- ActuatedArm
- ServoArm
"""
import Sofa
from stlib3.visuals import VisualModel
from s90_servo import ServoMotor # 引用上一节构建的电机
class ServoArm(Sofa.Prefab):
"""
Parameters:
parent: 说明摆动臂会被连接到哪个节点
mappingInput: 控制摆动臂方向的刚性机械物体，一般可以是电机伺服轮的dofs
indexInput: (int) 摆动臂应映射到的刚体的索引
"""
prefabParameters = [
{'name': 'mappingInputLink', 'type': 'string',
'help': 'the rigid mechanical object that will control the orientation of the servo arm', 'default': ''},
{'name': 'indexInput', 'type': 'int', 'help': 'index of the rigid the ServoArm should be mapped to',
'default': 1}]
def __init__(self, *args, **kwargs):
Sofa.Prefab.__init__(self, *args, **kwargs)
def init(self):
self.addObject('MechanicalObject',
name='dofs', # 这个多半是用来和电机的dofs对齐的
size=1,
template='Rigid3', # 奇奇怪怪的参数，可能有预设吧。。。找不到
showObject=False,
showObjectScale=5e-3,
translation2=[0, 0, -25e-3])
# 设置刚性映射
def setRigidMapping(self, path):
self.addObject('RigidRigidMapping', name='mapping', input=path, index=self.indexInput.value)
# 我猜这个visual也可以不显示
visual = self.addChild(VisualModel(visualMeshPath='Data/SG90_servoarm.stl', translation=[0, 0, 25.0e-3],
rotation=[-90, 0, 0],
scale=[1.0e-3, 1.0e-3, 1.0e-3], color=[1., 1., 1., 0.75]))
visual.OglModel.writeZTransparent = True
visual.addObject('RigidMapping', name='mapping')
class ActuatedArm(Sofa.Prefab):
"""
参数:
- 位置数组
- 欧拉姿态数组
结构体:
Node : {
name : 'ActuatedArm'
MechanicalObject // 电机的位置
ServoMotor // 电机及其伺服轮
ServoArm // 摆动臂
}
"""
prefabParameters = [
{'name': 'rotation', 'type': 'Vec3d', 'help': 'Rotation', 'default': [0.0, 0.0, 0.0]},
{'name': 'translation', 'type': 'Vec3d', 'help': 'Translation', 'default': [0.0, 0.0, 0.0]},
{'name': 'scale', 'type': 'Vec3d', 'help': 'Scale 3d', 'default': [1.0e-3, 1.0e-3, 1.0e-3]}]
prefabData = [
{'name': 'angleIn', 'group': 'ArmProperties', 'help': 'angle of rotation (in radians) of the arm',
'type': 'float', 'default': 0},
{'name': 'angleOut', 'group': 'ArmProperties', 'type': 'float', 'help': 'angle of rotation (in radians) of '
'the arm', 'default': 0}
]
def __init__(self, *args, **kwargs):
Sofa.Prefab.__init__(self, *args, **kwargs)
def init(self):
# 导入伺服电机
self.servomotor = self.addChild(ServoMotor(name="ServoMotor", translation=self.translation.value,
rotation=self.rotation.value))
# 导入摆动臂
self.servoarm = self.servomotor.Articulation.ServoWheel.addChild(ServoArm(name="ServoArm"))
# 将电机驱动轮的dofs 映射到摆动臂的dofs上（或者说将二者装配好）
self.servoarm.setRigidMapping(self.ServoMotor.Articulation.ServoWheel.dofs.getLinkPath())
# add a public attribute and connect it to the private one.
self.ServoMotor.angleIn.setParent(self.angleIn)
# add a public attribute and connect it to the internal one.
self.angleOut.setParent(self.ServoMotor.angleOut)
# 创建场景，不重要，使用时可直接参考这部分的用法
def createScene(rootNode):
from splib3.animation import animate
from stlib3.scene import Scene
import math
scene = Scene(rootNode, plugins=['SofaConstraint', 'SofaGeneralRigid', 'SofaRigid'], iterative=False)
scene.addMainHeader()
scene.addObject('DefaultVisualManagerLoop')
scene.addObject('FreeMotionAnimationLoop')
scene.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5)
scene.Simulation.addObject('GenericConstraintCorrection')
scene.dt = 0.01
scene.gravity = [0., -9810., 0.]
arm = scene.Simulation.addChild(ActuatedArm(name='ActuatedArm', translation=[0.0, 0.0, 0.0]))
def myanimate(target, factor):
target.angleIn.value = math.sin(factor * 2 * math.pi)
animate(myanimate, {'target': arm}, duration=2., mode='loop')
定义软体材料
在引入软体手指之前，要先定义软体材料。
这个软体材料其实是sofa库自带的，可以直接调用，不看这个python文件也没关系。不过可以看看它有哪些参数需要引入。
# -*- coding: utf-8 -*-
import Sofa.Core
from stlib3.visuals import VisualModel
class ElasticMaterialObject(Sofa.Prefab):
"""创建由弹性材质组成的对象"""
prefabParameters = [
{'name': 'volumeMeshFileName', 'type': 'string', 'help': 'Path to volume mesh file', 'default': ''}, # 体积网格文件
{'name': 'rotation', 'type': 'Vec3d', 'help': 'Rotation', 'default': [0.0, 0.0, 0.0]}, # 旋转
{'name': 'translation', 'type': 'Vec3d', 'help': 'Translation', 'default': [0.0, 0.0, 0.0]}, # 移动
{'name': 'scale', 'type': 'Vec3d', 'help': 'Scale 3d', 'default': [1.0, 1.0, 1.0]}, # 尺度倍率
{'name': 'surfaceMeshFileName', 'type': 'string', 'help': 'Path to surface mesh file', 'default': ''}, # 表面网格文件
{'name': 'collisionMesh', 'type': 'string', 'help': 'Path to collision mesh file', 'default': ''}, # 碰撞网格
{'name': 'withConstrain', 'type': 'bool', 'help': 'Add constraint correction', 'default': True}, # 添加约束
{'name': 'surfaceColor', 'type': 'Vec4d', 'help': 'Color of surface mesh', 'default': [1., 1., 1., 1.]}, # 面网格颜色
{'name': 'poissonRatio', 'type': 'double', 'help': 'Poisson ratio', 'default': 0.3}, # 泊松比
{'name': 'youngModulus', 'type': 'double', 'help': "Young's modulus", 'default': 18000}, # 杨氏模量
{'name': 'totalMass', 'type': 'double', 'help': 'Total mass', 'default': 1.0}, # 总质量
{'name': 'topoMesh', 'type': 'string', 'help': 'Mesh topology', 'default': "tetrahedron"}, # 网格拓扑结构
{'name': 'solverName', 'type': 'string', 'help': 'Solver name', 'default': ''}] # 求解器
def __init__(self, *args, **kwargs):
Sofa.Prefab.__init__(self, *args, **kwargs)
def init(self):
plugins = []
if self.solverName.value == '':
plugins.append('SofaImplicitOdeSolver')
self.integration = self.addObject('EulerImplicitSolver', name='integration')
plugins.append('SofaSparseSolver')
self.solver = self.addObject('SparseLDLSolver', name="solver")
if self.volumeMeshFileName.value == '':
Sofa.msg_error(self, "Unable to create an elastic object because there is no volume mesh provided.")
return None
if self.volumeMeshFileName.value.endswith(".msh"):
self.loader = self.addObject('MeshGmshLoader', name='loader', filename=self.volumeMeshFileName.value,
rotation=list(self.rotation.value), translation=list(self.translation.value),
scale3d=list(self.scale.value))
elif self.volumeMeshFileName.value.endswith(".gidmsh"):
self.loader = self.addObject('GIDMeshLoader', name='loader', filename=self.volumeMeshFileName.value,
rotation=list(self.rotation.value), translation=list(self.translation.value),
scale3d=list(self.scale.value))
else:
self.loader = self.addObject('MeshVTKLoader', name='loader', filename=self.volumeMeshFileName.value,
rotation=list(self.rotation.value), translation=list(self.translation.value),
scale3d=list(self.scale.value))
if self.topoMesh.value == "tetrahedron":
self.container = self.addObject('TetrahedronSetTopologyContainer', src=self.loader.getLinkPath(),
name='container')
# Sofa.msg_error(self,"tetra") #print("Tetra")
else:
self.container = self.addObject('HexahedronSetTopologyContainer', src=self.loader.getLinkPath(),
name='container')
# Sofa.msg_error(self,"hexa") #print("hexa")
self.dofs = self.addObject('MechanicalObject', template='Vec3', name='dofs')
# To be properly simulated and to interact with gravity or inertia forces, an elasticobject
# also needs a mass. You can add a given mass with a uniform distribution for an elasticobject
# by adding a UniformMass component to the elasticobject node
self.mass = self.addObject('UniformMass', totalMass=self.totalMass.value, name='mass')
# The next component to add is a FEM forcefield which defines how the elasticobject reacts
# to a loading (i.e. which deformations are created from forces applied onto it).
# Here, because the elasticobject is made of silicone, its mechanical behavior is assumed elastic.
# This behavior is available via the TetrahedronFEMForceField component.
plugins.append('SofaSimpleFem')
if self.topoMesh.value == "tetrahedron":
self.forcefield = self.addObject('TetrahedronFEMForceField', template='Vec3',
method='large', name='forcefield',
poissonRatio=self.poissonRatio.value, youngModulus=self.youngModulus.value)
else:
self.forcefield = self.addObject('HexahedronFEMForceField', template='Vec3',
method='large', name='forcefield',
poissonRatio=self.poissonRatio.value, youngModulus=self.youngModulus.value)
if self.withConstrain.value:
plugins.append('SofaConstraint')
self.correction = self.addObject('LinearSolverConstraintCorrection', name='correction')
if self.collisionMesh:
self.addCollisionModel(self.collisionMesh.value, list(self.rotation.value), list(self.translation.value),
list(self.scale.value))
if self.surfaceMeshFileName:
self.addVisualModel(self.surfaceMeshFileName.value, list(self.surfaceColor.value),
list(self.rotation.value), list(self.translation.value), list(self.scale.value))
self.addObject('RequiredPlugin', pluginName=plugins)
def addCollisionModel(self, collisionMesh, rotation=[0.0, 0.0, 0.0], translation=[0.0, 0.0, 0.0],
scale=[1., 1., 1.]):
self.collisionmodel = self.addChild('CollisionModel')
self.collisionmodel.addObject('MeshSTLLoader', name='loader', filename=collisionMesh, rotation=rotation,
translation=translation, scale3d=scale)
self.collisionmodel.addObject('TriangleSetTopologyContainer', src='@loader', name='container')
self.collisionmodel.addObject('MechanicalObject', template='Vec3', name='dofs')
self.collisionmodel.addObject('TriangleCollisionModel')
self.collisionmodel.addObject('LineCollisionModel')
self.collisionmodel.addObject('PointCollisionModel')
self.collisionmodel.addObject('BarycentricMapping')
def addVisualModel(self, filename, color, rotation, translation, scale=[1., 1., 1.]):
visualmodel = self.addChild(
VisualModel(visualMeshPath=filename, color=color, rotation=rotation, translation=translation, scale=scale))
# Add a BarycentricMapping to deform the rendering model to follow the ones of the
# mechanical model.
visualmodel.addObject('BarycentricMapping', name='mapping')
def createScene(rootNode):
from stlib3.scene import MainHeader
MainHeader(rootNode, gravity=[0, 0, 0])
rootNode.addChild(ElasticMaterialObject(name='ElasticMaterialObject1', volumeMeshFileName="Data/liver.msh",
translation=[3.0, 0.0, 0.0]))
rootNode.addChild(ElasticMaterialObject(name='ElasticMaterialObject2', volumeMeshFileName="mesh/liver.msh",
translation=[-3, 0, 0], surfaceMeshFileName="mesh/liver.obj",
surfaceColor=[1.0, 0.0, 0.0]))
构建软体手指
本部分先只介绍导入软体手指部分，完整代码在下面的刚软连接里
def elasticBody(self):
# Create a body as a child of the parent (the actuated finger)
body = self.addChild("ElasticBody")
"""
创建一个软体对象，它导入一个网格，为其指定自由度和机械特性。所有特性都以国际单位表示。
生成的网格中的尺寸以米为单位，杨氏模量以帕斯卡为单位
Filaflex70A长丝的制造商表示其杨氏模量为32MPa。泊松比固定在0.45，与标准弹性体（硅树脂、橡胶）一样。
旋转和平移进行调整，以使网格正确定位在伺服电机上
"""
e = body.addChild(ElasticMaterialObject(
volumeMeshFileName="Data/finger.msh", # 软体部分的网格模型文件
topoMesh="tetrahedron", # 选用四面体作为拓扑网格（除此以外还可选用六面体集拓扑容器）
scale=[1, 1, 1], # 使用原始尺度倍率
totalMass=0.015, # 总质量
youngModulus=32e6, # 杨氏模量
poissonRatio=0.45, # 泊松比
rotation=[90.0, 0.0, 0.0], # 旋转
translation=[-30.0e-3, 9.0e-3, 18.0e-3])) # 位移
# 视觉模型
visual = body.addChild("VisualFinger")
# 通过旋转平移将视觉模型与网格模型对齐
visual.addObject("MeshSTLLoader", name="visualLoader", filename="Data/finger.stl", rotation=[90.0, 0.0, 0.0],
translation=[-30.0e-3, 9.0e-3, 18.0e-3])
# 添加渲染器
visual.addObject("OglModel", name="renderer", src="@visualLoader", color=[1.0, 1.0, 1.0, 0.5])
# 质心映射，
# 将网格模型文件与视觉模型文件绑定
visual.addObject("BarycentricMapping", input=e.dofs.getLinkPath(), output=visual.renderer.getLinkPath())
return e
# 定义碰撞
def addCollision(self):
# Add a collision model
# 这里的第一个参数是父节点，这里写的是定义类时的软体部件作为父节点
# 注意旋转与位移要与前面定义的手指一致，防止模型错位。
CollisionMesh(self.elasticMaterial, name='SelfCollisionMesh1',
surfaceMeshFileName="Data/finger_surface_contact_in1.stl",
rotation=[90.0, 0.0, 0.0], translation=[-30.0e-3, 9.0e-3, 18.0e-3])
CollisionMesh(self.elasticMaterial, name='SelfCollisionMesh2',
surfaceMeshFileName="Data/finger_surface_contact_in2.stl",
rotation=[90.0, 0.0, 0.0], translation=[-30.0e-3, 9.0e-3, 18.0e-3])
CollisionMesh(self.elasticMaterial, name='SelfCollisionMesh3',
surfaceMeshFileName="Data/finger_surface_contact_out.stl",
rotation=[90.0, 0.0, 0.0], translation=[-30.0e-3, 9.0e-3, 18.0e-3])这部分对应的的树形图结构为：
构建被抓取的圆柱体
文件为cylinder.py，应该也是可以从sofa里直接引用的，源文件就不放了，只贴用法吧：
cylObst = Cylinder(parent=scene.Modelling.Obstacle, translation=[30.0e-3, 0.0, 50.0e-3],
surfaceMeshFileName='Data/cylinder.stl',
MOscale=10e-3,
uniformScale=0.5,
totalMass=0.032,
isAStaticObject=True)
cylObst.mass.showAxisSizeFactor = 1e-3
cylObst.mstate.name = 'dofs'2.3.2 刚软连接与闭环的实现
这一步写的代码仍然是被其他函数调用的类，因此里面仍然有些参数是未指定的，仅留了接口。
这里的代码注释写的更加粗糙了，因为有的是直接调用不用写，有的看不懂-.-
import Sofa
from elastic_material_object import ElasticMaterialObject
from stlib3.physics.mixedmaterial import Rigidify
from stlib3.physics.collision import CollisionMesh
from fixing_box import FixingBox
from actuated_arm import ActuatedArm
from stlib3.components import addOrientedBoxRoi
from splib3.constants import Key
import math
"""本文件包含两个主要部分，一是将电机、摆动臂和软体部件装配起来，作为一个类；二是定义了手指的控制器，作为一个类"""
class ActuatedFinger(Sofa.Prefab):
# 预设参数有俩：旋转和位移
prefabParameters = [
{"name": "rotation", "type": "Vec3d", "help": "Rotation in base frame", "default": [0.0, 0.0, 0.0]},
{"name": "translation", "type": "Vec3d", "help": "Translation in base frame", "default": [0.0, 0.0, 0.0]}
]
# 初始化继承的父类
def __init__(self, *args, **kwargs):
Sofa.Prefab.__init__(self, *args, **kwargs)
# 构造手指
def init(self):
# 加载手指网格并从中创建弹性体
self.elasticMaterial = self.elasticBody()
self.ElasticBody.init()
# 加载伺服电机
arm = self.addChild(ActuatedArm(name="ActuatedArm", rotation=[90.0, 0, 90.0], translation=[0, 0, 0]))
arm.ServoMotor.Articulation.dofs.position.value = [[arm.angleIn.value]] # 初始化旋转角度
arm.ServoMotor.minAngle.value = -2.02
arm.ServoMotor.maxAngle.value = -0.025
# 添加定向框，定义感兴趣的区域，以使夹在伺服臂中的手指网格的节点刚性化
# 该部分被定义在ActuatedFinger节点下，既不属于摆动臂也不属于软体，看看后面怎么用吧
box = addOrientedBoxRoi(self,
name="boxROIclamped",
position=[list(i) for i in self.elasticMaterial.dofs.rest_position.value], # 这一行没看懂
translation=[-25e-3, 0.0, 0.0],
eulerRotation=[0.0, 0.0, 90.0],
scale=[45e-3, 15e-3, 30e-3]) # 这一行可以理解为是在指定box的长宽高。
box.drawBoxes = True
box.init()
# 获取ROI中手指网格的索引和ROI框架。
# 结合这一行代码，上面这个box的作用可能是在初始状态时框选出手指与摆动臂连接处的网格
# 在仿真中测试，果然只有初始状态下被这个框框住的柔性部分才会在运动过程中不发生形变
indices = [[ind for ind in box.indices.value]]
frame = [[0, 0, 0, 0, 0, 0, 1]] # 这句意义不明，4-6参数改变后会改变网格大小形状，可能是固定写法，用来作为indices的mask
# 固定ROI中的手指节点。创建一个刚化对象并设置一个弹簧力场，以约束节点保持静止形状
# 第一个参数应该是指定父节点，第二个参数应该是指定作用对象，第三个是要进行刚性化的区域的索引，第四个参数不清楚，当成固定写法吧
# 框选的判定条件是网格是否完全处于框内，只框到一部分的不算
rigidifiedStruct = Rigidify(self, self.elasticMaterial, groupIndices=indices, frames=frame,
name="RigidifiedStructure")
# 使上面这个约束区域也作用于摆动臂，以实现摆动臂于软体材料的绑定。
servoArm = arm.ServoMotor.Articulation.ServoWheel.ServoArm
servoArm.addChild(rigidifiedStruct.RigidParts)
# 这一句的意思应该是约束区域的作用对象是摆动臂的dofs
servoArm.RigidParts.addObject('RigidRigidMapping', index=0, input=servoArm.dofs.getLinkPath())
# 为软体手指的另一端添加固定框，也就是说，这个末端并不是固定到电机上的，而是通过固定框固定在全局坐标系中的。
FixingBox(self, self.elasticMaterial, translation=[10.0e-3, 0.0, 14.0e-3], scale=[15e-3, 25e-3, 6e-3])
self.FixingBox.BoxROI.drawBoxes = True
# Add collision models, to compute collision between the finger and the object,
# and the inner surfaces of the left and right walls
self.addCollision()
def elasticBody(self):
# Create a body as a child of the parent (the actuated finger)
body = self.addChild("ElasticBody")
e = body.addChild(ElasticMaterialObject(
volumeMeshFileName="Data/finger.msh",
topoMesh="tetrahedron",
scale=[1, 1, 1],
totalMass=0.015,
youngModulus=32e6,
poissonRatio=0.45,
rotation=[90.0, 0.0, 0.0],
translation=[-30.0e-3, 9.0e-3, 18.0e-3]))
visual = body.addChild("VisualFinger")
visual.addObject("MeshSTLLoader", name="visualLoader", filename="Data/finger.stl", rotation=[90.0, 0.0, 0.0],
translation=[-30.0e-3, 9.0e-3, 18.0e-3])
visual.addObject("OglModel", name="renderer", src="@visualLoader", color=[1.0, 1.0, 1.0, 0.5])
visual.addObject("BarycentricMapping", input=e.dofs.getLinkPath(), output=visual.renderer.getLinkPath())
return e
def addCollision(self):
CollisionMesh(self.elasticMaterial, name='SelfCollisionMesh1',
surfaceMeshFileName="Data/finger_surface_contact_in1.stl",
rotation=[90.0, 0.0, 0.0], translation=[-30.0e-3, 9.0e-3, 18.0e-3])
CollisionMesh(self.elasticMaterial, name='SelfCollisionMesh2',
surfaceMeshFileName="Data/finger_surface_contact_in2.stl",
rotation=[90.0, 0.0, 0.0], translation=[-30.0e-3, 9.0e-3, 18.0e-3])
CollisionMesh(self.elasticMaterial, name='SelfCollisionMesh3',
surfaceMeshFileName="Data/finger_surface_contact_out.stl",
rotation=[90.0, 0.0, 0.0], translation=[-30.0e-3, 9.0e-3, 18.0e-3])
class FingerController(Sofa.Core.Controller):
def __init__(self, *args, **kwargs):
# 初始化继承的父类
Sofa.Core.Controller.__init__(self, *args, **kwargs)
# 读取输入的参数
self.node = kwargs["node"]
self.duration = 3.0
self.time = 0.0
self.objectDof = kwargs["objectDof"]
self.actuator = kwargs["actuator"]
self.forceContact = 0.0 # Norm of the contact force
self.numContact = 0 # Number of contact points:
# 施加在物体上的接触力的计算
self.node.getRoot().GenericConstraintSolver.computeConstraintForces.value = True
def onKeypressedEvent(self, event):
key = event['key']
if key == Key.P:
print("Number of contact points: " + str(self.numContact))
print("Norm of the contact force: " + str(self.forceContact))
def onAnimateBeginEvent(self, eventType):
# 伺服电机角位移的更新
# 在自持续时间内旋转π/6（初始值为5s）
angularStep = math.pi / 6
angleInit = 0.1
self.time += self.node.dt.value
if self.time < self.duration:
self.actuator.ServoMotor.angleIn = angleInit + angularStep * self.time / self.duration
else:
self.actuator.ServoMotor.angleIn = angleInit + angularStep
# 施加在物体上的接触力的计算
contactForces = self.node.getRoot().GenericConstraintSolver.constraintForces.value
# 打印接触的节点数和最大接触力的范数，在软件里并不能显示
self.numContact = 0
self.forceContact = 0
for contact in contactForces[0:-1:3]:
if contact > 0:
self.numContact += 1
self.forceContact += contact
self.forceContact /= self.node.dt.value
# 创建场景，可以参考一下前面定义的东西咋用，可不看。
def createScene(rootNode):
from stlib3.scene import Scene
# Define the main architecture of the scene, with a node Modelling, Setting and Simulation
# Define also the integration method as Euler implicit and the solver as Conjugate Gradient)
scene = Scene(rootNode, gravity=[0.0, 0.0, -9.81],
plugins=['SofaSparseSolver', 'SofaOpenglVisual', 'SofaSimpleFem', 'SofaDeformable', 'SofaEngine',
'SofaGeneralRigid', 'SofaRigid', 'SofaBoundaryCondition', 'SofaMeshCollision'],
iterative=False)
scene.addMainHeader()
# Setting the time step
rootNode.dt = 0.01
# Define the default view of the scene on SOFA
scene.addObject('DefaultVisualManagerLoop')
scene.VisualStyle.displayFlags = ["showInteractionForceFields", "showForceFields",
"showCollisionModels"]
# Add a grid on the scene with squares 10mm/10mm
rootNode.addObject("OglGrid", nbSubdiv=100, size=1)
# Set up the pipeline for the collision computation
scene.addObject('FreeMotionAnimationLoop')
scene.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5)
scene.Simulation.addObject('GenericConstraintCorrection')
scene.Settings.mouseButton.stiffness = 10
# Create one actuated finger
actuatedFinger = ActuatedFinger()
scene.Modelling.addChild(actuatedFinger)
# Add the simulated elements to the Simulation node
scene.Simulation.addChild(actuatedFinger.RigidifiedStructure.DeformableParts)
scene.Simulation.addChild(actuatedFinger.ActuatedArm)
actuatedFinger.ActuatedArm.ServoMotor.Articulation.ServoWheel.ServoArm.dofs.showObject = True
actuatedFinger.ActuatedArm.ServoMotor.Articulation.ServoWheel.ServoArm.dofs.showObjectScale = 0.01
actuatedFinger.ActuatedArm.ServoMotor.Articulation.ServoWheel.ServoArm.RigidParts.dofs.showObject = True
actuatedFinger.ActuatedArm.ServoMotor.Articulation.ServoWheel.ServoArm.RigidParts.dofs.showObjectScale = 0.02
# Temporary addition to have the system correctly built in SOFA
# Will no longer be required in SOFA v22.12
scene.Simulation.addObject('MechanicalMatrixMapper',
name="deformableAndFreeCenterCoupling",
template='Vec1,Vec3',
object1=actuatedFinger.ActuatedArm.ServoMotor.Articulation.dofs.getLinkPath(),
object2=actuatedFinger.RigidifiedStructure.DeformableParts.dofs.getLinkPath(),
nodeToParse=actuatedFinger.RigidifiedStructure.DeformableParts.ElasticMaterialObject.getLinkPath())
2.3.3 完整场景构建
没啥好看的，都是调用前面创建的模块。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
