Can QPInverseProblemSolver be used for parallel robot control?

[sodasoda520]

Dear SOFA community,
I'm working on a parallel robot with five degrees of freedom (3 translational + 2 rotational joints, with some translational joints decoupled). I've been studying the QPInverseProblemSolver in SOFA and found it very interesting for real-time inverse kinematics solving.
From the documentation and examples I've reviewed (like Diamond platform), most applications seem to focus on serial robots or soft robots. I'm wondering:
1.Is it possible to use QPInverseProblemSolver for parallel robot control?
2.If yes, how should we handle the closed-chain constraints? Are there any specific components or approaches recommended for parallel mechanisms?
3.Are there any existing examples or tutorials about using SOFA/QPInverseProblemSolver with parallel robots?
I've tried looking into components like RestShapeSpringsForceField and RigidMapping for handling the constraints, but I'm not sure if this is the right approach.
Any guidance, examples, or suggestions would be greatly appreciated!

Best regards,

[damienmarchal]

I think @EulalieCoevoet is the best one that can provides an answer.

[EulalieCoevoet]

Hello @sodasoda520,

I think you can try with the ArticulatedSystem plugin to model your parallel robot, and the SoftRobots and SoftRobots.Inverse plugins to solve its inverse kinematics. I've tried myself a long time ago. The robot had just one dof. I'm sorry I don't remember why I didn't add more dofs. Here's what I could excavate.

So in the following example, I use the ArticulatedSystemPlugin to create the kinematic chain. I close the chain using the RigidDistanceMapping from the Cosserat plugin (I guess this could be moved to SOFA, but it's another subject) and a RestShapeSpringsForceField (in short, this puts a spring between each extremity of the four parts). This might not be the best solution, but it's all I can think of at the moment.

from math import pi


def addCenter(node, name,
              parentIndex, childIndex,
              posOnParent, posOnChild,
              articulationProcess,
              isTranslation, isRotation, axis,
              articulationIndex):
    center = node.addChild(name)
    center.addObject('ArticulationCenter', parentIndex=parentIndex, childIndex=childIndex, posOnParent=posOnParent,
                     posOnChild=posOnChild, articulationProcess=articulationProcess)

    articulation = center.addChild('Articulation')
    articulation.addObject('Articulation', translation=isTranslation, rotation=isRotation, rotationAxis=axis,
                           articulationIndex=articulationIndex)

    return center


class Robot:

    def __init__(self, node, name='Articulations', inverse=False):
        self.node = node
        self.name = name
        self.inverse = inverse

        self.robot = self.__addRobot()
        if inverse:
            self.__addInverseComponents()

    def __addRobot(self):

        alpha1 = pi / 3.08
        alpha2 = pi / 6
        initAngles = [0, 0, 0, 0,
                      alpha1, alpha1, alpha1, alpha1,
                      alpha2, alpha2, alpha2, alpha2]
        width = 200
        height = width * 2.5

        positions = [[ width / 2, 0, -width / 2], [ width, 0, -width], [ width, height, -width], [0, height - 60, 0],
                     [ width / 2, 0,  width / 2], [ width, 0,  width], [ width, height,  width], [0, height - 60, 0],
                     [-width / 2, 0,  width / 2], [-width, 0,  width], [-width, height,  width], [0, height - 60, 0],
                     [-width / 2, 0, -width / 2], [-width, 0, -width], [-width, height, -width], [0, height - 60, 0]]

        # Articulations
        articulation = self.node.addChild(self.name)
        articulation.addObject('MechanicalObject', template='Vec1', position=initAngles) # The Dofs
        articulation.addObject('UniformMass', totalMass=1)
        articulation.addObject('RestShapeSpringsForceField', stiffness=1e3)

        for i in range(4):
            part = articulation.addChild("Part" + str(i))
            p = [positions[i * 4 + k]
                 + [[0, -0.924, 0, 0.383], [0, 0.924, 0, 0.383], [0, 0.383, 0, 0.924], [0, -0.383, 0, 0.924]][i]
                 for k in range(4)]
            part.addObject("MechanicalObject", template="Rigid3", 
                           position=p, showObject=True, showObjectScale=20, drawMode=0)

            # Visu
            visuPart = part.addChild("Visu")
            for k in range(2):
                visu = visuPart.addChild("Visu" + str(k))
                val = 10 / (k + 1)
                visu.addObject('RegularGridTopology', n=[2, 2, 2],
                               min=[[-width / 2, -height][k], -val, -val],
                               max=[[width / 2, 0][k], val, val])
                visu.addObject('OglModel')
                visu.addObject('RigidMapping', index=k + 1, globalToLocalCoords=False)

            addCenter(part, 'Center0', 0, 1, [0, 0, 0],
                      [width / 2., 0, 0],
                      0, 0, 1, [0, 0, 1], i)
            addCenter(part, 'Center1', 1, 2, [-width / 2, 0, 0],
                      [-height, 0, 0],
                      0, 0, 1, [0, 0, 1], 4 + i)
            addCenter(part, 'Center2', 2, 3, [0, 0, 0],
                      [0, 80, 0],
                      0, 0, 1, [0, 0, 1], 8 + i)

            part.addObject('ArticulatedHierarchyContainer')
            part.addObject('ArticulatedSystemMapping',
                           container=part.ArticulatedHierarchyContainer.linkpath,
                           input1=articulation.getMechanicalState().linkpath,
                           output=part.getMechanicalState().linkpath)

        # Attach extremities
        parts = [[articulation.Part0, articulation.Part1],
                 [articulation.Part1, articulation.Part2],
                 [articulation.Part2, articulation.Part3]]
        for i in range(3):
            distance = parts[i][0].addChild('Distance' + str(i))
            parts[i][1].addChild(distance)
            distance.addObject('MechanicalObject', template='Rigid3', rest_position=[0, 0, 0, 0, 0, 0, 1])
            distance.addObject('RestShapeSpringsForceField', points=0, stiffness=2e3, angularStiffness=1e8,
                               drawSpring=True)
            distance.addObject('RigidDistanceMapping',
                               input1=parts[i][0].getMechanicalState().linkpath,
                               input2=parts[i][1].getMechanicalState().linkpath,
                               output=distance.getMechanicalState().linkpath, first_point=[3], second_point=[3])

        return articulation

    def __addInverseComponents(self):
        # Target (red sphere)
        target = self.node.getRoot().Modelling.addChild('Target')
        target.addObject('EulerImplicitSolver', firstOrder=True)
        target.addObject('CGLinearSolver', iterations=25, threshold=1e-5, tolerance=1e-5)
        target.addObject('MechanicalObject', position=[100, 400, 0],
                         showObject=True, drawMode=2, showObjectScale=10)
        target.addObject('UncoupledConstraintCorrection')
        # Effector (black sphere)
        effector = self.robot.Part1.addChild('Effector')
        effector.addObject('MechanicalObject', position=[0, 0, 0], showObject=True, drawMode=1, showObjectScale=20,
                           showColor=[0, 0, 0, 1])
        effector.addObject('PositionEffector', name='effector', indices=[0],
                           effectorGoal=target.getMechanicalState().position.linkpath,
                           useDirections=[1, 1, 1])
        effector.addObject('RigidMapping', index=3)
        # Actuators (first joint of each part)
        for i in range(4):
            self.robot.addObject('JointActuator', name="joint" + str(i),
                                 index=i, # We add the actuator on the first joint
                                 maxAngleVariation=0.01,
                                 minAngle=-pi / 6, maxAngle=pi / 6)

# Test/example scene
def createScene(rootnode):
    INVERSE = True

    # Settings
    settings = rootnode.addChild('Settings')
    settings.addObject("ViewerSetting", resolution=[3840, 2160])
    settings.addObject('RequiredPlugin', name="ExternalPlugins",
                       pluginName=['SoftRobots', 'SoftRobots.Inverse', 'SofaPython3', 'Cosserat', 'ArticulatedSystemPlugin'])
    settings.addObject('RequiredPlugin', name='Sofa.Component.AnimationLoop')  # Needed to use components [FreeMotionAnimationLoop]
    settings.addObject('RequiredPlugin', name='Sofa.Component.Constraint.Lagrangian.Correction')  # Needed to use components [UncoupledConstraintCorrection]
    settings.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Direct')  # Needed to use components [SparseLDLSolver]
    settings.addObject('RequiredPlugin', name='Sofa.Component.LinearSolver.Iterative')  # Needed to use components [CGLinearSolver]
    settings.addObject('RequiredPlugin', name='Sofa.Component.Mapping.NonLinear')  # Needed to use components [RigidMapping]
    settings.addObject('RequiredPlugin', name='Sofa.Component.Mass')  # Needed to use components [UniformMass]  
    settings.addObject('RequiredPlugin', name='Sofa.Component.ODESolver.Backward')  # Needed to use components [EulerImplicitSolver]
    settings.addObject('RequiredPlugin', name='Sofa.Component.Setting')  # Needed to use components [BackgroundSetting,ViewerSetting]
    settings.addObject('RequiredPlugin', name='Sofa.Component.SolidMechanics.Spring')  # Needed to use components [RestShapeSpringsForceField]
    settings.addObject('RequiredPlugin', name='Sofa.Component.StateContainer')  # Needed to use components [MechanicalObject]
    settings.addObject('RequiredPlugin', name='Sofa.Component.Topology.Container.Grid')  # Needed to use components [RegularGridTopology]
    settings.addObject('RequiredPlugin', name='Sofa.Component.Visual')  # Needed to use components [VisualStyle]  
    settings.addObject('RequiredPlugin', name='Sofa.GL.Component.Rendering3D')  # Needed to use components [OglModel]  
    settings.addObject('RequiredPlugin', name='Sofa.GUI.Component')  # Needed to use components [AttachBodyButtonSetting]

    # Header
    rootnode.addObject('BackgroundSetting', color=[1.0, 1.0, 1.0, 1.0])
    rootnode.addObject('AttachBodyButtonSetting', stiffness=10)
    rootnode.addObject("DefaultVisualManagerLoop")
    rootnode.addObject('FreeMotionAnimationLoop')

    rootnode.addObject('VisualStyle', displayFlags="showInteractionForceFields")
    rootnode.gravity = [0, 9810, 0]
    rootnode.dt = 0.01

    rootnode.addChild('Modelling')
    simulation = rootnode.addChild('Simulation')

    if INVERSE:
        rootnode.addObject('QPInverseProblemSolver', maxIterations=500, tolerance=1e-5, epsilon=1, printLog=False)
    else:
        rootnode.addObject('GenericConstraintSolver', maxIterations=50, tolerance=1e-5, printLog=False)

    simulation.addObject('EulerImplicitSolver')
    simulation.addObject('SparseLDLSolver', template="CompressedRowSparseMatrixd")
    simulation.addObject('GenericConstraintCorrection')

    # Robot
    Robot(simulation, inverse=INVERSE)

    return

[EulalieCoevoet]

Otherwise, there is this plugin: Sofa.RigidBodyDynamics which is a work in progress to use the Pinocchio library with SOFA, but from what I understand it's not fully functional yet.

[sodasoda520]

Thank you very much！
I will try it later

[sodasoda520]

Hi Dear @EulalieCoevoet ,

I have studied your advice and am currently working on implementing kinematic simulation of a parallel mechanism using SOFA v23.06. I'm encountering some difficulties and would greatly appreciate your guidance.

I'm simulating a 4-bar parallel mechanism where:

• Parts 1 and 3 can slide along the X-axis (prismatic joints)
• Parts 2 and 4 are connected through three revolute joints
• The sliding motion of Parts 1 and 3 drives the rotation of the three revolute joints
• Part 4 serves as the end-effector platform
• The mechanism forms a closed kinematic chain

Current Status:

• Successfully implemented basic structure using ArticulatedSystemMapping
• Main challenge: maintaining closed-loop constraints
• Tried RigidDistanceMapping (found in CosseratPlugin) but got error:
  "Object type RigidDistanceMapping<> was not created"
• Also attempted BilateralInteractionConstraint without success

Questions:

1. Can RigidDistanceMapping or BilateralInteractionConstraint theoretically handle such parallel mechanism constraints?
2. How can I properly implement RigidDistanceMapping for this parallel mechanism?
3. Are there alternative approaches for implementing closed-loop constraints in SOFA?

Environment:

• SOFA v23.06 (pre-compiled)
• Python 3.8
• Windows 10

I would greatly appreciate any insights or suggestions on implementing these closed-loop constraints effectively in SOFA.

Thank you for your time and expertise.

Best regards