excavator simulator in browser
- Miro Board
- Joints
- Calulate q0 in XY plane
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
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;