[SOLVED] Cable-Driven Actuation

[Sara]

Hi SOFA forum,

I have been trying to implement a cable-driven actuation mechanism to my soft gripper. I followed roughly the same process as the CableGripper tutorial available for the SoftRobots plugin. I have debugged my code and every thing seems to be running okay (no obvious errors or warnings). However, when I try to interactively move my gripper down to grasp a body, the only thing that moves is my “pullPoint”, which is acting as the common origin to all my gripper’s fingers. I checked everything again but can’t seem to find where the problem might be.

Just a note:
– I have a U-shaped ‘channel’ running through the inner-face of my gripper finger.
This is where I will install pins around which the cable can slide, and so right
now, the cable is not actually penetrating any material in the model (Could
this be the problem?)

I am not sure how much of my code to post so I’ll organise it here below.

The file containing the bodies included in the scene as well as the controller:

# -*- coding: utf-8 -*-
# Script by: Sara Hijazi in fulfillment of the requirements of MSc Robotics @UoB, 2020
# Code extracts and Templates by SOFA used below are fully and correctly  referenced in the text accompanying these files

# Importing libraries
import Sofa
from stlib.physics.rigid import Floor
from stlib.scene import Node, get
# Utilising SOFA templates to model cable
from softrobots.actuators import PullingCable
from splib.loaders import loadPointListFromFile

# Some global variables
#trans_finger=[0, 30, 0]
#trans_hemi=[0, 0, 0]
#rot_finger=[-90, 0, 0]
#rot_hemi=[0, 0, 0]
scale3d_finger=[0.125, 0.125, 0.125]
scale3d_hemi=[1.0, 1.0, 1.0]

# A python class created by SOFA is inherited from, redefining interaction keys to implement a 'controller' script--> "PythonScriptController.h"
# SOFA Documentation is further used, supplying info on available functionality and options
class FingerController(Sofa.PythonScriptController):
    def __init__(self, node, cable):
        inputvalue =cable.getObject("CableConstraint").findData('value')
        self.name = "FingerController"

    # Inheriting onKeyPressed method to redefine interactive keys and create new behaviour
    # Function recovers user interaction via keyboard as input for body manipulation in scene
    def onKeyPressed(self,c):
        if (c == "+"):
            inputvalue.value =  inputvalue.value[0][0] + 1.

        elif (c == "-"):
            displacement = inputvalue.value[0][0] - 1.
            if(displacement < 0):
                displacement = 0
            inputvalue.value = displacement

# Defining a function for gripper finger
# Add fixing box
# Add in routing points to function attributes to implement tendon drive
def Finger(parentNode=None, name="Finger", rotation=[0.0, 0.0, 0.0],
    translation=[0.0, 0.0, 0.0], routingPoints=[0.0, 0.0, 0.0]):
    
    # Attach child to root node
    finger = parentNode.createChild(name)
    
    # Meshing and addition of mechanical model
    # Mesh is loaded using a MeshLoader GMSH component
    finger.createObject('MeshGmshLoader', name="mesh_loader_finger", filename='/Users/Sarah/Desktop/Thesis/Assemblies/Recents/finger_update_1_7_vol')
    # Mesh is then stored in a MeshContainer component linked to the loader through its source
    finger.createObject('Mesh', src='@mesh_loader_finger', name="finger_mesh_container")
    # MechanicalObject component created to store the DOFs and node positions of mesh
    finger.createObject('MechanicalObject', name='finger_tetras_obj', template='Vec3d',
        showObject='true', showObjectScale='1', translation=translation,
        rotation=rotation, scale3d=scale3d_finger)
    
    ## Implementing constitutive law of material and mass
    # Define material to be simulated by adding a ForceField component
        # This describes what internal forces are created when the object is deformed
        # Additionally, this will define how stiff or soft the material is as well as its behaviour
    finger.createObject('TetrahedronFEMForceField', template='Vec3d', name='FEM_finger',
        method='large', poissonRatio='0.48', youngModulus='20000000')
    # The vertexMass is defined with the uniformMass component which assumes a uniform and homogeneous distribution of node mass in the body
    finger.createObject('UniformMass', totalMass='0.03254')
    
    # Since dimensions are all in mm, rdefine gravity action on the scene in mm
    parentNode.findData('gravity').value = '0, -9810, 0';
    
    # Creating a tendon/cable object that will attach to the finger via the moveFinger function

    # This create a BarycentricMapping. A BarycentricMapping is a key element as it will create a bi-directional link
    # between the cable's DoFs and the finger's ones so that movements of the cable's DoFs will be mapped
    # to the finger and vice-versa;

    cable = PullingCable(finger,
            "PullingCable",
            pullPointLocation=routingPoints,
            rotation=rotation,
            translation=translation,
            cableGeometry=loadPointListFromFile("/Users/Sarah/Desktop/SOFA/SOFA_v19.06.99_custom_MacOS_v11/plugins/SoftRobots/my_projects/Testing/cableRouting.json"));

    cable.getObject("CableConstraint").findData('minForce').value = '0'
    
    # Instantiate function
    FingerController(finger, cable)
    
    return finger
        
        
# Defining a function for gripper finger
def Tissue(parentNode=None, name="breastTissue", rotation=[0.0, 0.0, 0.0],
    translation=[0.0, 0.0, 0.0], fixingBox=[0.0, 0.0, 0.0]):
    
    # Attach child to root node
    tissue = Node(parentNode, "breastTissue")
    
    # Meshing and addition of mechanical model
    # Mesh is loaded using a MeshLoader GMSH component
    tissue.createObject('MeshGmshLoader', name="mesh_loader_tissue", filename='/Users/Sarah/Desktop/Thesis/Assemblies/Recents/hemi_vol')
    # Mesh is then stored in a MeshContainer component linked to the loader through its source
    tissue.createObject('Mesh', src='@mesh_loader_tissue', name="tissue_mesh_container")
    # MechanicalObject component created to store the DOFs and node positions of mesh
    tissue.createObject('MechanicalObject', name='tissue_tetras_obj', template='Vec3d',
        showObject='true', showObjectScale='1', translation=translation,
        rotation=rotation, scale=scale3d_hemi)
    
    ## Implementing constitutive law of material and mass
    # Define material to be simulated by adding a ForceField component
        # This describes what internal forces are created when the object is deformed
        # Additionally, this will define how stiff or soft the material is as well as its behaviour
    tissue.createObject('TetrahedronFEMForceField', template='Vec3d', name='FEM_tissue',
        method='large', poissonRatio='0.495', youngModulus='27500')
    # The vertexMass is defined with the uniformMass component which assumes a uniform and homogeneous distribution of node mass in the body
    tissue.createObject('UniformMass', totalMass='0.4')
    
    # Since dimensions are all in mm, rdefine gravity action on the scene in mm
    parentNode.findData('gravity').value = '0, -9810, 0';
         
    return tissue

The file containing the gripper controller:

# -*- coding: utf-8 -*-
import Sofa

def getTranslated(points, vec):
    r=[]
    for v in points:
        r.append( [v[0]+vec[0], v[1]+vec[1], v[2]+vec[2]] )
    return r

class GripperController(Sofa.PythonScriptController):
    def __init__(self, node, fingers):
        self.fingers = fingers
        self.name = "FingerController"

    def onKeyPressed(self,c):
        dir = None
        # UP key :
        if ord(c)==19:
            dir = [0.0,1.0,0.0]
        # DOWN key : rear
        elif ord(c)==21:
            dir = [0.0,-1.0,0.0]
        # LEFT key : left
        elif ord(c)==18:
            dir = [1.0,0.0,0.0]
        elif ord(c)==20:
            dir = [-1.0,0.0,0.0]

        if dir != None:
            for finger in self.fingers:
                mechanicalObject = finger.getObject("finger_tetras_obj")
                mechanicalObject.findData('rest_position').value = getTranslated(mechanicalObject.rest_position, dir)

                cable = finger.getChild("PullingCable").getObject("CableConstraint")
                p = cable.pullPoint[0]
                cable.findData("pullPoint").value = [p[0]+dir[0], p[1]+dir[1], p[2]+dir[2]]

The file containing the assembled gripper and scene initiation:

# -*- coding: utf-8 -*-
# Script by Sara Hijazi in fulfillment of the requirements of MSc Robotics @UoB, 2020

import Sofa
import os
import sys
from stlib.scene import ContactHeader
from stlib.scene import Node, get
#from stlib.visuals import ShowGrid
from stlib.physics.rigid import Floor
from gripperComponents import Finger
from gripperComponents import Tissue
from moveGripper import GripperController

def Gripper(parentNode=None):
    # Create gripper common origin from which fingers attach
    gripper = Node(parentNode, "Gripper")
    
    # Attach fingers; orient, position and fix
    G_f1 = Finger(gripper, "Finger 1",
                    rotation=[-75, 60, 0],
                    translation=[-5.169, 30.0, -3.0],
                    routingPoints=[0, 45, 0])
    
    G_f2 = Finger(gripper, "Finger 2",
                    rotation=[-75, 180, 0],
                    translation=[0.0, 30.0, 6.0],
                    routingPoints=[0, 45, 0])
    
    G_f3 = Finger(gripper, "Finger 3",
                    rotation=[-75, -60, 0],
                    translation=[5.169, 30.0, -3.0],
                    routingPoints=[0, 45, 0])
    
    # Structure and set controller
    GripperController(gripper, fingers=[G_f1, G_f2, G_f3])
    
    return gripper
    
# Creating scene, initialising rootNode
def createScene(rootNode):
    ### Set-up ###
    # Required plugins
    rootNode.createObject('RequiredPlugin', name='soft_robots', pluginName='SoftRobots')
    rootNode.createObject('RequiredPlugin', name='sparse', pluginName='SofaSparseSolver')
    rootNode.createObject('RequiredPlugin', name='python', pluginName='SofaPython')
    # Toggle display options
    rootNode.createObject('VisualStyle', displayFlags='showForceFields showWireframe showBehaviorModels')
    
    # Add in contact header when collision models have been created
    
    # Attach children from root
    Tissue(parentNode=rootNode, name="breastTissue", rotation=[0.0, 0.0, 0.0],
    translation=[0.0, 0.0, 0.0]) # Add fixing box
    
    # Adding prefab floor to root
    floor = Floor(rootNode, name="Floor", translation=[0.0, 0.0, 0.0], uniformScale=0.25, isAStaticObject=True, color=[1.0, 0.3, 0.6])
    
    # Instantiate gripper and fingers
    Gripper(parentNode=rootNode)
    
    return rootNode

I am not sure as to what I could be missing or what has gone wrong so any pointers are helpful!

Thanks in advance,

Sara

[Sara]

Hi SOFA,

I am trying to debug my code to find out why my gripper finger is not moving. I have simplified it down to just one finger with all the basic components necessary to simulate cable actuation. I am not getting any results, I think I am missing something important or doing something wrong. I’ve attached a file below that contains both version of my code (The full gripper and the newer test code with just one gripper finger), as well as the mesh files I am using.

https://1drv.ms/u/s!ApELNE0leU38gjAAcZSebNS-RrPr?e=cbPkWN

If anyone has the time to have a look at the code, it would be very appreciated.

Thanks!

Sara

[elieK]

Hello Sara,

I tried to change a bit in your test scene, I managed to move the finger.
Can you try to replace the code in interactiveFinger.py to:

import Sofa
import math

class interaction(Sofa.PythonScriptController):
    def initGraph(self, node):
        self.node = node
        self.f1Node=self.node.getChild('finger')
        self.cableconstraint1Node=self.f1Node.getChild('PullingCable')
        self.cableconstraintvalue = self.cableconstraint1Node.getObject("CableConstraint").findData('value')

    def onKeyPressed(self,c):
        if (c == "M"):
            self.cableconstraintvalue.value = self.cableconstraintvalue.value[0][0] + 1.
        if (c == "-"):
            self.cableconstraintvalue.value = self.cableconstraintvalue.value[0][0] - 1.

def getTranslated(points, vec):
    r=[]
    for v in points:
        r.append( [v[0]+vec[0], v[1]+vec[1], v[2]+vec[2]] )
    return r

class GripperController(Sofa.PythonScriptController):
    def initGraph(self, node):
        self.node = node
        self.fingers = [self.node.getChild('finger')]
        self.name = "FingerController"

    def onKeyPressed(self,c):
        dir = None
        # UP key :
        if ord(c)==19:
            dir = [0.0,1.0,0.0]
        # DOWN key : rear
        elif ord(c)==21:
            dir = [0.0,-1.0,0.0]
        # LEFT key : left
        elif ord(c)==18:
            dir = [1.0,0.0,0.0]
        elif ord(c)==20:
            dir = [-1.0,0.0,0.0]

        if dir != None:
            for finger in self.fingers:
                mechanicalObject = finger.getObject("finger_tetras")
                mechanicalObject.findData('rest_position').value = getTranslated(mechanicalObject.rest_position, dir)

                cable = finger.getChild("PullingCable").getObject("CableConstraint")
                p = cable.pullPoint[0]
                cable.findData("pullPoint").value = [p[0]+dir[0], p[1]+dir[1], p[2]+dir[2]]

and add the additional controller to your testMesh.py:
rootNode.createObject('PythonScriptController', filename="./interactiveFinger.py", classname="GripperController")

With that I was able to actuate the finger and move it in the plane. Does this solve your issue?
I wasn’t able to test your initial gripping scene because the file hemi_vol is missing.

Best regards,
Elie

[Sara]

Hi Elie,

Thanks very much for your help! You’re right, I have forgotten to include that mesh. I am able to move the finger in the plane now after implementing your code, but it will not close or open (bend inwards or outwards). I have had this similar problem when I tried the CableGripper/PneunetGripper SoftRobots tutorials; is it something to do with my definition of the cable or cable points?

Many thanks,

Sara

[elieK]

Hello Sarah,

If you take the complete code in the previous comment, you should be able to bend the finger with ctr M and ctr – in its initial position.

I saw that there is still an issue where the finger actuation doesn’t work when you move the finger too far from its original position, I think I found the issue: When creating the fixed box you need to specify that you don’t want it to update during the simulation. I also propose that you use a slightly bigger boxROI because a small portion of the finger is left out from below.

    finger.createObject('BoxROI', name='boxROI', box='-20 20 70 20 -2 80', doUpdate=False, drawBoxes='true')

Replacing the line above in your code, I was able to move around the finger and bend it with no problems. Let me know if this works.

Best regards,
Elie

[Sara]

Hi Elie,

Thanks for getting back to me so quickly! I have used your exact code and it has solved my translation issue- the finger moves in the plane, it is just not bending. I am sure the implementation is now correct, it is just not working in my scene. I am using mac OSX- do you think it could be a hotkey issue? Even though I am using [cmd + “-“] or [cmd + “M”], it has no effect on the finger. Sorry to to trouble you with this, any advice you can give me on what I could do would be helpful.

Also, I have added in the fixes to my bounding box and it seems to be behaving better during actuation- thanks again!

Sara

[elieK]

Hello Sara,

I am not sure if it could be a hotkey issue as I have never used Sofa on a mac. I have put the code on a drive below:

https://drive.google.com/drive/folders/1-MfPOVGQQQIxwVqzzRvpJwjnZ9iwnA7Z?usp=sharing

In this drive, there is both your modified implementation, and a second script testMesh-2.py. In this second script, I added the collision model and added the “TetrahedronSetTopologyContainer” as in the tutorial. Both scripts work for me, but you can try to see maybe adding these elements in testMesh-2.py could fix your issue. If not, could you add a print(c) command inside the onKeyPressed in the interaction class to check if it capturing the correct keys and if it is going into the conditional blocks.

Best regards,
Elie

[Sara]

Hi Elie,

It works!!! I kept having problems until I added print(self.cableconstraintvalue.value) as well as print(c) like you recommended.

Thanks so much for your help,

Sara

[Sara]

I am having some problems again as I add more elements to the scene:

I have assembled the gripper fingers with the full scene, but something is wrong with my animation again during bending. I think I might be forgetting to solve a constraint or have defined something incorrectly. I am printing the cable constraint values to terminal, and they are increasing and decreasing as they should, but the actual model itself is not bending, only flickering between the visual model and the mesh (I will post a link to some pictures below for clarification).

https://1drv.ms/u/s!ApELNE0leU38gje5AYDDvvPYROEP?e=HlWRtJ

I checked my code (especially the controllers and where they are attached and implemented) but I can’t find what could be wrong. I have put the meshes and *.py files in a the link below.

https://1drv.ms/u/s!ApELNE0leU38gighSP97f_BdaPeS?e=WEY5kP

Any advice on what I could do to solve this or what might be wrong is really appreciated.

Thanks!

Sara

[Sara]

It was the fixing boxes… It’s solved!

[Author]

Posts