import $"{AGX_DATA_DIR}/models/URDF/panda/panda.urdf" as panda_with_tool.panda

PlxPandaWithTool is panda_with_tool.panda:
    urdf_package is Urdf.PackagePath:
        name: "franka_description"
        path: $"{AGX_DATA_DIR}/models/URDF/panda/franka_ros-kinetic/franka_description"

    reference_body: panda_link0
    panda_link0.is_dynamic: false

    collision_group is Simulation.CollisionGroup:
        systems: [this]

    disable_collisions is Simulation.DisableCollisionPair:
        group1: collision_group
        group2: collision_group

    mimic_coupling is Robotics.Joints.CoupledJointDriveTrain:
        primary_actuator becomes DriveTrain.PrismaticActuator:
            mate_3d: panda_finger_joint1
            .agx_actuator_internal_inertia: 1E-9
        mimic_actuator becomes DriveTrain.PrismaticActuator:
            mate_3d: panda_finger_joint2
            .agx_actuator_internal_inertia: 1E-9

    initial_finger_position is Real: panda_finger_joint1_range.end
    panda_finger_joint1.initial_position: initial_finger_position
    panda_finger_joint2.initial_position: initial_finger_position

    tool_actuator is Physics3D.Interactions.LinearSpring:
        connectors: panda_finger_joint1.connectors
        position: initial_finger_position
        min_effort: -10
        max_effort: 10

    panda_link7.panda_joint8_mc.position: {0,0,0.105}

trait PandaInitialJointStates:
    panda_joint1.initial_angle: Math.Functions.min([Math.Functions.max([0, panda_joint1_range.start]), panda_joint1_range.end])
    panda_joint2.initial_angle: Math.Functions.min([Math.Functions.max([0, panda_joint2_range.start]), panda_joint2_range.end])
    panda_joint3.initial_angle: Math.Functions.min([Math.Functions.max([0, panda_joint3_range.start]), panda_joint3_range.end])
    panda_joint4.initial_angle: Math.Functions.min([Math.Functions.max([0, panda_joint4_range.start]), panda_joint4_range.end])
    panda_joint5.initial_angle: Math.Functions.min([Math.Functions.max([0, panda_joint5_range.start]), panda_joint5_range.end])
    panda_joint6.initial_angle: Math.Functions.min([Math.Functions.max([0, panda_joint6_range.start]), panda_joint6_range.end])
    panda_joint7.initial_angle: Math.Functions.min([Math.Functions.max([0, panda_joint7_range.start]), panda_joint7_range.end])

    panda_joint1.angle_output.enabled: true
    panda_joint2.angle_output.enabled: true
    panda_joint3.angle_output.enabled: true
    panda_joint4.angle_output.enabled: true
    panda_joint5.angle_output.enabled: true
    panda_joint6.angle_output.enabled: true
    panda_joint7.angle_output.enabled: true

    panda_joint1.angular_velocity_output.enabled: true
    panda_joint2.angular_velocity_output.enabled: true
    panda_joint3.angular_velocity_output.enabled: true
    panda_joint4.angular_velocity_output.enabled: true
    panda_joint5.angular_velocity_output.enabled: true
    panda_joint6.angular_velocity_output.enabled: true
    panda_joint7.angular_velocity_output.enabled: true

trait PandaVelocityControl is PandaInitialJointStates:
    panda_joint1_actuator becomes Physics3D.Interactions.RotationalVelocityMotor
    panda_joint2_actuator becomes Physics3D.Interactions.RotationalVelocityMotor
    panda_joint3_actuator becomes Physics3D.Interactions.RotationalVelocityMotor
    panda_joint4_actuator becomes Physics3D.Interactions.RotationalVelocityMotor
    panda_joint5_actuator becomes Physics3D.Interactions.RotationalVelocityMotor
    panda_joint6_actuator becomes Physics3D.Interactions.RotationalVelocityMotor
    panda_joint7_actuator becomes Physics3D.Interactions.RotationalVelocityMotor

trait PandaTorqueControl is PandaInitialJointStates:
    panda_joint1_actuator becomes Physics3D.Interactions.TorqueMotor
    panda_joint2_actuator becomes Physics3D.Interactions.TorqueMotor
    panda_joint3_actuator becomes Physics3D.Interactions.TorqueMotor
    panda_joint4_actuator becomes Physics3D.Interactions.TorqueMotor
    panda_joint5_actuator becomes Physics3D.Interactions.TorqueMotor
    panda_joint6_actuator becomes Physics3D.Interactions.TorqueMotor
    panda_joint7_actuator becomes Physics3D.Interactions.TorqueMotor

trait PandaPositionControl is PandaInitialJointStates:
    panda_joint1_actuator becomes Physics3D.Interactions.TorsionSpring
    panda_joint2_actuator becomes Physics3D.Interactions.TorsionSpring
    panda_joint3_actuator becomes Physics3D.Interactions.TorsionSpring
    panda_joint4_actuator becomes Physics3D.Interactions.TorsionSpring
    panda_joint5_actuator becomes Physics3D.Interactions.TorsionSpring
    panda_joint6_actuator becomes Physics3D.Interactions.TorsionSpring
    panda_joint7_actuator becomes Physics3D.Interactions.TorsionSpring

PlxPandaWithToolPositionControl is PlxPandaWithTool:
    with PandaPositionControl

PlxPandaWithToolTorqueControl is PlxPandaWithTool:
    with PandaTorqueControl

PlxPandaWithToolVelocityControl is PlxPandaWithTool:
    with PandaVelocityControl

ScenePandaWithTool is Physics3D.System:
    panda is PlxPandaWithTool

ScenePandaWithToolVelocityControl is ScenePandaWithTool:
    panda becomes PlxPandaWithToolVelocityControl

ScenePandaWithToolTorqueControl is ScenePandaWithTool:
    panda becomes PlxPandaWithToolTorqueControl
