wzyn20051216/matlab-agent-skills / skills/matlab-robotics-autonomy
matlab-robotics-autonomy
VerifiedMATLAB robotics and autonomy workflow for rigid-body transforms, quaternions, forward/inverse kinematics, trajectory planning, rigid-body dynamics, ROS/ROS 2 bags, sensor fusion, state estimation, SLAM, and reproducible robot experiments. Use whenever the user asks MATLAB to verify a rotation or quaternion, compute FK/IK/Jacobians, check a trajectory against waypoints and limits, check dynamics (M/C/g, energy), or judge whether a recorded state estimate is consistent with its covariance. Missing toolbox, bag, robot model or hardware means SKIP, never a claimed result.
| File | Kind | Size |
|---|---|---|
| SKILL.md | skill md | 10.1 KB |
| references/robotics-verification-contract.md | markdown | 17.6 KB |
| scripts/mas_dyn_check.m | other | 11.9 KB |
| scripts/mas_ekf_check.m | other | 6.9 KB |
| scripts/mas_estimate_check.m | other | 13.2 KB |
| scripts/mas_frame_check.m | other | 8.6 KB |
| scripts/mas_kin_check.m | other | 4.5 KB |
| scripts/mas_kin_ik.m | other | 6.1 KB |
| scripts/mas_quat_check.m | other | 12.2 KB |
| scripts/mas_traj_check.m | other | 11.0 KB |
| tests/test_robotics.m | other | 13.5 KB |
| tests/test_robotics_contract.m | other | 25.1 KB |
scripts/mas_kin_ik.m
View on GitHubfunction r = mas_kin_ik(L, pTarget, q0, opts)
%MAS_KIN_IK Damped-least-squares inverse kinematics for a planar n-R chain.
%
% r = mas_kin_ik([0.5 0.4], [0.6; 0.3], [0.1; 0.1])
% r = mas_kin_ik(L, p, q0, struct('method','normal')) % the fragile variant
%
% r.q joint solution
% r.pos tip actually reached (forward kinematics of r.q)
% r.err |pTarget - r.pos|
% r.nIter iterations used
% r.converged err <= tol
% r.feasible target inside maxReach (checked before iterating)
% r.stepRawMax largest UNBOUNDED |dq| -- the number that explodes at a singularity
% r.maxStep largest |dq| actually applied, after the step limit
% r.condMin smallest Jacobian condition number seen along the path
% r.verdict
%
% WHY DAMPING, AND WHAT THE `normal` METHOD IS FOR
% Solving (J'J)dq = J'e is the form most people write first. At a singularity J'J
% loses rank, MATLAB warns and returns a step of order 1/eps, and the arm is thrown
% across the workspace. DLS solves
%
% dq = J'(JJ' + lambda^2 I)^{-1} e
%
% which stays bounded; the cost is that the tip slows near the boundary instead of
% teleporting, which is the behaviour you want.
%
% `method='normal'` is kept deliberately, not as a curiosity: it is the negative
% control the test uses to prove `stepRawMax` measures something. A "robustness"
% metric that stays flat when the fragile method blows up would be worthless, so the
% test asserts the normal method's raw step is orders of magnitude larger than DLS's
% at a boundary target.
%
% STEPS ARE LIMITED, AND THE UNLIMITED VALUE IS STILL REPORTED
% The update is clamped to `stepLimit` rad per joint per iteration, because the
% Jacobian is a first-order model: a large Cartesian error produces a step that
% overshoots the region where that model holds, and the iteration oscillates
% instead of converging. Suppressing the clamp silently would hide the very
% information that tells you the configuration is bad, so both numbers are returned:
% `maxStep` is what was applied, `stepRawMax` is what the linearization asked for.
%
% Joint limits are applied AFTER the step -- clamping inside would corrupt the
% least-squares direction -- and `r.limitHit` records whether the clamp ever bound,
% because a solution found while clamped solves a DIFFERENT problem.
if nargin < 4, opts = struct(); end
opts = dflt(opts, struct('lambda', 1e-3, 'maxIter', 200, 'tol', 1e-10, ...
'method', 'dls', 'stepLimit', 0.5, 'qMin', [], 'qMax', []));
% L, q and theta are COLUMNS throughout.
L = L(:); n = numel(L);
pTarget = pTarget(:);
if numel(pTarget) ~= 2
error('mas_kin_ik:target', 'pTarget must be a 2-element [x; y]');
end
if nargin < 3 || isempty(q0), q0 = zeros(n, 1); end
q0 = q0(:);
if numel(q0) ~= n
error('mas_kin_ik:size', 'L has %d links but q0 has %d joints', n, numel(q0));
end
maxReach = sum(L);
biggest = max(L);
minReach = max(0, 2*biggest - maxReach); % only > 0 if one link dominates
d = norm(pTarget);
feasible = (d <= maxReach + 1e-9) && (d >= minReach - 1e-9);
q = q0;
warn = {};
if ~feasible
% Say which side, because "infeasible" alone sends the caller looking in the
% wrong place: too far for the arm, or too close for it to fold back.
if d > maxReach + 1e-9
why = sprintf('beyond max reach %.4f', maxReach);
else
why = sprintf('inside min reach %.4f (the longest link cannot fold back that far)', minReach);
end
warn{end+1} = sprintf('target at |p| = %.4f is %s: no solution exists, no iterations run', d, why);
nIter = 0; maxStep = 0; stepRawMax = 0; condMin = Inf; err = norm(pTarget - fk(L, q));
else
maxStep = 0; stepRawMax = 0; condMin = Inf; nIter = opts.maxIter;
for it = 1:opts.maxIter
e = pTarget - fk(L, q);
err = norm(e);
nIter = it;
if err <= opts.tol, break; end
J = jac(L, q);
s = svd(J);
condMin = min(condMin, s(1)/max(s(end), eps(s(1))));
switch lower(opts.method)
case 'dls'
dq = J.' * ((J*J.' + opts.lambda^2*eye(2)) \ e);
case 'normal'
dq = (J.'*J) \ (J.'*e);
otherwise
error('mas_kin_ik:method', 'unknown method "%s"', opts.method);
end
raw = norm(dq);
if ~isfinite(raw)
warn{end+1} = sprintf('non-finite step at iteration %d: Jacobian is singular here', it);
break;
end
stepRawMax = max(stepRawMax, raw);
dq = clamp_norm(dq, opts.stepLimit);
maxStep = max(maxStep, norm(dq));
q = q + dq;
if ~isempty(opts.qMin), q = max(q, opts.qMin(:)); end
if ~isempty(opts.qMax), q = min(q, opts.qMax(:)); end
end
end
limitHit = false;
if ~isempty(opts.qMin), limitHit = limitHit || any(q <= opts.qMin(:) + 1e-12); end
if ~isempty(opts.qMax), limitHit = limitHit || any(q >= opts.qMax(:) - 1e-12); end
if limitHit
warn{end+1} = ['a joint limit bound at the solution: this is a constrained ' ...
'solution, not the unconstrained IK that was asked for.'];
end
r = struct();
r.q = q; r.pos = fk(L, q); r.err = err; r.nIter = nIter;
r.converged = (err <= opts.tol);
r.feasible = feasible; r.maxReach = maxReach; r.minReach = minReach;
r.maxStep = maxStep; r.stepRawMax = stepRawMax;
r.condMin = condMin; r.limitHit = limitHit;
r.warnings = warn;
r.ok = r.converged;
r.verdict = tern(r.ok, 'PASS', 'FAIL');
r.detail = sprintf('err %.3e in %d iters, maxStep %.3g, rawMax %.3g, minCond %.3g -- %s', ...
err, nIter, maxStep, stepRawMax, condMin, r.verdict);
end
% ---------------------------------------------------------------------------
function r = dflt(r, d)
f = fieldnames(d);
for k = 1:numel(f)
if ~isfield(r, f{k}), r.(f{k}) = d.(f{k}); end
end
end
function s = tern(c, a, b)
if c, s = a; else, s = b; end
end
function p = fk(L, q)
th = cumsum(q(:));
p = [sum(L(:).*cos(th)); sum(L(:).*sin(th))];
end
function J = jac(L, q)
n = numel(L);
th = cumsum(q(:));
J = zeros(2, n);
for j = 1:n
J(1, j) = -sum(L(j:n) .* sin(th(j:n)));
J(2, j) = sum(L(j:n) .* cos(th(j:n)));
end
end
function v = clamp_norm(v, lim)
nv = norm(v);
if nv > lim, v = v * (lim/nv); end
end
Related skills
| Skill | Stars | Updated | Works with | Status | Install |
|---|---|---|---|---|---|
| planning-and-task-breakdownaddyosmani/agent-skillsBreaks work into ordered tasks. Use when you have a spec or clear requirements and need to break work into implementable tasks. Use when a task feels too large to start, when you need to estimate scope, or when parallel work is possible. | Stars: 119k | Updated | Claude CodeCodex | Verified | Install |
| screenpipethedotmack/claude-memSearch screen and audio history recorded by Screenpipe and optionally save selected context into claude-mem. Use when the user asks to recall desktop activity, a meeting, or something seen in another app through Screenpipe. | Stars: 118k | Updated | Claude CodeCodex | Verified | Install |
| contact-widgetnexu-io/open-designSelf-contained floating chat widget with welcome screen, social links, meeting button, and message input. Single HTML file, zero dependencies. | Stars: 108k | Updated | Claude CodeCodex | Verified | Install |
| meeting-notesnexu-io/open-designMeeting notes page — title bar with attendees, agenda checklist, decisions block, action items table with owners + dates, and a "next meeting" footer. Use when the brief mentions "meeting notes", "minutes", "1:1 notes", "all-hands recap", or "会议纪要". | Stars: 108k | Updated | Claude CodeCodex | Verified | Install |