Sofa framework 学习笔记（三）

最新推荐文章于 2024-09-12 09:07:40 发布

林三程

最新推荐文章于 2024-09-12 09:07:40 发布

阅读量855

点赞数

分类专栏：软体仿真文章标签：学习

本文链接：https://blog.csdn.net/aniclever/article/details/129468497

版权

软体仿真专栏收录该内容

5 篇文章

订阅专栏

本篇想要探究的是在SOFA中使用气动驱动软体结构，添加ROS环境并使用游戏手柄控制，以及怎样设置刚性结构之间的被动旋转关节。

其中，气动驱动模块可以直接参照实例PneunetGripper，只分析其中有用的代码。

刚性结构结构之间的被动旋转关节暂时没有教程可以参考，愁人。

一、气动手指

在开始前，先介绍以下需要用到的模型：

pneunetCavityCut.stl，手指内部空腔的网格，面网格，用于被气动驱动变形

pneunetCut.vtk、pneunetCutCoarse.vtk，手指的网格和粗切网格，体积网格，是仿真主体

pneunetCut.stl，用于作为视觉模型和碰撞模型，面网格

1.1 导入模型网格：


import Sofa

def createScene(rootNode):
    rootNode.addObject('RequiredPlugin', pluginName='SoftRobots SofaPython3 SofaLoader')
    finger = rootNode.addChild('finger')
    finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk')
    finger.addObject('MeshTopology', src='@loader', name='container')
    finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1)

step1：将vtk文件通过MeshVTKLoader加载到场景中，名称为loader；

step2：将网格拓扑模块的src源文件链接到刚才加载的loader，命名为container；

step3：创建一个MechanicalObject组件来存储机器人的自由度(这是网格中所有节点的位置)。

1.2 定义要仿真的材料

接下来，我们需要定义将要模拟的材料类型，这是通过添加ForceField组件来完成的，该组件描述了当物体变形时产生的内力。

特别是，这将定义材料的柔软或坚硬程度，如果它具有弹性或更复杂的行为(超弹性，塑料等)。

在这个例子中，我们使用TetrehedronFEMForceField组件（四面体有限元力场），它对应于弹性材料变形，但具有较大的旋转。可以在该组件中设置杨氏模量和泊松比等属性。

另外，加上下面这句话之后，显示出来的点会变成四面体有限元力场


finger.addObject('TetrahedronFEMForceField', template='Vec3d', name='FEM', 
                method='large', poissonRatio=0.3,  youngModulus=500)

前后对比如下：

材质的vertexMass（顶点质量）可以用UniformMass（统一的质量）组件定义，该组件假定vertexMass在主体内部是均匀分布的


finger.addObject('UniformMass', totalMass=0.0008)

注意，默认情况下，重力被定义为[0 -9.81 0]。您可以在rootNode中使用以下命令重新定义它


rootNode.findData('gravity').value=[-9810, 0, 0];

为了定义刚性层,我们将创建一个新节点，并定义一个新的力场，仅在该刚性层的点上具有更刚性的参数。（也就是将框出来的部分定义为与前文不同的材料，这样做可以防止指腹部分也做相同的膨胀，从而影响手指弯曲效果。）

为了方便地定义将要选择的点的索引，我们使用boxROI组件，该组件允许定义一个包含该层所有点的框（由节点“modelSubTopo”引用）。方框组件依次包含沿x、y和z的极端坐标


finger.addObject('BoxROI', name='boxROISubTopo', 
                box=[-100, 22.5, -8, -19, 28, 8])
modelSubTopo = finger.addChild('modelSubTopo')
modelSubTopo.addObject('Mesh', position='@loader.position', 
                tetrahedra="@boxROISubTopo.tetrahedraInROI", name='container')
modelSubTopo.addObject('TetrahedronFEMForceField', template='Vec3d', 
                name='FEM', method='large', poissonRatio=0.3,  youngModulus=1500)

1.3 手指根部的固定

为了在空间中固定手指，需要在物体的某些部分上定义边界条件。在SOFA中可以通过几种方式实现这一点。在本例中，我们使用组件RestShapeSpringsForceField，它创建了主体部分的当前位置与其初始位置之间的弹簧。这些弹簧的刚度可以调节。特别地，设置一个非常大的刚度相当于在空间中固定这些点。为了方便地定义将固定的点的索引，我们使用boxROI组件。


finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20])
finger.addObject('RestShapeSpringsForceField', points='@boxROI.indices', 
                stiffness=1e12, angularStiffness=1e12)

1.4 隐式时间积分与矩阵求解

现在已经描述了场景的所有最小元素，现在已经可以模拟手指的变形了。为此，我们需要定义一个时间积分方案，该方案将定义在仿真的每个时间步骤中要求解的系统，以及一个矩阵求解器，以求解该系统并计算机器人节点的更新速度和位置。在这种情况下，我们选择使用隐式欧拉方案和基于LDL矩阵分解的稀疏直接求解器


finger.addObject('EulerImplicit', name='odesolver')
finger.addObject('SparseLDLSolver', name='directSolver')

在这种状态下，场景在模拟中不会发生太多事情，仅仅是由于手指上的重力造成的小变形。事实上，我们还没有包括气动执行器。

1.5 气动执行器和Python脚本控制器

在本节中，我们将介绍一种气动执行器，它可以交互式地模拟手指腔的膨胀。为此，我们首先创建一个负责此任务的节点。然后，腔体的表面网格必须使用网格加载器加载，位置存储在网格组件中，这是一个容器。创建一个MechanicalObject来存储在模拟过程中会变形的腔体的自由度。执行器是使用SurfacePressureConstraint组件创建的，该组件直接与网格容器相关联


cavity = finger.addChild('cavity')
cavity.addObject('MeshSTLLoader', name='loader', filename='data/mesh/pneunetCavityCut.stl')
cavity.addObject('Mesh', src='@loader', name='cavityMesh')
cavity.addObject('MechanicalObject', name='cavity')
cavity.addObject('SurfacePressureConstraint', name="SurfacePressureConstraint", template='Vec3d', value=0.0001, triangles='@topo.triangles', valueType="pressure")
cavity.addObject('BarycentricMapping', name='mapping')

为了交互式地膨胀空腔，我们使用了PythonScriptController，这是一个引用python文件的组件，在初始化或模拟期间执行一些操作.

这里在添加气动空腔的时候，应该也是可以顺便添加 translation和rotation的属性的，以对其网格。

如果觉得手动对齐网格比较麻烦，可以在制作模型文件时就定好位置（比如在solidworks里，可以先装配，然后在装配场景中分别保存零件，这样保存的零件之间的相对位置就是对的了）


rootNode.addObject('PythonScriptController', filename="fingerController.py, classname="controller")

在这种情况下，控制器允许通过按ctrl + 交互地使空腔膨胀，并通过按ctrl -进行放气。

下面是fingerController.py的内容：


#!/usr/bin/env python
# -*- coding: utf-8 -*-
import Sofa.Core
import Sofa.constants.Key as Key


class FingerController(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, args, kwargs)
        self.node = args[0]
        self.fingerNode = self.node.getChild('finger')
        self.pressureConstraint = self.fingerNode.cavity.getObject('SurfacePressureConstraint')

    def onKeypressedEvent(self, e):
        pressureValue = self.pressureConstraint.value.value[0]

        if e["key"] == Key.plus:
            pressureValue += 0.01
            if pressureValue > 1.5:
                pressureValue = 1.5

        if e["key"] == Key.minus:
            pressureValue -= 0.01
            if pressureValue < 0:
                pressureValue = 0

        self.pressureConstraint.value = [pressureValue]

1.6 完整代码


import Sofa
from fingerController import FingerController


def createScene(rootNode):
    rootNode.addObject('VisualStyle', displayFlags='showForceFields showBehaviorModels')
    rootNode.addObject('RequiredPlugin',
                       pluginName='SoftRobots SofaPython3 SofaLoader SofaSimpleFem SofaEngine SofaDeformable SofaImplicitOdeSolver SofaConstraint SofaSparseSolver')
    rootNode.gravity.value = [-9810, 0, 0]
    rootNode.addObject('FreeMotionAnimationLoop')
    rootNode.addObject('GenericConstraintSolver', tolerance=1e-12, maxIterations=10000)

    finger = rootNode.addChild('finger')
    finger.addObject('EulerImplicitSolver', name='odesolver', rayleighStiffness=0.1, rayleighMass=0.1)
    finger.addObject('SparseLDLSolver', name='directSolver')
    finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/pneunetCutCoarse.vtk')
    finger.addObject('MeshTopology', src='@loader', name='container')
    finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1)
    finger.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3,
                     youngModulus=500)
    finger.addObject('UniformMass', totalMass=0.0008)
    finger.addObject('BoxROI', name='boxROISubTopo', box=[-100, 22.5, -8, -19, 28, 8], strict=False)
    finger.addObject('BoxROI', name='boxROI', box=[-10, 0, -20, 0, 30, 20], drawBoxes=True)
    finger.addObject('RestShapeSpringsForceField', points='@boxROI.indices', stiffness=1e12, angularStiffness=1e12)
    finger.addObject('LinearSolverConstraintCorrection', solverName='directSolver')

    modelSubTopo = finger.addChild('modelSubTopo')
    modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra='@boxROISubTopo.tetrahedraInROI',
                           name='container')
    modelSubTopo.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3,
                           youngModulus=1500)

    cavity = finger.addChild('cavity')
    cavity.addObject('MeshSTLLoader', name='cavityLoader', filename='data/mesh/pneunetCavityCut.stl')
    cavity.addObject('MeshTopology', src='@cavityLoader', name='cavityMesh')
    cavity.addObject('MechanicalObject', name='cavity')
    cavity.addObject('SurfacePressureConstraint', name='SurfacePressureConstraint', template='Vec3', value=1,
                     triangles='@cavityMesh.triangles', valueType='pressure')
    cavity.addObject('BarycentricMapping', name='mapping', mapForces=False, mapMasses=False)

    rootNode.addObject(FingerController(rootNode))

1.7 实操

参照这个实例做了一个双气腔的软体手指，因为只是想实现双气腔驱动，所以没有编写手指表面碰撞、抓取物体等后续步骤。

代码大部分照搬了，改了模型和多气腔，对照实例的代码可以加强理解。

main.py:


import Sofa
from fingerController import FingerController


def createScene(rootNode):
    rootNode.addObject('VisualStyle', displayFlags='showForceFields showInteractions showBehaviorModels')
    rootNode.addObject('RequiredPlugin',
                       pluginName='SoftRobots SofaPython3 SofaLoader SofaSimpleFem SofaEngine SofaDeformable '
                                  'SofaImplicitOdeSolver SofaConstraint SofaSparseSolver')
    rootNode.gravity.value = [-9810, 0, 0]
    rootNode.addObject('FreeMotionAnimationLoop')
    rootNode.addObject('GenericConstraintSolver', tolerance=1e-12, maxIterations=10000)

    finger = rootNode.addChild('finger')
    finger.addObject('EulerImplicitSolver', name='odesolver', rayleighStiffness=0.1, rayleighMass=0.1)
    finger.addObject('SparseLDLSolver', name='directSolver')
    finger.addObject('MeshVTKLoader', name='loader', filename='data/mesh/out2.vtk')
    finger.addObject('MeshTopology', src='@loader', name='container')
    finger.addObject('MechanicalObject', name='tetras', template='Vec3', showObject=True, showObjectScale=1)
    finger.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3,
                     youngModulus=500)
    finger.addObject('UniformMass', totalMass=0.0008)
    finger.addObject('BoxROI', name='boxROISubTopo', box=[-2, -7, 0, 2, 7, 120], strict=False, drawBoxes=True)
    finger.addObject('BoxROI', name='boxROI', box=[-15, -12, -5, 15, 12, 5], drawBoxes=True)
    finger.addObject('RestShapeSpringsForceField', points='@boxROI.indices', stiffness=1e12, angularStiffness=1e12)
    finger.addObject('LinearSolverConstraintCorrection', solverName='directSolver')

    modelSubTopo = finger.addChild('modelSubTopo')
    modelSubTopo.addObject('MeshTopology', position='@loader.position', tetrahedra='@boxROISubTopo.tetrahedraInROI',
                           name='container')
    modelSubTopo.addObject('TetrahedronFEMForceField', template='Vec3', name='FEM', method='large', poissonRatio=0.3,
                           youngModulus=1500)

    cavity1 = finger.addChild('cavity1')
    cavity1.addObject('MeshSTLLoader', name='cavityLoader', filename='data/mesh/in1.stl', translation=[-3, 0, 0])
    cavity1.addObject('MeshTopology', src='@cavityLoader', name='cavityMesh')
    cavity1.addObject('MechanicalObject', name='cavity')
    cavity1.addObject('SurfacePressureConstraint', name='SurfacePressureConstraint', template='Vec3', value=0.0001,
                      triangles='@cavityMesh.triangles', valueType='pressure')
    cavity1.addObject('BarycentricMapping', name='mapping', mapForces=False, mapMasses=False)

    cavity2 = finger.addChild('cavity2')
    cavity2.addObject('MeshSTLLoader', name='cavityLoader', filename='data/mesh/in2.stl', translation=[3, 0, 0])
    cavity2.addObject('MeshTopology', src='@cavityLoader', name='cavityMesh')
    cavity2.addObject('MechanicalObject', name='cavity')
    cavity2.addObject('SurfacePressureConstraint', name='SurfacePressureConstraint', template='Vec3', value=0.0001,
                      triangles='@cavityMesh.triangles', valueType='pressure')
    cavity2.addObject('BarycentricMapping', name='mapping', mapForces=False, mapMasses=False)
    rootNode.addObject(FingerController(node=rootNode))

fingerController.py


#!/usr/bin/env python
# -*- coding: utf-8 -*-
import Sofa.Core
import Sofa.constants.Key as Key


class FingerController(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.node = kwargs['node']
        self.fingerNode = self.node.getChild('finger')
        self.pressureConstraint = self.fingerNode.cavity1.SurfacePressureConstraint

    def onKeypressedEvent(self, e):
        pressureValue = self.pressureConstraint.value.value[0]

        if e["key"] == Key.KP_0:
            pressureValue += 0.01
            if pressureValue > 1:
                pressureValue = 1

        if e["key"] == Key.KP_1:
            pressureValue -= 0.01
            if pressureValue < 0:
                pressureValue = 0

        self.pressureConstraint.value = [pressureValue]

二、在ROS中添加手柄控制SOFA

可供参考的实例的路径：Sofa_python3.8/plugins/SoftRobots/docs/sofapython3/examples/sofaros

该实例中使用的是ROS2.0，我想试试改写成ROS。

2.1 手柄的使用

参考博客ROS配置和使用北通手柄

我用的北通手柄，若使用xbox手柄应该也是通用的。

按照博客配置好后，在终端中执行：

roscore

然后新开一个终端，执行：

rosrun joy joy_node

若想查看此时手柄发布的话题，可以再开一个终端，执行：

rostopic echo joy

可以看到以下窗口：

t2.2 在仿真中订阅手柄话题

手柄能够发布话题后，接下来只要将仿真的程序运行在ROS环境中，并且在手指的控制器中订阅手柄话题即可。注意在创建功能包时，使用手柄需要添加 sensor_msgs 依赖库，如果忘了添加，要到cmakelist.txt里添加。

2.2.1 创建ROS环境

如果没建过ROS工作空间要先创建工作空间，简单说一下创建过程，遇到bug百度解决。

创建工作空间的路径：

$ mkdir -p ~/catkin_ws/src    # 创路径，catkin_ws可自定名称，但src一定要有
$ cd ~/catkin_ws/src    # 进入路径
$ catkin_ini_workspace    # 工作空间初始化，把该文件夹定义为工作空间的属性

编译工作空间：

$ cd ~/catkin_ws/    # 换到根路径
$ catkin_make     # 对该目录进行编译，编译之后要设置环境变量
$ catkin_make install    # 编译出install文件夹

设置环境变量：

在终端中使用source命令设置的环境变量只能在当前终端生效，若要在所有终端生效，则需要在终端配置文件中加入环境变量的设置：

echo "source /WORKSPACE/devel/setup.bash">>~/.bashrc

$ source devel/setup.bash

检查环境变量：

$ echo $ROS_PACKAGE_PATH    #

如果打印的路径中已经包含当前工作空间的路径，则说明环境变量设置成功

然后，创建好工作空间后，在工作空间中创建功能包：

$ catkin_create_pkg <pkg_name> [depend1] [depend2] [depend3]

e.g. (同一个工作空间下，不允许存在同名功能包，不同工作空间下，允许存在)

功能包创建好后还要到工作空间路径下进行编译。

示例：

# 创建功能包
$ cd ~/catkin_ws/src    # 功能包在代码空间内创建
$ catkin_create_pkg test_pkg std_msgs sensor_msgs rospy roscpp 
#功能包必须放到src里，后面是该功能包要用到哪些依赖
# std_msgs是ROS定义的标准的数据结构

# 编译功能包
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash

2.2.2 创建python文件

最基础的demo：


#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import String
from sensor_msgs.msg import Joy
import sys

def callback(data):
    rospy.loginfo("get data")
    print(1)
    print("data.axes[0]")
    print(type(data.axes[0]))
    print("data.axes[1]")
    print(data.axes[1])
    print("data.buttons[0]")
    print(data.buttons[0])
    print("data.buttons[1]")
    print(data.buttons[1])
    print("--------------------------------")

if __name__ == '__main__':
    print("start")
    rospy.init_node("listener")
    print("sub")
    sub = rospy.Subscriber("joy", Joy, callback, queue_size=10)

    rospy.spin()
    rospy.shutdown()

2.3 SOFA中使用手柄的案例

ROS和sofa具体配置见笔记（四）

实现效果：可以用手柄控制小球在一个立方体内移动

（因为我将手柄取值映射到小球位置，若改为映射到速度或加速度等其他映射方法，则会有不同的效果）

（本段代码中，只有当手柄有操作时，才会调用改变小球位置的回调函数。可以改为：小球位置每个控制周期读取一遍手柄位置，这样，当手柄停止在某一位置时，小球可以根据当前值改变位置，而不是不刷新位置）

sofa 主程序：


# coding: utf8
import sofaros_receiver
from std_msgs.msg import Float32MultiArray
from sensor_msgs.msg import Joy

def recv(data, datafield):
    t = list(data)
    datafield.value = [[t[0] + 1.0, t[1], t[2]]]


def createScene(rootNode):
    rootNode.addObject('RequiredPlugin',
                       pluginName=["Sofa.Component.ODESolver.Backward", "Sofa.Component.SceneUtility"])
    rootNode.addObject("EulerImplicitSolver")
    rootNode.addObject("CGLinearSolver", iterations=25, threshold=1e-5, tolerance=1e-5)
    rootNode.addObject('DefaultAnimationLoop')
    rootNode.addObject('DefaultVisualManagerLoop')

    a = rootNode.addChild("animated")
    a.addObject("MechanicalObject", name="receiver", position=[0.0, 0.0, 0.0])
    a.receiver.showObject = True
    a.receiver.showObjectScale = 1.0
    a.receiver.drawMode = 1
    a.receiver.showColor = [0, 1, 0, 1]

    # 下面的内容涉及到ROS的使用了
    # sofaros_sender.init("SenderNode")
    sofaros_receiver.init("ReceiverNode")
    rootNode.addObject(sofaros_receiver.RosReceiver("joy", a.receiver.findData("position"), Joy, recv))
    // joy是手柄的话题名称，Joy是手柄话题的数据类型
    return rootNode

sofa通讯子程序：


# coding: utf8
import Sofa.Core
import rospy
from std_msgs.msg import Float32MultiArray
from sensor_msgs.msg import Joy


class RosReceiver(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.name = "RosReceiver"

        rosname = args[0]
        self.datafield = args[1]
        msgtype = args[2]
        self.recvcb = args[3]

        # Create or connect to the topic rosname as a subscription
        self.sub = rospy.Subscriber(rosname, msgtype, self.callback, queue_size=10)
        self.data = None
        self.pos = [0, 0, 0]

    def callback(self, data):
        self.pos = [data.axes[0], data.axes[1], data.axes[3]]
        print("pos:----------------")
        print(self.pos)
        self.data = self.pos

    def onAnimateBeginEvent(self, event):
        if self.data is not None:
            self.recvcb(self.data, self.datafield)
            self.data = None


def init(nodeName="Sofa"):
    rospy.init_node(nodeName)
    rospy.loginfo('Created Receiver node')