FingerJoint is Physics3D.System:
    finger_connector is Physics3D.Interactions.MateConnector reference
    palm_connector is Physics3D.Interactions.MateConnector reference

    mate is Physics3D.Interactions.Hinge:
        connectors: [palm_connector, finger_connector]
        initial_angle: -1

    lock is Physics3D.Interactions.TorsionSpring:
        min_effort: -5
        max_effort: 5
        flexibility becomes Physics.Interactions.Flexibility.LinearElastic:
            stiffness: 100000
        connectors: mate.connectors
        angle: mate.initial_angle

THIRD_OF_CIRCLE is Real: Math.PI * 2 / 3

ThreeFingerGripper is Physics3D.System:
    finger_1 is Panda.Assets.Finger
    finger_2 is Panda.Assets.Finger
    finger_3 is Panda.Assets.Finger

    palm_radius is Real: 0.03
    palm_height is Real: 0.04

    connector is Physics3D.Interactions.RedirectedMateConnector:
        redirected_parent: palm
        main_axis: {1,0,0}

    palm is Physics3D.Bodies.RigidBody:
        kinematics.local_transform.position.z: palm_height * 0.5
        geometry is Physics3D.Geometries.Cylinder:
            radius: palm_radius
            height: palm_height
            local_transform.rotation: Math.Quat.from_to({0,1,0}, {0,0,1})
        visual is Visuals.Geometries.Cylinder:
            radius: palm.geometry.radius
            height: palm.geometry.height
            local_transform: palm.geometry.local_transform
        finger_connector_1 is Physics3D.Interactions.MateConnector:
            position: Math.Vec3.from_xyz(palm_radius, 0.0, palm_height * 0.5 + finger_1.part_1.tip.r)
            main_axis: {0,1,0}
        finger_connector_2 is Physics3D.Interactions.MateConnector:
            position: Math.Vec3.from_xyz(palm_radius * Math.Functions.cos(THIRD_OF_CIRCLE), palm_radius * Math.Functions.sin(THIRD_OF_CIRCLE), palm_height * 0.5 + finger_1.part_1.tip.r)
            main_axis: Math.Quat.angle_axis(THIRD_OF_CIRCLE, {0,0,1}).rotate(Math.Vec3.from_xyz(0,1,0))
        finger_connector_3 is Physics3D.Interactions.MateConnector:
            position: Math.Vec3.from_xyz(palm_radius * Math.Functions.cos(-THIRD_OF_CIRCLE), palm_radius * Math.Functions.sin(-THIRD_OF_CIRCLE), palm_height * 0.5 + finger_1.part_1.tip.r)
            main_axis: Math.Quat.angle_axis(-THIRD_OF_CIRCLE, {0,0,1}).rotate(Math.Vec3.from_xyz(0,1,0))

    joint_1 is FingerJoint:
        finger_connector: finger_1.part_1.connector_in
        palm_connector: palm.finger_connector_1

    joint_2 is FingerJoint:
        finger_connector: finger_2.part_1.connector_in
        palm_connector: palm.finger_connector_2

    joint_3 is FingerJoint:
        finger_connector: finger_3.part_1.connector_in
        palm_connector: palm.finger_connector_3

    palm_and_connecting is Simulation.CollisionGroup:
        bodies: [finger_1.part_1, finger_2.part_1, finger_3.part_1, palm]

    disable_local is Simulation.DisableCollisionPair:
        group1: palm_and_connecting
        group2: palm_and_connecting
