Simulate pivot liaison using RigidMapping and ArticulatedSystem

Hi,

I’m new in using… rigid bodies and I don’t know how to create the behavior I want.
Basically I have a rigid linear tool, it’s position is mapped on the 2nd dof but it should go through the 1st dof. Like a pivot and joint ball liaison. See picture of the motion:

But when my tool start colliding with for example a rigid floor, the collision model is not anymore mapped through the 1st dof.

I’m using a basic rigid mapping toward the collision model and a articulated system mapping to constraint the motion. Should I use a rigidrigid mapping here to change the center of mass?

<Node name="Grasper" >     
        <EulerImplicitSolver name="cg odesolver" printLog="false"  rayleighStiffness="0.1" rayleighMass="0.1" />
        <CGLinearSolver iterations="100" name="linear solver" threshold="1e-20" tolerance="1e-20" />
        
        <MechanicalObject name="Articulations" template="Vec1d" position="0 0 0 0 0 0" />        
            
        <Node name="model">
            <MechanicalObject name="instrumentState" template="Rigid3d" position="@../Device.position"/>
            <RestShapeSpringsForceField points="0 1 2 3 4 5" 
                stiffness='1000000000000' 
                angularStiffness='1000000000000' 
                external_rest_shape='@../../Instrument/GrasperDOFs'  />
            <UncoupledConstraintCorrection/>
            <UniformMass name="mass" totalMass="100.0" />
            
           <ArticulatedSystemMapping input1="@../Articulations" output="@instrumentState" />
           <Node name="Shaft_collision" tags="toolCollision">
                <MeshObjLoader filename="./mesh/grasper_shaft_collision.obj"  name="loader"/>
                <MeshTopology src="@loader" name="InstrumentCollisionModel" />
                <MechanicalObject src="@loader" name="instrumentCollisionState"  ry="0" rz="0" dy="0" tags="toolPosition"/> 
                <SphereCollisionModel name="instrumentPoint" radius="0.2" group="0" /> 
                <LineCollisionModel group="0"/>
                <TriangleCollisionModel group="0"/>
                <RigidMapping name="MM->CM mapping" input="@.." output="@instrumentCollisionState" 
                index="1"/>
            </Node>
           
        </Node>
        
        <ArticulatedHierarchyContainer />
        <Node name="articulationCenters">
            <Node name="articulationCenter1">
                <ArticulationCenter parentIndex="0" childIndex="1" posOnParent="0 0 0" posOnChild="0 0 0" />
                <Node name="articulations">
                    <Articulation translation="1" rotation="0" rotationAxis="0 1 0" articulationIndex="0" />
                </Node>
            </Node>
        </Node>
    </Node>

Any idea is welcomed. thanks for your help!