Skip to content

Use Node-Red to implement inverse kinematics to control excavator.

Notifications You must be signed in to change notification settings

wolfboyyang/excavator-control-challenge

Folders and files

NameName
Last commit message
Last commit date

Latest commit

 

History

13 Commits
 
 
 
 
 
 
 
 
 
 
 
 

Repository files navigation

Control Excavator with Inverse Kinematics

excavator simulator in browser

excavator simulator repo

Dashboard Screenshot

Inverse Kinematics

As p3.y is not 0, (0.02), to simplify the solution, let move p3.y to p2.y as mq2 is make p3 rotate around y axis, 

With this operation, j2/j3/j4 are in the same line in global xy plane

angle_j0_j2_j0_base_0 is fixed for every given q1

rotate j0_j2_0 by -aj0 to j0_j4_0 is direction of j0_j4 when q0=0

when j0_j4_0 X j0_j4 < 0, q0 = -acos(j0_j4*j0_j4_0/(a*c))
when j0_j4_0 X j0_j4 > 0, q0 = acos(j0_j4*j0_j4_0/(a*c))

  • Calulate q2/q3 in j2/j3/j4 arm plane
Law of Cosines
c^2 = a^2 + b^2 - 2 * a * b * cos(a3)

Law of Sines
c/sin(a3) = a/sin(a4) = b/sin(a2)

mimic q2 = q2 * multiplier + offset

for IK
a2_0 + mq2 = a2

a3_0 + mq3 = a3

Code for Inverse Kinematics Node

On Setup Tab:

  • add mathjs as math
  • add clamp as clamp
  • change output to 5
const rot_z =  [[0, -1, 0],
                [1,  0, 0],
                [0,  0, 1]]

const p0 = [0.68, -0.63, 0.38]
const p10 = [0.3, -0.31, 0.08]
const p11 = [-0.142, 0.107, 0.02]
const p1 = math.chain(rot_z).multiply(p11).add(p10).done();
// add p3.y to p2.y to make j2,j3,j4 in line when projected to global xy plane
const p2 = [-0.17, -0.08 + 0.02, 0.09] // origin p2 [-0.17, -0.08, 0.09]
const p3 = [-1.4, 0.02 - 0.02, 1.06] // origin p3 [-1.4, 0.02, 1.06]
const p4 = [0.05, 0, -1.14]

const mq2_range = [-0.4517, 0.46378]
const mq3_range = [-0.217, 1.910777]


tx = flow.get('x')
ty = flow.get('y')
tz = flow.get('z')
j4 = [tx, ty, tz]

function length_of_vector(v) {
    if (math.count(v) == 2)
        return math.distance(v, [0, 0])
    else 
        return math.distance(v, [0, 0, 0])
}
function angle_of_vector(v1, v2){
    return math.evaluate(`acos(${math.multiply(v1,v2)}/(${length_of_vector(v1)}*${length_of_vector(v2)}))`)
}
function rotate_axis_z(angle) {
    return [[math.cos(angle), -math.sin(angle), 0],
            [math.sin(angle),  math.cos(angle), 0],
            [              0,                0, 1]]
}
function rotate_axis_y(angle) {
    return [[ math.cos(angle), 0, math.sin(angle)],
            [            0, 1,            0],
            [-math.sin(angle), 0, math.cos(angle)]]
}
q1 = flow.get('q1')
if(!q1) q1 = 0

// base_link <- Kabine
j0 = p0

// caluclate q0 in xy plane
// regardless q0, aj2 and distance of j2 to j0 is fixed 
// aj2 is the angle between -x axis at j2 with j2_j0

// q1 - Kabine <- Bagger_Verbindung_Arm
//             <- Arm1_zu_BaggerVerbindungArm2
// in j0 local space (no effect by q0)
mq1 = rotate_axis_z(q1)
m1_j0 = math.chain(rot_z).multiply(mq1).done()
j0_j2_j0 = math.chain(m1_j0).multiply(p2).add(p1).done()
j0_j2_j0_xy = math.subset(j0_j2_j0, math.index([0,1]))

axis_j4_j2_j0_xy = math.chain(m1_j0).multiply([1,0,0])
        .subset(math.index([0,1])).done()
aj2 = angle_of_vector(j0_j2_j0_xy, axis_j4_j2_j0_xy)
a = length_of_vector(j0_j2_j0_xy)

// back to global word (from base_link)
j0_j4_xy = math.chain(j4).subtract(j0)
        .subset(math.index([0,1])).done()
c = length_of_vector(j0_j4_xy)
aj4 = math.evaluate(`asin(sin(${aj2})*${a}/${c})`)
aj0 = math.evaluate(`pi - ${aj2} -${aj4}`)

// when q0 = 0
j0_j4_0_xy = math.chain(rot_z).multiply(rotate_axis_z(-aj0))
        .multiply(j0_j2_j0)
        .subset(math.index([0,1])).done()
sign = math.sign(math.det([j0_j4_0_xy, j0_j4_xy]))
q0 = sign* angle_of_vector(j0_j4_0_xy, j0_j4_xy)

// q0 is done
// update j1,j2

// q1 - Kabine <- Bagger_Verbindung_Arm
//             <- Arm1_zu_BaggerVerbindungArm2
mq0 = rotate_axis_z(q0)
m0 = math.chain(rot_z).multiply(mq0).done()
j1 = math.chain(m0).multiply(p1).add(j0).done()

mq1 = rotate_axis_z(q1)
m1 = math.chain(m0).multiply(rot_z).multiply(mq1).done()
j2 = math.chain(m1).multiply(p2).add(j1).done()

// calculate q2, q3 in j2,j4, axis z plane
// mimic q2 - Arm1_zu_BaggerVerbindungArm2 <- Arm1
multiplier = -4.5774
offset = -0.4517

// when mq2 = 0
j2_p3_0 = math.chain(m1).multiply(p3).done()
j2_j4 = math.subtract(j4, j2)
a2_0 = angle_of_vector(j2_j4,j2_p3_0)

// for q2
// Law of Cosines
// c^2 = a^2 + b^2 - 2 * a * b * cos(a3)
a = length_of_vector(p3)
b = length_of_vector(p4)
c = math.distance(j4, j2)

a3 = math.evaluate(`acos((${a}^2+${b}^2-${c}^2)/(2*${a}*${b}))`)
// Law of Sines
// c/sin(a3) = a/sin(a4) = b/sin(a2)
a2 = math.evaluate(`asin(sin(${a3})*${b}/${c})`)

j2_j4 = math.subtract(j4, j2)
// in local space of j2
// j2_p3_v_j2 || mmq2 * p3
m1_trans = math.transpose(m1)
j2_p3_v_j2 = math.chain(rotate_axis_y(a2)).multiply(m1_trans)
        .multiply(j2_j4).done()
sign = math.sign(math.chain(p3).cross(j2_p3_v_j2).multiply([0,1,0]).done())
mq2 = sign * angle_of_vector(p3, j2_p3_v_j2)

mq2 = clamp(mq2, mq2_range[0], mq2_range[1])
// For forward kinematic, q2 = q2 * multiplier + offset
// For inverse kinematic
q2 = (mq2 - offset) / multiplier

mmq2 = rotate_axis_y(mq2)
m2 = math.chain(m1).multiply(mmq2).done();

j3 = math.chain(m2).multiply(p3).add(j2).done()

// mimic q3 - Arm1 <- Arm2
multiplier = -5.1897
offset = -0.217

// calculate q3
// when mq3 = 0
j3_j4 = math.subtract(j4, j3)
// in local space of j4
// j3_p4_v_j3 || mmq3 * p4
m2_trans = math.transpose(m2)
j3_p4_v_j3 = math.multiply(m2_trans, j3_j4)
sign = math.sign(math.chain(p4).cross(j3_p4_v_j3).multiply([0,1,0]).done())
mq3 = sign * angle_of_vector(p4, j3_p4_v_j3)

mq3 = clamp(mq3, mq3_range[0], mq3_range[1])
// For forward kinematic, mq3 = q3 * multiplier + offset
// For inverse kinematic

q3 = (mq3 - offset) / multiplier

mmq3 = rotate_axis_y(mq3)
m3 = math.chain(m2).multiply(mmq3).done();

j4 = math.chain(m3).multiply(p4).add(j3).done()
// Arm2 <- Loeffel
// target is j4 where Loeffel attached 

dp = j4

flow.set('dx', dp[0])
flow.set('dy', dp[1])
flow.set('dz', dp[2])

outputMsgs = []
outputMsgs.push({payload: q0})
outputMsgs.push({payload: q1})
outputMsgs.push({payload: q2})
outputMsgs.push({payload: q3})

return outputMsgs;

About

Use Node-Red to implement inverse kinematics to control excavator.

Resources

Stars

Watchers

Forks

Releases

No releases published

Packages

No packages published