A whole-body fit: the static posture nearest a preferred one in which
points of the body reach their goals, the parts that bear rest on the
floor, nothing passes under it, the centre of mass stands over the base
of support and the folded segments stay apart, all within the joint
ranges and solved together: the static multi-contact posture problem of
Bouyarmane and Kheddar 2010 (IEEE Humanoids, "Static multi-contact
inverse problem for multiple humanoid robots and manipulated objects")
and Escande, Kheddar and Miossec 2013 (Robot Auton Syst 61:428).
The variables are any set of the posture's own (HumanFitVariables), the
whole body's when none is given, and the objective keeps them nearest the
preferred posture in joint space. A target brings a point riding a bone
to a world goal on the axes it names, a rotation to a goal, or, as a
planar contact, the contact frame's normal parallel to the surface's with
its turn about it free. Included angles between bone-carried landmarks
and projections of the physical centre of mass are constrained by the
same fit, with their own analytic Jacobians (HumanFitGoals). Goals may
ride another bone, whose frame and Jacobian move with the fit. A physical
point radius is one distance inequality, without widening it by the
equality rows' numerical band (HumanFitTargets). A bearing part touches the floor at its lowest
point and every other point of it stays out of the floor, so a rigid
curved part rests on one point and a planar contact's normal lays it
flat. Every row is an exact penalty at its price (an elastic l1
sequential quadratic program; Fletcher 1985) linearised by the analytic
Jacobian of the posed body (HumanJacobian), and the merit
(HumanBodyFitMerit) prices exactly
what the rows do, so the model and the merit agree at every iterate
(Nocedal and Wright 2006, section 18.5). The steps are taken by the shared
penalty sequential convex loop (HumanSequentialConvex), its penalty
steered at each iterate (Byrd, Nocedal and Waltz 2008), each convex model
solved by the shared quadratic program (HumanQuadraticProgram) inside a
trust region, with one second-order correction of a rejected step
(Nocedal and Wright 2006, (15.36) and Algorithm 15.2).
Each convex model is solved by the interior-point method
(HumanQuadraticProgram.interior, Mehrotra's predictor-corrector as HPIPM
implements it, Frison and Diehl 2020), whose iteration count does not grow
with the conditioning of the model's dense joint-space curvature, and by
the operator splitting (OSQP) only where the interior method does not
converge. Solved by the splitting first, a third of a rise's models ended
unfinished at its iteration guard, short of the minimiser, and their steps
were rejected by the trust region for the error (measured on the get-up's
supine rise: 24 of 72 programs, 8.0 s of a 9.4 s fit, against 0.7 s by
the interior method, which met the splitting's polished minimisers to
1e-9).
A whole-body fit: the static posture nearest a preferred one in which points of the body reach their goals, the parts that bear rest on the floor, nothing passes under it, the centre of mass stands over the base of support and the folded segments stay apart, all within the joint ranges and solved together: the static multi-contact posture problem of Bouyarmane and Kheddar 2010 (IEEE Humanoids, "Static multi-contact inverse problem for multiple humanoid robots and manipulated objects") and Escande, Kheddar and Miossec 2013 (Robot Auton Syst 61:428).
The variables are any set of the posture's own (
HumanFitVariables), the whole body's when none is given, and the objective keeps them nearest the preferred posture in joint space. A target brings a point riding a bone to a world goal on the axes it names, a rotation to a goal, or, as a planar contact, the contact frame's normal parallel to the surface's with its turn about it free. Included angles between bone-carried landmarks and projections of the physical centre of mass are constrained by the same fit, with their own analytic Jacobians (HumanFitGoals). Goals may ride another bone, whose frame and Jacobian move with the fit. A physical point radius is one distance inequality, without widening it by the equality rows' numerical band (HumanFitTargets). A bearing part touches the floor at its lowest point and every other point of it stays out of the floor, so a rigid curved part rests on one point and a planar contact's normal lays it flat. Every row is an exact penalty at its price (an elastic l1 sequential quadratic program; Fletcher 1985) linearised by the analytic Jacobian of the posed body (HumanJacobian), and the merit (HumanBodyFitMerit) prices exactly what the rows do, so the model and the merit agree at every iterate (Nocedal and Wright 2006, section 18.5). The steps are taken by the shared penalty sequential convex loop (HumanSequentialConvex), its penalty steered at each iterate (Byrd, Nocedal and Waltz 2008), each convex model solved by the shared quadratic program (HumanQuadraticProgram) inside a trust region, with one second-order correction of a rejected step (Nocedal and Wright 2006, (15.36) and Algorithm 15.2).Each convex model is solved by the interior-point method (
HumanQuadraticProgram.interior, Mehrotra's predictor-corrector as HPIPM implements it, Frison and Diehl 2020), whose iteration count does not grow with the conditioning of the model's dense joint-space curvature, and by the operator splitting (OSQP) only where the interior method does not converge. Solved by the splitting first, a third of a rise's models ended unfinished at its iteration guard, short of the minimiser, and their steps were rejected by the trust region for the error (measured on the get-up's supine rise: 24 of 72 programs, 8.0 s of a 9.4 s fit, against 0.7 s by the interior method, which met the splitting's polished minimisers to 1e-9).