# This file was automatically generated by the ILK-compiler on Wed Apr 10 14:11:52 2019

module ur5

import LinearAlgebra
import ILKBackend

struct ConstPosesContainer
    elbow__upperarm::ILKBackend.MatrixType
    forearm__wr1::ILKBackend.MatrixType
    shoulder__shoulder_lift::ILKBackend.MatrixType
    shoulder_lift__shoulder::ILKBackend.MatrixType
    shoulder_pan__base::ILKBackend.MatrixType
    upperarm__elbow::ILKBackend.MatrixType
    wr1__forearm::ILKBackend.MatrixType
    wr2__wrist_1::ILKBackend.MatrixType
    wr3__wrist_2::ILKBackend.MatrixType
    wrist_1__wr2::ILKBackend.MatrixType
    wrist_2__wr3::ILKBackend.MatrixType
end

function ConstPosesInit()
    elbow__upperarm = ILKBackend.pose()
    ILKBackend.setPosition(elbow__upperarm,0.425,0,-0.1197)
    ILKBackend.setRotation(elbow__upperarm,1,-0,0,0,1,-0,0,0,1)
    forearm__wr1 = ILKBackend.pose()
    ILKBackend.setPosition(forearm__wr1,-0.39225,0,-0.09315)
    ILKBackend.setRotation(forearm__wr1,1,-0,0,0,1,-0,0,0,1)
    shoulder__shoulder_lift = ILKBackend.pose()
    ILKBackend.setPosition(shoulder__shoulder_lift,0,0,-0.13585)
    ILKBackend.setRotation(shoulder__shoulder_lift,0,-0,1,-0,-1,0,1,-0,-0)
    shoulder_lift__shoulder = ILKBackend.pose()
    ILKBackend.setPosition(shoulder_lift__shoulder,0.13585,0,0)
    ILKBackend.setRotation(shoulder_lift__shoulder,0,-0,1,-0,-1,0,1,-0,-0)
    shoulder_pan__base = ILKBackend.pose()
    ILKBackend.setPosition(shoulder_pan__base,0,0,0.089159)
    ILKBackend.setRotation(shoulder_pan__base,1,-0,0,0,1,-0,0,0,1)
    upperarm__elbow = ILKBackend.pose()
    ILKBackend.setPosition(upperarm__elbow,-0.425,0,0.1197)
    ILKBackend.setRotation(upperarm__elbow,1,-0,0,0,1,-0,0,0,1)
    wr1__forearm = ILKBackend.pose()
    ILKBackend.setPosition(wr1__forearm,0.39225,0,0.09315)
    ILKBackend.setRotation(wr1__forearm,1,-0,0,0,1,-0,0,0,1)
    wr2__wrist_1 = ILKBackend.pose()
    ILKBackend.setPosition(wr2__wrist_1,0.09475,0,0)
    ILKBackend.setRotation(wr2__wrist_1,0,-0,1,-0,-1,0,1,-0,-0)
    wr3__wrist_2 = ILKBackend.pose()
    ILKBackend.setPosition(wr3__wrist_2,0.0825,0,0)
    ILKBackend.setRotation(wr3__wrist_2,-0,0,1,0,1,0,-1,0,-0)
    wrist_1__wr2 = ILKBackend.pose()
    ILKBackend.setPosition(wrist_1__wr2,0,0,-0.09475)
    ILKBackend.setRotation(wrist_1__wr2,0,0,1,0,-1,0,1,0,0)
    wrist_2__wr3 = ILKBackend.pose()
    ILKBackend.setPosition(wrist_2__wr3,0,0,-0.0825)
    ILKBackend.setRotation(wrist_2__wr3,0,0,-1,0,1,0,1,0,-0)

    ret = ConstPosesContainer(
        elbow__upperarm,
        forearm__wr1,
        shoulder__shoulder_lift,
        shoulder_lift__shoulder,
        shoulder_pan__base,
        upperarm__elbow,
        wr1__forearm,
        wr2__wrist_1,
        wr3__wrist_2,
        wrist_1__wr2,
        wrist_2__wr3,
    )
    return ret
end

const ModelConstants = ConstPosesInit()



function solver1(mc, q, qd)

    elbow__forearm = ILKBackend.rot_z__b_x_a(q[3])
    forearm__elbow = ILKBackend.rot_z__a_x_b(q[3])
    shoulder__shoulder_pan = ILKBackend.rot_z__a_x_b(q[1])
    shoulder_lift__upperarm = ILKBackend.rot_z__b_x_a(q[2])
    upperarm__shoulder_lift = ILKBackend.rot_z__a_x_b(q[2])
    wr1__wrist_1 = ILKBackend.rot_z__b_x_a(q[4])
    wr2__wrist_2 = ILKBackend.rot_z__b_x_a(q[5])
    wr3__wrist_3 = ILKBackend.rot_z__b_x_a(q[6])
    wrist_1__wr1 = ILKBackend.rot_z__a_x_b(q[4])
    wrist_2__wr2 = ILKBackend.rot_z__a_x_b(q[5])
    wrist_3__wr3 = ILKBackend.rot_z__a_x_b(q[6])

    shoulder_lift__shoulder_pan = shoulder__shoulder_pan * mc.shoulder_lift__shoulder
    shoulder_lift__base = mc.shoulder_pan__base * shoulder_lift__shoulder_pan
    elbow__shoulder_lift = upperarm__shoulder_lift * mc.elbow__upperarm
    elbow__base = shoulder_lift__base * elbow__shoulder_lift
    wr1__elbow = forearm__elbow * mc.wr1__forearm
    wr1__base = elbow__base * wr1__elbow
    wr2__wr1 = wrist_1__wr1 * mc.wr2__wrist_1
    wr2__base = wr1__base * wr2__wr1
    wr3__wr2 = wrist_2__wr2 * mc.wr3__wrist_2
    wr3__base = wr2__base * wr3__wr2
    forearm__wrist_1 = wr1__wrist_1 * mc.forearm__wr1
    upperarm__forearm = elbow__forearm * mc.upperarm__elbow
    wrist_2__wrist_3 = wr3__wrist_3 * mc.wrist_2__wr3
    shoulder__upperarm = shoulder_lift__upperarm * mc.shoulder__shoulder_lift
    wrist_1__wrist_2 = wr2__wrist_2 * mc.wrist_1__wr2
    wrist_3__base = wr3__base * wrist_3__wr3

    v__shoulder__base = ILKBackend.matrix(6,1);
    v__shoulder__base[3] = qd[1];

    v__upperarm__base = ILKBackend.matrix(6,1);
    ILKBackend.ct_twist( shoulder__upperarm, v__shoulder__base, v__upperarm__base );
    v__upperarm__base[3] += qd[1];
    v__forearm__base = ILKBackend.matrix(6,1);
    ILKBackend.ct_twist( upperarm__forearm, v__upperarm__base, v__forearm__base );
    v__forearm__base[3] += qd[2];
    v__wrist_1__base = ILKBackend.matrix(6,1);
    ILKBackend.ct_twist( forearm__wrist_1, v__forearm__base, v__wrist_1__base );
    v__wrist_1__base[3] += qd[3];
    v__wrist_2__base = ILKBackend.matrix(6,1);
    ILKBackend.ct_twist( wrist_1__wrist_2, v__wrist_1__base, v__wrist_2__base );
    v__wrist_2__base[3] += qd[4];
    v__wrist_3__base = ILKBackend.matrix(6,1);
    ILKBackend.ct_twist( wrist_2__wrist_3, v__wrist_2__base, v__wrist_3__base );
    v__wrist_3__base[3] += qd[5];

    J_wrist_3_base = ILKBackend.matrix(6,6)
    poi_J_wrist_3_base = ILKBackend.positionView(wrist_3__base)

    ILKBackend.geometricJacobianColumn_revolute(
      poi_J_wrist_3_base,
      ILKBackend.positionView(mc.shoulder_pan__base),
      ILKBackend.zaxisView(mc.shoulder_pan__base),
      view(J_wrist_3_base, :,1) )
    ILKBackend.geometricJacobianColumn_revolute(
      poi_J_wrist_3_base,
      ILKBackend.positionView(shoulder_lift__base),
      ILKBackend.zaxisView(shoulder_lift__base),
      view(J_wrist_3_base, :,2) )
    ILKBackend.geometricJacobianColumn_revolute(
      poi_J_wrist_3_base,
      ILKBackend.positionView(elbow__base),
      ILKBackend.zaxisView(elbow__base),
      view(J_wrist_3_base, :,3) )
    ILKBackend.geometricJacobianColumn_revolute(
      poi_J_wrist_3_base,
      ILKBackend.positionView(wr1__base),
      ILKBackend.zaxisView(wr1__base),
      view(J_wrist_3_base, :,4) )
    ILKBackend.geometricJacobianColumn_revolute(
      poi_J_wrist_3_base,
      ILKBackend.positionView(wr2__base),
      ILKBackend.zaxisView(wr2__base),
      view(J_wrist_3_base, :,5) )
    ILKBackend.geometricJacobianColumn_revolute(
      poi_J_wrist_3_base,
      ILKBackend.positionView(wr3__base),
      ILKBackend.zaxisView(wr3__base),
      view(J_wrist_3_base, :,6) )


    return wrist_3__base, v__wrist_3__base, J_wrist_3_base
end



end
