Rotation/orientation of a rigid model

[Anonymous]

I have a scissors made from 4 parts, I am trying to rotate the tips of the scissors using the transform engine and the splib library while using the Omni Phantom.

The issue is I am not sure how to rotate/change the orientation of the model around it self and not the world axes. I am using quaternions to do the rotation using splib. When I apply the rotation, I want the scissors to rotate around themselves but it rotates around the world axes.

I have attached the files that I am using to have a better idea of the issue. https://we.tl/t-RrnKGeZW1b

Any help is appreciated 🙂

Best,
Fayad

[Hugo]

Hi @fayad

Did my previous tip not work properly ?
I enclose here a temporary link for finding my working case

Do you have a short video clip of the result ?

Hugo

[Anonymous]

I did try what you told me to use last time but with no luck, I did download your code and ran it but got similar results to what I had.

https://imgur.com/rHK0gXE

Best,
Fayad

[Hugo]

Hmm I see, I did not have the issue, since I guess my scissor was in (0,0,0).
I will check this again and get back to you.

Hugo

[Anonymous]

Any updates.

Best,
Fayad

[Hugo]

Hi Fayad

The issue was indeed due to the fact that I was testing the interaction while (the frames controlling the scissor) being in the center of space (0,0,0).

To fix your issue, you need to compute a local rotation (with regards to your current position and not regarding the reference frame).

I have currently not a lot of time to implement it myself.
Let me know about your progresses.

Best

Hugo

[Hugo]

Hi @fayad

Did you succeed in solving your issue or do you still need support?
Best

Hugo

[Hugo]

Hi @fayad

I made it work during the night.
Please find the scn and python file below:

<Node name="root" dt="0.02" showBoundingTree="0" gravity="0 0 0">

    <!-- ================================== SETUP ================================== -->
    
    <!-- Collision Dependencies -->
    <VisualStyle displayFlags="" />
    <CollisionPipeline verbose="0" />
    <BruteForceDetection name="N2" />
    <CollisionResponse response="default" />
    <MinProximityIntersection name="Proximity" alarmDistance="0.08" contactDistance="0.05" useSurfaceNormals="false"/>
    <CollisionGroup />
    <!-- Collision Dependencies -->

    <!-- Omni Phantom Plugin & Settings -->
    <RequiredPlugin name="Geomagic plugin" pluginName="Geomagic" />
    <LCPConstraintSolver tolerance="0.001" maxIt="1000"/>
    <GeomagicDriver name="GeomagicDevice" deviceName="Default Device" scale="1" drawDeviceFrame="1" positionBase="0 0 0" orientationBase="0 0.707 0 -0.707"  />
    <!-- Omni Phantom Plugin & Settings -->

    <!-- Carving Plugin & Settings -->
    <RequiredPlugin name="Carving" pluginName="SofaCarving" />
    <CarvingManager active="true"/>
    <!-- Carving Plugin & Settings -->

    <PythonScriptController filename="scissors.py" classname="Scissors"/>

    <!-- ================================== SETUP ================================== -->

   

    <!-- ================================== OMNI ================================== -->

    <Node name="Omni">

        <MechanicalObject template="Rigid3d" name="DOFs" position="@GeomagicDevice.positionDevice"/>
        <MechanicalStateController template="Rigid3d" listening="true" mainDirection="-1.0 0.0 0.0" handleEventTriggersUpdate="true"/>

        <Node name="VisuAvatar1" activated="false" >
            <OglModel name="Visual" fileMesh="scissor_very_low_base_origin.obj" color="gray"/>
            <RigidMapping input="@.." output="@Visual" index="0"/>
        </Node>

        <Node name="VisuAvatar2" activated="false" >
            <OglModel name="Visual" fileMesh="scissor_very_low_connector_origin.obj" color="gray"/>
            <RigidMapping input="@.." output="@Visual" index="1"/>
        </Node>

        <Node name="VisuAvatar3" activated="false" >
            <OglModel name="Visual" fileMesh="scissor_very_low_left_tip_origin.obj" color="gray"/>
            <RigidMapping input="@.." output="@Visual" index="2"/>
        </Node>

        <Node name="VisuAvatar4" activated="false" >
            <OglModel name="Visual" fileMesh="scissor_very_low_right_tip_origin.obj" color="gray"/>
            <RigidMapping input="@.." output="@Visual" index="3"/>
        </Node>

        <Node name="RefModel1" >
            <MeshObjLoader filename="scissor_very_low_base_origin.obj"  name="loader"/>
            <Mesh src="@loader" name="InstrumentCollisionModel"/>
            <MechanicalObject src="@loader" name="instrumentCollisionState" scale="0.2" />
            <RigidMapping />
        </Node>

        <Node name="RefModel2" >
            <MeshObjLoader filename="scissor_very_low_connector_origin.obj"  name="loader"/>
            <Mesh src="@loader" name="InstrumentCollisionModel"/>
            <MechanicalObject src="@loader" name="instrumentCollisionState" scale="0.2" />
            <RigidMapping />
        </Node>

        <Node name="RefModel3" >
            <MeshObjLoader filename="scissor_very_low_left_tip_origin.obj"  name="loader"/>
            <Mesh src="@loader" name="InstrumentCollisionModel"/>
            <MechanicalObject src="@loader" name="instrumentCollisionState" scale="0.2" />
            <RigidMapping />
        </Node>

<!--         <Node name="RefModel4" >
            <MeshObjLoader filename="scissor_very_low_right_tip_origin.obj"  name="loader"/>
            <Mesh src="@loader" name="InstrumentCollisionModel"/>
            <MechanicalObject src="@loader" name="instrumentCollisionState" scale="0.2" />
            <RigidMapping />
        </Node> -->

    </Node>

    <Node name="ScissorPart">

        <MechanicalObject template="Rigid3d" name="DOFs" position="@GeomagicDevice.positionDevice" showObject="1" showObjectScale="02"/>
        <MechanicalStateController template="Rigid3d" listening="true" mainDirection="-1.0 0.0 0.0" handleEventTriggersUpdate="true"/>

            <Node name="RigidScissor" >
                <MeshObjLoader filename="scissor_very_low_right_tip_origin.obj"  name="loader"/>
                <Mesh src="@loader" name="InstrumentCollisionModel"/>
                <MechanicalObject template="Rigid3d" src="@loader" name="rigidInstrumentCollisionState" scale="0.2" />
                <RigidRigidMapping />

                <Node name="RefModel4" >
                    <MechanicalObject name="instrumentCollisionState" />
                    <IdentityMapping />
                </Node>
            </Node>
    </Node>

    <!-- ================================== OMNI ================================== -->

    <!-- ================================== SCISSORS MODELS ================================== -->

    <Node name="left_tip" >

        <!-- Solvers & Physics -->
        <EulerImplicitSolver name="ODE solver" rayleighStiffness="0.05" rayleighMass="1.0" />
        <CGLinearSolver name="linear solver" iterations="25" tolerance="1e-10" threshold="10e-10" />
        <MechanicalObject name="instrumentState" position="@GeomagicDevice.positionDevice" rest_position="0 0 0 0 0 0 1" template="Rigid3d" />
        <UniformMass name="mass" totalmass="0.005" />
        <LCPForceFeedback activate="true" forceCoef="0.1"/> <!-- ADDED : Compute a force-feedback for the device -->
        <UncoupledConstraintCorrection/>
        <!-- Solvers & Physics -->

        <!-- Visual Model -->
        <Node name="VisualModel" >
            <OglModel name="InstrumentVisualModel" fileMesh="scissor_very_low_left_tip_origin.obj" scale="0.2" />
            <RigidMapping name="MM->VM mapping" input="@../instrumentState" output="@InstrumentVisualModel" listening="true"/>
        </Node>
        <!-- Visual Model -->

        <!-- Collision Model -->
        <Node name="CollisionModel" >
            <MeshObjLoader filename="scissor_very_low_left_tip_origin.obj"  name="loader"/>
            <Mesh src="@loader" name="InstrumentCollisionModel" />
            <MechanicalObject src="@loader" name="instrumentCollisionState" scale="0.2" />
            <Line name="instrument" group="1" contactStiffness="20" />
            <Point name="instrument" group="1" contactStiffness="20" />
            <Triangle name="instrument" group="1" contactStiffness="20" />
            <RigidMapping name="MM->CM mapping" input="@../instrumentState" output="@instrumentCollisionState"  listening="true"/>
        </Node>
        <!-- Collision Model -->

        <!-- Force feedback mapper to omni -->
        <VectorSpringForceField  template="Vec3d" object1="@Omni/RefModel3/instrumentCollisionState" object2="@left_tip/CollisionModel/instrumentCollisionState" stiffness="10" viscosity="0" />
        <!-- Force feedback mapper to omni -->
    
    </Node>

    <Node name="base" >

        <!-- Solvers & Physics -->
        <EulerImplicitSolver name="ODE solver" rayleighStiffness="0.05" rayleighMass="1.0" />
        <CGLinearSolver name="linear solver" iterations="25" tolerance="1e-10" threshold="10e-10" />
        <MechanicalObject name="instrumentState" position="@GeomagicDevice.positionDevice" template="Rigid3d" showObject="1" showObjectScale="0.5"/>
        <UniformMass name="mass" totalmass="0.005" />
        <LCPForceFeedback activate="true" forceCoef="0.1"/> <!-- ADDED : Compute a force-feedback for the device -->
        <UncoupledConstraintCorrection/>
        <!-- Solvers & Physics -->

        <!-- Visual Model -->
        <Node name="VisualModel" >
            <OglModel name="InstrumentVisualModel" fileMesh="scissor_very_low_base_origin.obj" color="black"  scale="0.2"/>
            <RigidMapping name="MM->VM mapping" input="@../instrumentState" output="@InstrumentVisualModel" />
        </Node>
        <!-- Visual Model -->

        <!-- Collision Model -->
        <Node name="CollisionModel" >
            <MeshObjLoader filename="scissor_very_low_base_origin.obj"  name="loader"/>
            <Mesh src="@loader" name="InstrumentCollisionModel" />
            <MechanicalObject src="@loader" name="instrumentCollisionState" scale="0.2" />
            <Line name="instrument" group="1" contactStiffness="20" />
            <Point name="instrument" group="1" contactStiffness="20" />
            <Triangle name="instrument" group="1" contactStiffness="20" />
            <RigidMapping name="MM->CM mapping" input="@../instrumentState" output="@instrumentCollisionState" />
        </Node>
        <!-- Collision Model -->

        <!-- Force feedback mapper to omni -->
        <VectorSpringForceField  template="Vec3d" object1="@Omni/RefModel1/instrumentCollisionState" object2="@base/CollisionModel/instrumentCollisionState" stiffness="10" viscosity="0" />
        <!-- Force feedback mapper to omni -->
    
    </Node>

    <Node name="connector" >

        <!-- Solvers & Physics -->
        <EulerImplicitSolver name="ODE solver" rayleighStiffness="0.05" rayleighMass="1.0" />
        <CGLinearSolver name="linear solver" iterations="25" tolerance="1e-10" threshold="10e-10" />
        <MechanicalObject name="instrumentState" position="@GeomagicDevice.positionDevice" template="Rigid3d" />
        <UniformMass name="mass" totalmass="0.005" />
        <LCPForceFeedback activate="true" forceCoef="0.1"/> <!-- ADDED : Compute a force-feedback for the device -->
        <UncoupledConstraintCorrection/>
        <!-- Solvers & Physics -->

        <!-- Visual Model -->
        <Node name="VisualModel" >
            <OglModel name="InstrumentVisualModel" fileMesh="scissor_very_low_connector_origin.obj" scale="0.2" />
            <RigidMapping name="MM->VM mapping" input="@../instrumentState" output="@InstrumentVisualModel" />
        </Node>
        <!-- Visual Model -->

        <!-- Collision Model -->
        <Node name="CollisionModel" >
            <MeshObjLoader filename="scissor_very_low_connector_origin.obj"  name="loader"/>
            <Mesh src="@loader" name="InstrumentCollisionModel" />
            <MechanicalObject src="@loader" name="instrumentCollisionState" scale="0.2" />
            <Line name="instrument" group="1" contactStiffness="20" />
            <Point name="instrument" group="1" contactStiffness="20" />
            <Triangle name="instrument" group="1" contactStiffness="20" />
            <RigidMapping name="MM->CM mapping" input="@../instrumentState" output="@instrumentCollisionState" />
        </Node>
        <!-- Collision Model -->

        <!-- Force feedback mapper to omni -->
        <VectorSpringForceField  template="Vec3d" object1="@Omni/RefModel2/instrumentCollisionState" object2="@connector/CollisionModel/instrumentCollisionState" stiffness="10" viscosity="0" />
        <!-- Force feedback mapper to omni -->
    
    </Node>

    <Node name="right_tip" >
        <!-- Solvers & Physics -->
        <EulerImplicitSolver name="ODE solver" rayleighStiffness="0.05" rayleighMass="1.0" />
        <CGLinearSolver name="linear solver" iterations="25" tolerance="1e-10" threshold="10e-10" />
        <MechanicalObject name="instrumentState" position="@GeomagicDevice.positionDevice" template="Rigid3d" showObject="1" showObjectScale="0.5" />
        <UniformMass name="mass" totalmass="0.005" />
        <LCPForceFeedback activate="true" forceCoef="0.1"/> <!-- ADDED : Compute a force-feedback for the device -->
        <UncoupledConstraintCorrection/>
        <!-- Solvers & Physics -->

        <!-- Visual Model -->
        <Node name="VisualModel" >
            <OglModel name="InstrumentVisualModel" fileMesh="scissor_very_low_right_tip_origin.obj" scale="0.2" />
            <RigidMapping name="MM->VM mapping" input="@../instrumentState" output="@InstrumentVisualModel" />
        </Node>
        <!-- Visual Model -->

        <!-- Collision Model -->
        <Node name="CollisionModel" >
            <MeshObjLoader filename="scissor_very_low_right_tip_origin.obj"  name="loader"/>
            <Mesh src="@loader" name="InstrumentCollisionModel" />
            <MechanicalObject src="@loader" name="instrumentCollisionState" scale="0.2" />
            <Line name="instrument" group="1" contactStiffness="20" />
            <Point name="instrument" group="1" contactStiffness="20" />
            <Triangle name="instrument" group="1" contactStiffness="20" />
            <RigidMapping name="MM->CM mapping" input="@../instrumentState" output="@instrumentCollisionState" />
        </Node>
        <!-- Collision Model -->

        <!-- Force feedback mapper to omni -->
        <VectorSpringForceField  template="Vec3d" object1="@ScissorPart/RigidScissor/RefModel4/instrumentCollisionState" object2="@right_tip/CollisionModel/instrumentCollisionState" stiffness="10" viscosity="0" />
        <!-- Force feedback mapper to omni -->
    
    </Node>

    <!-- ================================== SCISSORS MODELS ================================== -->

</Node>

Python script:

import sys
import Sofa
import math
import numpy as np

# The main function that SOFA will run
# The class will map the keyboard keys to the model to be able to move it
class Scissors (Sofa.PythonScriptController):

    def createGraph(self,rootNode):
        return 0

    # The initial function that will run when the class is called
    def initGraph(self, node):
        print '=================== initGraph called (python side) ==================='
        self.rootNode = node
        self.children = node.getChildren()
        self.button = 0
        for node in self.children:
            if node.name == "left_tip":
                self.left_tip = node.getObject('instrumentState')
            if node.name == "right_tip":
                self.right_tip = node.getObject('instrumentState')
                # self.TE = node.getObject('TE')
                nodeCol = node.getChild('CollisionModel')
                self.right_tip_col = nodeCol.getObject('instrumentCollisionState')
            if node.name == "connector":
                self.connector = node.getObject('instrumentState')
            if node.name == "base":
                self.base = node.getObject('instrumentState')
        
        # self.TE = self.rootNode.getChild('ScissorPart').getChild('RefModel4-Transformed').getObject('TE')
        self.movingScissorPart = self.rootNode.getChild('ScissorPart').getObject('DOFs')
        self.angleX = 0
        self.angleY = 0
        self.angleZ = 0
        return 0

    # This function will get called every time a key is pressed
    def onKeyPressed(self, c):
        if c == '+':
            positionScissor = self.movingScissorPart.findData('position').value
            positionRightTip = self.right_tip.findData('position').value
            if positionScissor!=positionRightTip:
                print "Error ScissorPart is not equal to RightTip"

            self.quaternion_to_euler(positionScissor[0][3],positionScissor[0][4],positionScissor[0][5],positionScissor[0][6])
            
            self.angleX = self.angleX + 5

            quat = self.euler_to_quaternion()

            positionScissor[0][3] = quat[0]
            positionScissor[0][4] = quat[1]
            positionScissor[0][5] = quat[2]
            positionScissor[0][6] = quat[3]
            self.movingScissorPart.findData('position').value = positionScissor

            positionRightTip[0][3] = quat[0]
            positionRightTip[0][4] = quat[1]
            positionRightTip[0][5] = quat[2]
            positionRightTip[0][6] = quat[3]
            self.right_tip.findData('position').value = positionScissor

        if c == '-':
            positionScissor = self.movingScissorPart.findData('position').value
            positionRightTip = self.right_tip.findData('position').value
            if positionScissor!=positionRightTip:
                print "Error ScissorPart is not equal to RightTip"

            self.quaternion_to_euler(positionScissor[0][3],positionScissor[0][4],positionScissor[0][5],positionScissor[0][6])
            
            self.angleX = self.angleX - 5
            
            quat = self.euler_to_quaternion()
            
            positionScissor[0][3] = quat[0]
            positionScissor[0][4] = quat[1]
            positionScissor[0][5] = quat[2]
            positionScissor[0][6] = quat[3]
            self.movingScissorPart.findData('position').value = positionScissor

            positionRightTip[0][3] = quat[0]
            positionRightTip[0][4] = quat[1]
            positionRightTip[0][5] = quat[2]
            positionRightTip[0][6] = quat[3]
            self.right_tip.findData('position').value = positionRightTip
        return 0

    def euler_to_quaternion(self):
        roll = self.angleX * math.pi /180
        pitch = self.angleY * math.pi /180
        yaw = self.angleZ * math.pi /180
        qx = np.sin(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) - np.cos(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
        qy = np.cos(roll/2) * np.sin(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.cos(pitch/2) * np.sin(yaw/2)
        qz = np.cos(roll/2) * np.cos(pitch/2) * np.sin(yaw/2) - np.sin(roll/2) * np.sin(pitch/2) * np.cos(yaw/2)
        qw = np.cos(roll/2) * np.cos(pitch/2) * np.cos(yaw/2) + np.sin(roll/2) * np.sin(pitch/2) * np.sin(yaw/2)
        return [qx, qy, qz, qw]

    def quaternion_to_euler(self, x, y, z, w):
        t0 = 2.0 * (w * x + y * z)
        t1 = 1.0 - 2.0 * (x * x + y * y)
        X = math.degrees(math.atan2(t0, t1))

        t2 = 2.0 * (w * y - z * x)
        t2 = 1.0 if t2 > +1.0 else t2
        t2 = -1.0 if t2 < -1.0 else t2
        Y = math.degrees(math.asin(t2))

        t3 = 2.0 * (w * z + x * y)
        t4 = 1.0 - 2.0 * (y * y + z * z)
        Z = math.degrees(math.atan2(t3, t4))

        self.angleX = X
        self.angleY = Y
        self.angleZ = Z
        return 0

Hope this helps.
Best

Hugo

[Anonymous]

Hi @Hugo,

Thank you so much for the help & I am really sorry for the late reply. I tried the code you provided, but the scissors don’t rotate no matter what as long as the omni is attached to it.

Best,
Fayad

[Hugo]

Hi @fayad

Damned. I designed the scene without a Geomagic, since I don’t have one myself.
Strange that it does not work. I will need more time to find a Geomagic and get back to you.

Best wishes,

Hugo

[Anonymous]

Hi @Hugo,

Sorry for the frequent questions about this thing, but are there any updates?

Best,
Fayad

[Hugo]

Hi Fayad

Sorry the weeks are busy.
I will try to get a Geomagic this week and test it. Best

Hugo

[Anonymous]

Ok, will be waiting 🙂

Thanks @Hugo

[Hugo]

Hey @fayad

I tried last week to get back with the Geomagic.
I saw what you meant. I had less than a hour to build and test. From this, I would really go for a TransformEngine recovering as input the Geomagic position, and providing an output position for the right_tip MechanicalObject. Then, you could apply rotation, but a bit of code in needed to enforce a local rotation and not a rotation regarding the reference frame (0,0,0).

Hope this helps

Hugo

[Author]

Posts