skrobot.kinematics.LoopClosureSolver

class skrobot.kinematics.LoopClosureSolver(robot_model, config)

Solve a robot’s declared loop closures in place.

Set the driven (independent) joints to their targets, then call solve(): the dependent joints are updated so every closure’s two witness points – two points on the cut hinge axis, one frame per side of the cut – coincide. Large driver motions are sub-stepped so the solution stays on the assembled branch of the mechanism instead of jumping to a mirror configuration.

Parameters:

• robot_model (skrobot.model.RobotModel) – Robot built from the loop-cut URDF (e.g. via RobotAssembly.build_robot_model()).

• config (dict) – Closure config {closures: [{link_a, link_b, point, axis}], dependent: [...], independent: [...]} with point and axis expressed in the root frame at the zero pose – the loop_closures.yaml contract.

Examples

>>> robot = assembly.build_robot_model()
>>> solver = LoopClosureSolver(robot, assembly.loop_closures)
>>> robot.crank_hinge.joint_angle(0.4)
>>> solver.solve()

Methods

closure_error()

Largest witness-point gap (metres) over all closures.

Unlike the stacked residual norm solve() returns, this is a per-witness maximum, so the two differ by up to sqrt(2 * n_closures) on the same state.

classmethod from_yaml(robot_model, path)

Build a solver from a loop_closures.yaml sidecar file.

solve(tol=1e-10, max_iter=50, max_step=0.1, max_dq=0.5, raise_error=True)

Update the dependent joints so every loop closes.

Reads the CURRENT independent joint values as the target, interpolates from the previously solved values in steps of at most max_step (radians / metres), and Gauss-Newton solves the dependent joints at each sub-step. Each update is the minimum-norm least-squares step (SVD), which stays stable on the rank-deficient Jacobians every planar mechanism produces (the out-of-plane residual rows are identically zero).

Parameters:

• tol (float) – Success threshold on the residual norm (metres).

• max_iter (int) – Gauss-Newton iterations per sub-step.

• max_step (float) – Largest independent-joint change per sub-step; keeps the solution on the assembled branch of the mechanism.

• max_dq (float) – Cap on a single Gauss-Newton update per joint (radians / metres), guarding against wild steps near singularities.

• raise_error (bool) – Raise ValueError when the final residual exceeds tol (e.g. the loop cannot close at these driver values). Pass False to just return the residual.

Returns:

Final residual norm in metres.

Return type:

float

Notes

On failure the robot is left at the unconverged pose, but the warm-start state is NOT advanced: the next call sub-steps from the last successfully solved independent values, so one unreachable target does not poison later reachable ones.

__eq__(value, /)

Return self==value.

__ne__(value, /)

Return self!=value.

__lt__(value, /)

Return self<value.

__le__(value, /)

Return self<=value.

__gt__(value, /)

Return self>value.

__ge__(value, /)

Return self>=value.