Robotics engineer: MQL recipes

Recipes

Forward kinematics and Jacobian of a planar 3R arm
Inverse kinematics of a 2-link arm and static torque
Joint trajectory through via points with speed limits
Rotation matrix of a tool from ZYX Euler angles
Odometry of a differential drive robot
Tool orientation: rotation matrix to axis-angle and quaternion
Re-orthonormalize a drifting rotation matrix with QR
Jerk-limited point-to-point move of an axis
16-bit encoder rollover and commutation angle
Nearest obstacle in the path from a 2D lidar scan

executed on MentDB

Forward kinematics and Jacobian of a planar 3R arm

For a planar arm with links 0.5, 0.4 and 0.2 m at joint angles 30, 45 and -30 deg, chain the homogeneous transforms to get the tool position and orientation, build the 2 x 3 position Jacobian, its singular values and the manipulability, and the joint speeds that move the tool at 0.1 m/s along x (resolved rate with the pseudo-inverse of the redundant Jacobian).

Pipeline

01 · anglesTrigonometry in degrees with [_angle]math calc→02 · transformsT = T1 T2 T3matrix mul · matrix get · matrix det→03 · JacobianSingular valuesmatrix svd→04 · resolved ratedq = J+ vmatrix pinv

Script

admin

#1. Planar 3R arm: links 0.5, 0.4, 0.2 m, joint angles in degrees;
-> "[_angle]" "deg";
-> "[q1]" 30; -> "[q2]" 45; -> "[q3]" -30;
#2. Homogeneous transforms Rot of q then Trans of L, chained with matrix mul;
-> "[T1]" (concat "[[" (math calc "cos([q1])") ", " (math calc "-sin([q1])") ", " (math calc "0.5*cos([q1])") "], [" (math calc "sin([q1])") ", " (math calc "cos([q1])") ", " (math calc "0.5*sin([q1])") "], [0, 0, 1]]");
-> "[T2]" (concat "[[" (math calc "cos([q2])") ", " (math calc "-sin([q2])") ", " (math calc "0.4*cos([q2])") "], [" (math calc "sin([q2])") ", " (math calc "cos([q2])") ", " (math calc "0.4*sin([q2])") "], [0, 0, 1]]");
-> "[T3]" (concat "[[" (math calc "cos([q3])") ", " (math calc "-sin([q3])") ", " (math calc "0.2*cos([q3])") "], [" (math calc "sin([q3])") ", " (math calc "cos([q3])") ", " (math calc "0.2*sin([q3])") "], [0, 0, 1]]");
-> "[T]" (matrix mul (matrix mul [T1] [T2]) [T3]);
#3. Position Jacobian 2 x 3 with the partial sums of the angles;
-> "[a]" (math calc "[q1]"); -> "[b]" (math calc "[q1]+[q2]"); -> "[c]" (math calc "[q1]+[q2]+[q3]");
-> "[J]" (concat "[[" (math calc "-0.5*sin([a])-0.4*sin([b])-0.2*sin([c])") ", " (math calc "-0.4*sin([b])-0.2*sin([c])") ", " (math calc "-0.2*sin([c])") "], [" (math calc "0.5*cos([a])+0.4*cos([b])+0.2*cos([c])") ", " (math calc "0.4*cos([b])+0.2*cos([c])") ", " (math calc "0.2*cos([c])") "]]");
#4. Manipulability from the singular values, resolved-rate joint speeds for v = 0.1, 0 m/s;
json load "rx_sv" (matrix svd [J]);
-> "[dq]" (matrix mul (matrix pinv [J]) "[0.1, 0]");
concat "tool at x = " (math round (matrix get [T] 1 3) 4) " m, y = " (math round (matrix get [T] 2 3) 4) " m, orientation " (math round (math calc (concat "atan2(" (matrix get [T] 2 1) ", " (matrix get [T] 1 1) ")")) 2) " deg, det R = " (math round (matrix det [T]) 6) ", singular values " (math round (json select "rx_sv" "/S[0]") 4) " and " (math round (json select "rx_sv" "/S[1]") 4) ", manipulability " (math round (math calc (concat (json select "rx_sv" "/S[0]") "*" (json select "rx_sv" "/S[1]"))) 4) ", joint speeds " [dq] " rad/s"

Result

mentdb

tool at x = 0.678 m, y = 0.7778 m, orientation 45 deg, det R = 1, singular values 1.1926 and 0.1447, manipulability 0.1726, joint speeds [0.1308645595868869,-0.3997423493050609,0.0650207473163015] rad/s

Go further

The session variable [_angle] stays in degrees afterwards: set it back with -> "[_angle]" "rad"
Check the result with the closed form x = sum Li cos(q1+...+qi)
Find the configurations where the smallest singular value is 0 with num minimize
Use matrix transpose to get J^T F, the joint torques for a tool force

executed on MentDB

Inverse kinematics of a 2-link arm and static torque

Find the joint angles that bring a 2-link arm (0.4 m and 0.3 m) to a part at (0.5, 0.3) m in the elbow-up configuration by solving the two non-linear equations, check the elbow angle with the law of cosines, the position error and the tool position as a sum of complex numbers L1 e^(ia) + L2 e^(i(a+b)), then compute the base torque r x m g0 for a 2.04 kg part and the wrist angle needed to point the gripper down.

Pipeline

01 · solveNon-linear system of the tool positionnum solve_system→02 · checkLaw of cosines, position error, complex formmath calc · vector add · vector sub · vector norm · complex from_polar→03 · torquetau = r x F with g0const get · vector scale · vector cross→04 · wristAngle between the forearm and the verticalvector unit · vector angle · vector dot

Script

admin

#1. 2-link arm L1 = 0.4 m, L2 = 0.3 m: reach the part at x = 0.5, y = 0.3 m, elbow up;
-> "[ik]" (num solve_system "[\"0.4*cos(a)+0.3*cos(a+b)=0.5\", \"0.4*sin(a)+0.3*sin(a+b)=0.3\"]" "{\"a\": 1, \"b\": -1}");
json load "rx_ik" [ik];
-> "[a]" (json select "rx_ik" "/solution/a"); -> "[b]" (json select "rx_ik" "/solution/b");
#2. Closed form check of the elbow angle: law of cosines;
-> "[bc]" (math calc "-acos((0.5^2+0.3^2-0.4^2-0.3^2)/(2*0.4*0.3))");
#3. Elbow and tool positions, position error, same tool position as a complex number;
-> "[e]" (concat "[" (math calc "0.4*cos([a])") ", " (math calc "0.4*sin([a])") "]");
-> "[p]" (vector add [e] (concat "[" (math calc "0.3*cos([a]+[b])") ", " (math calc "0.3*sin([a]+[b])") "]"));
-> "[err]" (vector norm (vector sub [p] "[0.5, 0.3]"));
-> "[z]" (complex add (complex from_polar 0.4 [a]) (complex from_polar 0.3 (+ [a] [b])));
#4. Static torque at the base for a 2.04 kg part at the tool: tau = r x m g0;
-> "[F]" (vector scale "[0, -1, 0]" (math calc (concat "2.04*" (const get "g0"))));
-> "[tau]" (vector cross (concat "[" (json load select "rx_p" [p] "/[0]") ", " (json select "rx_p" "/[1]") ", 0]") [F]);
#5. Angle between the forearm and a vertical gripper approach;
-> "[fa]" (vector unit (vector sub [p] [e]));
concat "a = " (math round (math rad_to_deg [a]) 2) " deg, b = " (math round (math rad_to_deg [b]) 2) " deg (closed form " (math round (math rad_to_deg [bc]) 2) "), " (json select "rx_ik" "/iterations") " iterations, error " (math round [err] 9) " m, tool as complex " [z] ", base torque " [tau] " N.m, forearm direction " [fa] ", wrist angle to point down " (math round (math rad_to_deg (vector angle [fa] "[0, -1]")) 2) " deg, cos = " (math round (vector dot [fa] "[0, -1]") 4)

Result

mentdb

a = 59.45 deg, b = -67.98 deg (closed form -67.98), 5 iterations, error 0 m, tool as complex 0.5+0.30000000000000004i, base torque [0,0,-10.002782999999999] N.m, forearm direction [0.9889499332542205,-0.148249888757034], wrist angle to point down 81.47 deg, cos = 0.1482

Go further

Get the elbow-down solution with another initial value {"a": 0, "b": 1}
Draw the arm with plot xy through the base, the elbow and the tool
Map the reachable workspace with a double for loop

executed on MentDB

Joint trajectory through via points with speed limits

A joint must pass through 0, 30, 75, 90 and 60 deg at t = 0 to 4 s, starting and ending at rest. A clamped cubic spline gives the angle, the speed and the acceleration on a 0.05 s grid; the script checks the limits (60 deg/s, 120 deg/s2) and computes the time scaling factor that keeps both within the limits.

Pipeline

01 · gridTimes every 0.05 smath calc→02 · splineClamped spline value, speed, accelerationnum interpolate · num interpolate_derivative→03 · limitsMax of |speed| and |acceleration|stat max · stat min · math max→04 · meanMean angle from the integralnum interpolate_integral

Script

admin

#1. Joint 1 via points: time in s, angle in deg;
-> "[t]" "[0, 1, 2, 3, 4]"; -> "[q]" "[0, 30, 75, 90, 60]";
#2. Clamped cubic spline: zero speed at both ends, speed and acceleration on a 0.05 s grid;
-> "[g]" "["; -> "[sep]" "";
for (-> "[i]" 0) (<= [i] 80) (++ "[i]") { -> "[g]" (concat [g] [sep] (math calc "[i]*0.05")); -> "[sep]" ","; };
-> "[g]" (concat [g] "]");
-> "[v]" (num interpolate_derivative [t] [q] [g] "clamped");
-> "[a]" (num interpolate_derivative [t] [q] [g] "clamped" 2);
#3. Limits of the joint: 60 deg/s and 120 deg/s2, time scaling factor if exceeded;
-> "[vmax]" (math max (stat max [v]) (math abs (stat min [v])));
-> "[amax]" (math max (stat max [a]) (math abs (stat min [a])));
-> "[s]" (math max 1 (math max (/ [vmax] 60) (math sqrt (/ [amax] 120))));
-> "[mean]" (math calc (concat (num interpolate_integral [t] [q] 0 4 "clamped") "/4"));
concat "angle at t = 2.5 s: " (math round (num interpolate [t] [q] 2.5 "clamped") 2) " deg, max speed " (math round [vmax] 1) " deg/s, max acceleration " (math round [amax] 1) " deg/s2, mean angle " (math round [mean] 2) " deg, time scaling x" (math round [s] 3) " so the motion lasts " (math round (math calc "4*[s]") 2) " s"

Result

mentdb

angle at t = 2.5 s: 89.93 deg, max speed 47.3 deg/s, max acceleration 138.2 deg/s2, mean angle 56.25 deg, time scaling x1.073 so the motion lasts 4.29 s

Go further

Draw the spline and the via points with plot interpolate and the mode "clamped"
Compare with a quintic polynomial between two points (coefficients by matrix solve)
Repeat for every joint and take the largest scaling factor

executed on MentDB

Rotation matrix of a tool from ZYX Euler angles

The orientation of an end effector is given as yaw 30, pitch 15, roll 10 degrees. Build R = Rz Ry Rx, check that it is a proper rotation (R Rt = I, det R = 1), express the tool tip in the base frame and recover the three angles from R.

Pipeline

01 · rotationsRz, Ry, Rxmath cos→02 · composeR = Rz Ry Rx and tip positionmatrix mul→03 · checkR Rt - I and det Rmatrix norm_frobenius→04 · anglesatan2 and asin of the entriesmatrix get

Script

admin

#1. Orientation of the end effector: yaw 30, pitch 15, roll 10 degrees;
-> "[y]" (math calc "30*pi/180"); -> "[p]" (math calc "15*pi/180"); -> "[r]" (math calc "10*pi/180");
#2. Elementary rotations about Z, Y and X;
-> "[Rz]" (concat "[[" (math cos [y]) ", " (- 0 (math sin [y])) ", 0], [" (math sin [y]) ", " (math cos [y]) ", 0], [0, 0, 1]]");
-> "[Ry]" (concat "[[" (math cos [p]) ", 0, " (math sin [p]) "], [0, 1, 0], [" (- 0 (math sin [p])) ", 0, " (math cos [p]) "]]");
-> "[Rx]" (concat "[[1, 0, 0], [0, " (math cos [r]) ", " (- 0 (math sin [r])) "], [0, " (math sin [r]) ", " (math cos [r]) "]]");
#3. ZYX convention R = Rz Ry Rx and orthonormality check;
-> "[R]" (matrix mul (matrix mul [Rz] [Ry]) [Rx]);
-> "[err]" (matrix norm_frobenius (matrix sub (matrix mul [R] (matrix transpose [R])) (matrix identity 3)));
#4. Tool tip 0.2 m along x and 0.1 m along z of the flange, in the base frame;
-> "[tip]" (matrix mul [R] "[[0.2], [0], [0.1]]");
#5. Recover the angles from R;
-> "[yaw]" (math calc (concat "atan2(" (matrix get [R] 2 1) ", " (matrix get [R] 1 1) ")*180/pi"));
-> "[pitch]" (math calc (concat "-asin(" (matrix get [R] 3 1) ")*180/pi"));
-> "[roll]" (math calc (concat "atan2(" (matrix get [R] 3 2) ", " (matrix get [R] 3 3) ")*180/pi"));
concat "det R = " (math round (matrix det [R]) 12) ", ||R Rt - I|| = " (math round_sig [err] 2) " | tip = " (math round (matrix get [tip] 1 1) 4) ", " (math round (matrix get [tip] 2 1) 4) ", " (math round (matrix get [tip] 3 1) 4) " m | recovered yaw " (math round [yaw] 6) ", pitch " (math round [pitch] 6) ", roll " (math round [roll] 6) " deg"

Result

mentdb

det R = 1, ||R Rt - I|| = 0.00000000000000027 | tip = 0.1981, 0.0943, 0.0434 m | recovered yaw 30, pitch 15, roll 10 deg

Go further

Build the homogeneous 4 x 4 transform with the tip offset and chain two frames.
Look at pitch = 90 deg: the gimbal lock makes yaw and roll undetermined.

executed on MentDB

Odometry of a differential drive robot

A mobile robot with 0.05 m wheels, a 0.30 m track and 1024-tick encoders reports the tick increments of both wheels every 100 ms. Integrate the pose x, y, theta with the midpoint method and compare the travelled path with the straight-line displacement.

Pipeline

01 · ticksIncrements of the encodersjson load→02 · integrateDead reckoning with midpoint headingmath calc→03 · pathSum of the arc lengthsstat sum→04 · displacementNorm of x, yvector norm

Script

admin

#1. Differential drive: wheel radius 0.05 m, track 0.30 m, 1024 ticks per turn;
-> "[k]" (math calc "2*pi*0.05/1024"); -> "[track]" 0.30;
#2. Encoder increments left and right read every 100 ms;
json load "rx_ticks" "[[820, 820], [820, 820], [700, 940], [700, 940], [700, 940], [820, 820], [940, 700], [820, 820]]";
-> "[x]" 0; -> "[y]" 0; -> "[th]" 0; -> "[path]" "[]";
json load "rx_ds" "[]";
#3. Dead reckoning with the midpoint heading;
for (-> "[i]" 0) (< [i] (json count "rx_ticks" /)) (++ "[i]") {
  -> "[dl]" (* [k] (json select "rx_ticks" (concat "/[" [i] "]/[0]")));
  -> "[dr]" (* [k] (json select "rx_ticks" (concat "/[" [i] "]/[1]")));
  -> "[ds]" (/ (+ [dl] [dr]) 2);
  -> "[dth]" (/ (- [dr] [dl]) [track]);
  -> "[x]" (math calc "[x]+[ds]*cos([th]+[dth]/2)");
  -> "[y]" (math calc "[y]+[ds]*sin([th]+[dth]/2)");
  -> "[th]" (+ [th] [dth]);
  json iarray "rx_ds" / [ds] NUM;
};
#4. Travelled path, straight-line displacement and heading;
-> "[travel]" (stat sum (json doc "rx_ds"));
-> "[disp]" (vector norm (concat "[" [x] ", " [y] "]"));
concat "pose x = " (math round [x] 3) " m, y = " (math round [y] 3) " m, heading = " (math round (math calc "[th]*180/pi") 2) " deg | path " (math round [travel] 3) " m, displacement " (math round [disp] 3) " m"

Result

mentdb

pose x = 1.807 m, y = 0.699 m, heading = 28.13 deg | path 2.013 m, displacement 1.938 m

Go further

Store the poses in a JSON array and draw the trajectory with plot xy.
Propagate the encoder noise into a pose covariance with matrix mul.

executed on MentDB

Tool orientation: rotation matrix to axis-angle and quaternion

The robot controller reports the tool orientation as a 3x3 rotation matrix; ROS and the vision system expect an axis-angle and a unit quaternion. The script gets the angle from the trace, the axis from the skew-symmetric part and builds the quaternion.

Pipeline

01 · angletheta = acos((tr R - 1) / 2)matrix trace→02 · inverseArc cosinemath acos→03 · axis(R32 - R23, ...) csc(theta) / 2math csc→04 · quaternionw = cos(theta/2), v = u sin(theta/2)math sin

Script

admin

#1. Tool orientation read from the controller;
-> "[R]" "[[0.555556, -0.466239, 0.688461], [0.688461, 0.722222, -0.066453], [-0.466239, 0.510897, 0.722222]]";
#2. Rotation angle from the trace;
-> "[th]" (math acos (/ (- (matrix trace [R]) 1) 2));
#3. Rotation axis from the skew-symmetric part, scaled by csc theta / 2;
-> "[k]" (/ (math csc [th]) 2);
-> "[ux]" (* [k] (- (matrix get [R] 3 2) (matrix get [R] 2 3)));
-> "[uy]" (* [k] (- (matrix get [R] 1 3) (matrix get [R] 3 1)));
-> "[uz]" (* [k] (- (matrix get [R] 2 1) (matrix get [R] 1 2)));
#4. Unit quaternion w, x, y, z;
-> "[w]" (math cos (/ [th] 2)); -> "[s]" (math sin (/ [th] 2));
concat "theta = " (math round (math calc "[th]*180/pi") 3) " deg, axis = [" (math round [ux] 4) ", " (math round [uy] 4) ", " (math round [uz] 4) "] | q = [" (math round [w] 4) ", " (math round (* [s] [ux]) 4) ", " (math round (* [s] [uy]) 4) ", " (math round (* [s] [uz]) 4) "]"

Result

mentdb

theta = 60 deg, axis = [0.3333, 0.6667, 0.6667] | q = [0.866, 0.1667, 0.3333, 0.3333]

Go further

Handle theta close to 0 or 180 deg, where csc theta explodes, with the diagonal terms
Interpolate two orientations with SLERP on the quaternions

executed on MentDB

Re-orthonormalize a drifting rotation matrix with QR

After thousands of integrations of gyro increments, the attitude matrix of a robot is no longer a rotation. The script projects it back onto a rotation with a QR decomposition, fixes the column signs with the signs of the diagonal of R and measures the correction.

Pipeline

01 · decomposeM = Q.R (Householder)matrix qr→02 · signsS = diag(sign(R_ii))math signum→03 · fixQ.Smatrix diag→04 · checkdet = 1, orthogonality, correction sizematrix norm_frobenius

Script

admin

#1. Attitude matrix after drift;
-> "[M]" "[[0.7738, -0.5766, 0.2822], [0.54, 0.8242, 0.1922], [-0.344, 0.003, 0.9457]]";
-> "[err0]" (matrix norm_frobenius (matrix sub (matrix mul (matrix transpose [M]) [M]) (matrix identity 3)));
#2. QR decomposition;
json load "rx_p4f_qr" (matrix qr [M]);
-> "[Q]" (json select "rx_p4f_qr" "/Q");
#3. Make the diagonal of R positive: Q times diag of the signs;
-> "[S]" (matrix diag (concat "[" (math signum (json select "rx_p4f_qr" "/R/[0]/[0]")) ", " (math signum (json select "rx_p4f_qr" "/R/[1]/[1]")) ", " (math signum (json select "rx_p4f_qr" "/R/[2]/[2]")) "]"));
json unload "rx_p4f_qr";
-> "[Rn]" (matrix mul [Q] [S]);
#4. Checks: orthogonality, determinant and size of the correction;
-> "[err1]" (matrix norm_frobenius (matrix sub (matrix mul (matrix transpose [Rn]) [Rn]) (matrix identity 3)));
concat "orthogonality error " (math round_sig [err0] 3) " -> " (math round_sig [err1] 3) " | det = " (math round (matrix det [Rn]) 9) " | correction = " (math round (matrix norm_frobenius (matrix sub [M] [Rn])) 5)

Result

mentdb

orthogonality error 0.0192 -> 0.000000000000000489 | det = 1 | correction = 0.00998

Go further

Compare with the polar projection U.V^T from matrix svd, which is the closest rotation
Run the projection every N steps of the attitude integration

executed on MentDB

Jerk-limited point-to-point move of an axis

A linear axis must move 0.4 m with a jerk limit of 15 m/s^3. The script sizes the 4-phase jerk-limited profile with a cube root, checks the acceleration and speed limits of the drive, counts the 4 ms interpolation cycles and compares with an acceleration-limited profile.

Pipeline

01 · phasetj = cbrt(d / 2J)math cbrt→02 · limitsa = J tj, v = J tj^2math max→03 · cyclesNearest number of 4 ms cyclesmath rint→04 · compareBang-bang time 2 sqrt(d/a)math sqrt

Script

admin

#1. Move 0.4 m with jerk 15 m/s3, drive limits 4 m/s2 and 1.5 m/s;
-> "[d]" 0.4; -> "[J]" 15; -> "[amax]" 4; -> "[vmax]" 1.5;
#2. Four jerk phases of length tj, distance d = 2 J tj^3;
-> "[tj]" (math cbrt (/ [d] (* 2 [J])));
-> "[apk]" (* [J] [tj]); -> "[vpk]" (* [J] (* [tj] [tj]));
#3. Total time and number of 4 ms interpolation cycles;
-> "[T]" (* 4 [tj]);
-> "[n]" (math rint (/ [T] 0.004));
#4. Same peak acceleration without jerk limit: T = 2 sqrt of d over a;
-> "[Tb]" (* 2 (math sqrt (/ [d] [apk])));
concat "tj = " (math round [tj] 4) " s, T = " (math round [T] 4) " s = " [n] " cycles | a peak " (math round [apk] 3) " m/s2, v peak " (math round [vpk] 3) " m/s -> " (if (and (<= [apk] [amax]) (<= [vpk] [vmax])) {"within the drive limits"} {"limit reached: add constant phases"}) " | use of the limits " (math round (* 100 (math max (/ [apk] [amax]) (/ [vpk] [vmax]))) 1) " pct" " | bang-bang " (math round [Tb] 4) " s"

Result

mentdb

tj = 0.2371 s, T = 0.9485 s = 237 cycles | a peak 3.557 m/s2, v peak 0.843 m/s -> within the drive limits | use of the limits 88.9 pct | bang-bang 0.6707 s

Go further

Add constant acceleration and constant speed phases when a limit is reached (7-phase S-curve)
Plot the position, speed and acceleration with plot xy

executed on MentDB

16-bit encoder rollover and commutation angle

A servo drive reads a 16-bit incremental counter every 1 ms. The script unwraps the rollover with a bit mask, accumulates the counts, converts them into motor turns and joint angle through a 1:100 gearbox, and computes the electrical angle for commutation of a 4 pole-pair motor.

Pipeline

01 · unwrapdelta = (new - old) AND 0xFFFFmath bit_and→02 · signSigned deltajson select→03 · turnsFull turns and fractionmath trunc→04 · commutationElectrical anglemath frac_part

Script

admin

#1. Raw 16-bit counter samples, 1024 counts per motor turn, gearbox 1:100;
json load "rx_p4f_enc" "[65200, 65480, 230, 610, 980, 1290, 1180]";
-> "[cnt]" 0;
#2. Unwrap: delta modulo 65536 with a mask, then signed;
for (-> "[i]" 1) (< [i] (json count "rx_p4f_enc" /)) (++ "[i]") {
  -> "[dl]" (math bit_and (- (json select "rx_p4f_enc" (concat "/[" [i] "]")) (json select "rx_p4f_enc" (concat "/[" (- [i] 1) "]"))) 65535);
  if (> [dl] 32767) {-> "[dl]" (- [dl] 65536);};
  -> "[cnt]" (+ [cnt] [dl]);
};
json unload "rx_p4f_enc";
#3. Motor turns, joint angle and electrical angle with 4 pole pairs;
-> "[turns]" (/ [cnt] 1024);
concat "counts = " [cnt] " | motor = " (math trunc [turns]) " turns + " (math round (* 360 (math frac_part [turns])) 2) " deg | joint = " (math round (/ (* 360 [turns]) 100) 4) " deg | electrical angle = " (math round (* 360 (math frac_part (* 4 [turns]))) 2) " deg"

Result

mentdb

counts = 1516 | motor = 1 turns + 172.97 deg | joint = 5.3297 deg | electrical angle = 331.88 deg

Go further

Estimate the speed with a moving average of the deltas, dsp moving_average
Detect a lost count by comparing with the index pulse every 1024 counts

executed on MentDB

Nearest obstacle in the path from a 2D lidar scan

A mobile robot 0.70 m wide drives at 0.8 m/s with a lidar 0.20 m ahead of its center. The script converts the beams to robot coordinates, keeps the points inside the driving corridor, finds the nearest one with hypot and atan2, and compares it with the braking distance.

Pipeline

01 · projectBeams to x, ymath cos→02 · corridorKeep |y| < half widthmath abs→03 · nearestDistance and bearingmath hypot→04 · bearingatan2(y, x)math atan2→05 · safetyBraking distance v^2/2amath calc

Script

admin

#1. Scan from -30 to +30 deg every 10 deg, ranges in m;
json load "rx_p4f_scan" "[2.10, 1.85, 1.62, 1.95, 1.40, 0.98, 2.60]";
-> "[best]" 99; -> "[bear]" 0;
#2. Robot frame: lidar 0.20 m ahead, corridor half width 0.35 m;
for (-> "[i]" 0) (< [i] (json count "rx_p4f_scan" /)) (++ "[i]") {
  -> "[a]" (math calc "(-30+10*[i])*pi/180");
  -> "[r]" (json select "rx_p4f_scan" (concat "/[" [i] "]"));
  -> "[x]" (+ 0.20 (* [r] (math cos [a]))); -> "[y]" (* [r] (math sin [a]));
  if (and (< (math abs [y]) 0.35) (< (math hypot [x] [y]) [best])) {
    -> "[best]" (math hypot [x] [y]); -> "[bear]" (math atan2 [y] [x]);
  };
};
json unload "rx_p4f_scan";
#3. Braking distance at 0.8 m/s with 1.2 m/s2 and 0.15 s latency, plus the front half length 0.30 m;
-> "[stop]" (math calc "0.8*0.15+0.8^2/(2*1.2)+0.30");
concat "nearest obstacle in the corridor " (math round [best] 3) " m at " (math round (math calc "[bear]*180/pi") 1) " deg | stop distance " (math round [stop] 3) " m -> " (if (< [best] (* 1.5 [stop])) {"slow down"} {"keep speed"}) ", time to contact " (math round (/ (- [best] 0.30) 0.8) 2) " s"

Result

mentdb

nearest obstacle in the corridor 1.17 m at 16.6 deg | stop distance 0.687 m -> keep speed, time to contact 1.09 s

Go further

Group the points into obstacles with ml cluster before choosing the nearest
Adapt the speed so that the stop distance stays below the nearest obstacle with num solve