msim.control

Class Solver

java.lang.Object

vona.math.PDLS

msim.control.Solver

All Implemented Interfaces:

java.lang.Comparable<Solver>

@ThreadSafe
public class Solver
extends vona.math.PDLS
implements java.lang.Comparable<Solver>

Numeric solver for coupled DoF.

This is a specialization of the generic Prioritized Damped Least Squares solver PDLS, with implementations for problem update, residual, and Jacobian computation specific to the MSim model.

Solver Structure

All solvers are reconstructed in Controller.analyze() whenever the model topology has changed. Less conservative handling of structure change is TBD. Every Joint in the model is assigned to at most one Solver. A closure joint is constrained iff it Joint.hasAnyGoal(). Every constrained closure Joint is assigned to a Solver that handles it together with all other closure joints supported by the transitive overlap of its tree Joint.supports, its solve group. Locked tree Joints have no solver; unlocked tree Joints that are not in the Joint.support of any constrained closure Joint are open-chain and also have no Solver; if they have set goals (and are not locked) then they are moved directly to them as a stage in the update cycle. Every other tree Joint is assigned to the (single) Solver that handles (all of) the constrained closure Joints in its support.

Also see the Linkage and Controller class header doc.

Solver Action

The action of a Solver is to iteratively adjust the DoF of its tree Joints, called actuators, to reduce the magnitude of residuals that correspond to the DoI, DoF, potential energy, and limit penalty functions (if any) of its effectors. Each chain closure that is not Joint.Type.GENERAL induces one of the invariantEffectors corresponding to its Joint.RX_INVARIANT residual, and limitEffectors for each limited DoF, if any. Each chain closure also possibly induces entries in either

• lockEffectors corresponding to its lock residual, if locked
• targetEffectors if not locked and Joint.getHighestPrioritySoftGoal() is Joint.RX_TARGET
• postureEffectors if not locked and Joint.getHighestPrioritySoftGoal() is Joint.RX_POSTURE

Tree joints can only induce entries in potentialEffectors or targetEffectors.

Potential Energy Minimization

Potential energy minimization (quasistatic simulation) is enabled whenever any actuator or effector Joint.hasPotential(). A RootJoint's potential models gravitational potential energy acting on the mass of the attached Link. A normal Joint's potential models elastic potential energy stored in the joint. Both types are optional and are only enabled when the appropriate quantities have been set.

As long as there is at least one joint that Joint.hasPotential(), a single residual and corresponding Jacobain row are synthesized which represent the total potential energy of the system as the sum of the potential energy stored in each joint that Joint.hasPotential(). The residual of this effector is driven towards zero, thereby minimizing the L1 norm of the vector of joint potentials.

Any target or posture goals are ignored for Joints that also have potential energy goals (as the latter has higher priority, and in the current implementation must be set on all or none of the Joint's DoF).

Solver Priorities and Jacobian Structure

The effectors are assigned a hierarchical partial order which induces the PDLS priority levels. invariantEffectors always have highest priority and are all solved at the same priority, thus constituting PDLS priority level 0. limitEffectors, if any constitute the next priority level, followed by lockEffectors, then potentialEffectors, and then by "soft goal" effectors, which are partitioned in a two-hierarchy: all targetEffectors are higher priority than all postureEffectors, and each kind is further given a partial order by their Joint.softGoalPriority.

Each of the invariantEffectors produces Joint.getNumDoI() rows corresponding in order to Joint.getDoIIndices(int, int[]). Each limitEffectors produces rows corresponding to the DoF with set limits. Each lock or soft goal effector produces Joint.getNumGoalComponentsSet(int) rows corresponding in order to Joint.getDoFIndices(int, int[]), for lock effectors, or Joint.getSetGoalComponents(int), for soft goal effectors. There is at most one row for the potential energy of the entire system, as described above. Jacobian and residual rows for soft goal effectors are multiplied by the corresponding Joint.softGoalWeights.

The Jacobian columns correspond to the actuators, in order, with each actuator producing Joint.getNumDoF() columns corresponding in order to Joint.getDoFIndices(int, int[]).

Copyright (C) 2008 Marsette A. Vona, III

Author:

Marsette A. Vona, III

Nested Class Summary

Nested classes/interfaces inherited from class vona.math.PDLS

vona.math.PDLS.SolveStatus, vona.math.PDLS.TimeMetrics

Field Summary

Fields

Modifier and Type	Field and Description
protected java.util.Set<Joint>	actuators Actuating Joints (see header doc), in order, collected in init().
protected java.util.List<vona.math.PDLS.TimeMetrics>	allMetrics collection of all metrics
protected java.util.List<LinkageNode>	allSupportNodes All supporting nodes between (and including) the most ancestral actuators and any chainClosures.
protected vona.math.RX	aToEP Actuator mobility to effector parent frame or effector child frame to actuator mobility transform.
protected double[]	aToEPQ Actuator mobility to effector parent frame or effector child frame to actuator mobility quaternion/rotmatrix.
protected double[]	aToEPRM Actuator mobility to effector parent frame or effector child frame to actuator mobility quaternion/rotmatrix.
protected java.util.Set<Joint>	chainClosures Chain closure Joints (see header doc), in order, collected in Controller.analyze().
protected java.lang.String[]	constraintName cached names for indicated element
private static java.lang.String	cvsid
static double	DBG_CHECK_JACOBIAN_EPS slop factors allowed in compareAnalyticToNumeric(double, int, msim.model.Joint, msim.model.Joint, int, int)
static double	DBG_CHECK_JACOBIAN_POTENTIAL_EPS slop factors allowed in compareAnalyticToNumeric(double, int, msim.model.Joint, msim.model.Joint, int, int)
static boolean	dbgCheckJacobian Whether to compare analytic and numeric Jacobians for debugging.
static double	DEF_ALWAYS_DAMP_SIGMA initial value for all PDLS.alwaysDampSigma
static double	DEF_AUTO_LAMBDA_SLOWDOWN_THRESHOLD initial value for all PDLS.autoLambdaSlowdownThreshold
static double	DEF_AUTO_LAMBDA_SPEEDUP_THRESHOLD initial value for all PDLS.autoLambdaSpeedupThreshold
static double	DEF_AUTO_LAMBDA_THETA_DIFF_MAX_MAG_SCALE initial value for all PDLS.autoLambdaThetaDiffMaxMagScale
static double	DEF_DIVERGENCE_FACTOR initial value for all PDLS.divergenceFactor
static double	DEF_DIVERGENCE_TOL initial value for all PDLS.divergenceTol
static int	DEF_MAX_CONVERGENCE_ITERATIONS maximum number of convergence loop iterations per PDLS.solve()
static int	DEF_MAX_CONVERGENCE_MS maximum number of ms spent in convergence iterations PDLS.solve()
static double	DEF_RESIDUAL_MAX_MAG initial value for all PDLS.residualMaxMag
static double	DEF_RESIDUAL_TOL initial value for all PDLS.residualTol
static double	DEF_SLOWDOWN_LIMIT initial value for all PDLS.slowdownLimit
static double	DEF_SPEEDUP_LIMIT initial value for all PDLS.speedupLimit
static double	DEF_SVD_TOL initial value for all PDLS.svdTol
static double	DEF_THETA_DIFF_MAX_MAG initial value for PDLS.thetaDiffMaxMag
static double	DEF_THETA_DIFF_TOL initial value of PDLS.thetaDiffTol
protected double[]	dEGravitydAemc workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])
protected vona.math.RX	eCToA Actuator mobility to effector parent frame or effector child frame to actuator mobility transform.
protected double[]	eCToAQ Actuator mobility to effector parent frame or effector child frame to actuator mobility quaternion/rotmatrix.
static java.lang.String	FMT decimal printf format
static java.lang.String	FMT_S string printf format same width as FMT
static int	FMT_WIDTH field width for FMT
static java.util.Comparator<Joint>	GOAL_PRIORITY_COMPARATOR Orders Joints based on their Joint.softGoalPriority.
protected double[]	gravity temp storage for updateJacobian(int)
protected vona.math.PDLS.TimeMetrics	initMetrics metrics for each call to the indicated method
protected java.util.List<Joint>	invariantEffectors Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().
protected java.util.List<Joint>	limitEffectors Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().
protected double[]	limitFactor temp storage for updateJacobian(int)
protected double[]	linkCoM temp storage for updateJacobian(int)
protected java.util.List<Joint>	lockEffectors Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().
protected java.lang.String	name name of this Solver, auto-generated
protected static int	nextSolverNumber next unique Solver number
static double	NSJ_EM_H default perturbation magnitude for numeric partial derivative
static double	NSJ_TX_H default perturbation magnitude for numeric partial derivative
protected vona.math.RX	nsjA workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])
protected vona.math.RX	nsjAOrig workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])
protected double[]	nsjCol workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])
protected vona.math.RX	nsjE workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])
protected vona.math.RX	nsjEOrig workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])
protected double[]	nsjLPOrig workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])
protected double[]	numericJacobian numeric mirror of PDLS.jacobian when dbgCheckJacobian
protected int	numInvariantLevels total number of priority levels in indicated part of Jacobian
protected int	numInvariantRows total number of rows in indicated part of Jacobian
protected int	numLimitLevels total number of priority levels in indicated part of Jacobian
protected int	numLimitRows total number of rows in indicated part of Jacobian
protected int	numLockLevels total number of priority levels in indicated part of Jacobian
protected int	numLockRows total number of rows in indicated part of Jacobian
protected int	numPostureLevels total number of priority levels in indicated part of Jacobian
protected int	numPostureRows total number of rows in indicated part of Jacobian
protected int	numPotentialLevels total number of priority levels in indicated part of Jacobian
protected int	numPotentialRows total number of rows in indicated part of Jacobian
protected int	numTargetLevels total number of priority levels in indicated part of Jacobian
protected int	numTargetRows total number of rows in indicated part of Jacobian
protected java.util.List<Joint>	postureEffectors Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().
protected java.util.List<Joint>	potentialEffectors Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().
protected java.lang.String[]	priorityLevelName cached names for indicated element
protected int	solveRound Solve ordering, see compareTo(msim.control.Solver)
protected java.util.List<Joint>	targetEffectors Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().
protected double[]	tempM temp storage for updateJacobian(int)
protected double[]	tempM2 temp storage for updateJacobian(int)
protected double[]	tempV temp storage for updateJacobian(int)
protected double[]	tempV2 temp storage for updateJacobian(int)
protected java.util.List<Joint>	treeEffectors Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().
protected vona.math.PDLS.TimeMetrics	updateActuatorsMetrics metrics for each call to the indicated method
protected vona.math.PDLS.TimeMetrics	updateJacobianMetrics metrics for each call to the indicated method
protected vona.math.PDLS.TimeMetrics	updatePostureVariationMetrics metrics for each call to the indicated method
protected vona.math.PDLS.TimeMetrics	updateResidualMetrics metrics for each call to the indicated method
protected vona.math.PDLS.TimeMetrics	updateThetaMetrics metrics for each call to the indicated method
protected java.lang.String[]	variableName cached names for indicated element

Fields inherited from class vona.math.PDLS

abortOnDiverge, abortOnInterrupt, abortOnSVDError, adaptiveStepSizeEnabled, allPriorityMetricNames, allPriorityMetrics, allSolveMetrics, alwaysDampSigma, alwaysUpdateWholeProblem, autoLambdaSlowdownThreshold, autoLambdaSpeedupThreshold, autoLambdaThetaDiffMaxMagScale, C_FMT, checkConvergenceMetrics, clampResidualByComponent, clampThetaDiffByComponent, compensatedResidual, compensatedResidualMetrics, convergenceCycleMetrics, convergenceEnabled, dampedPseudoinverse, dampingEnabled, DBG_ALL, DBG_ALL_BUT_MATRICES, DBG_CONTROL, DBG_LAMBDA, DBG_LIMITING, DBG_MATRICES, DBG_POSTURE, DBG_RESIDUAL, DBG_STEPSIZE, DBG_STEPSIZE_AND_LAMBDA, DBG_SVD, DBG_THETA, DBG_THETA_AND_RESIDUAL, dbgFlags, dbgStream, DEF_EPS, DEF_MIN_DIVERGABLE_LEVEL, DEF_NEVER_DAMP_SIGMA, DEF_SLOWDOWN_FACTOR, DEF_SLOWDOWN_THRESHOLD, DEF_SPEEDUP_FACTOR, DEF_SPEEDUP_THRESHOLD, DEF_SVD_IMPLS, divergenceFactor, divergenceTol, eps, globalMetrics, globalMetricsEnabled, jacobian, lastLevelLambda, lastResidualMag, lastSolveStatus, lastThetaDiffMag, levelLambda, levelLambdaMetrics, levelThetaDiffMetrics, limitingCycleMetrics, limitingEnabled, limitingVariation, lockingMetrics, maxConvergenceIterations, maxConvergenceMS, metricsEnabled, minDivergableLevel, neverDampSigma, nullspaceProjector, nullspaceProjectorMetrics, numConstraints, numPriorities, numVariables, postureVariation, postureVariationEnabled, postureVariationMetrics, priorityCycleMetrics, priorityLevelsEnabled, priorityPartition, pseudoinverse, pseudoinverseMetrics, reallocMetrics, residual, residualMaxMag, residualTol, residualWas, restrictedJacobian, restrictedJacobianMetrics, restrictedJacobianRank, singleStep, slowdownFactor, slowdownLimit, slowdownThreshold, solveCycleMetrics, speedupFactor, speedupLimit, speedupThreshold, svdA, svdImpl, svdMetrics, svdS, svdStatus, svdTol, svdU, svdVT, theta, thetaDiff, thetaDiffClampingEnabled, thetaDiffMaxMag, thetaDiffMetrics, thetaDiffTol, thetaLimitLower, thetaLimitUpper, thetaWas, updateProblemMetrics

Constructor Summary

Constructors

Constructor and Description
Solver() sets name

Method Summary

Methods

Modifier and Type	Method and Description
protected void	compareAnalyticToNumeric(double av, int ni, Joint e, Joint a, int ec, int ac) Helper to compare analytic and numeric Jacobian components.
int	compareTo(Solver o) Compares this Solver with the specified Solver for order.
void	dumpActuators() dumpActuators(PrintStream) to System.out
void	dumpActuators(java.io.PrintStream s) dumps actuators, in order
void	dumpChainClosures() dumpChainClosures(PrintStream) to System.out
void	dumpChainClosures(java.io.PrintStream s) dumps chainClosures, in order
void	dumpDetails() dumpDetails(java.io.PrintStream) to System.out
void	dumpDetails(java.io.PrintStream s) prepends paths of all joints to dumpNumericDetails(java.io.PrintStream)
void	dumpEffectors() dumpEffectors(PrintStream) to System.out
void	dumpEffectors(int which) dumpEffectors(int, PrintStream) to System.out
void	dumpEffectors(int which, java.io.PrintStream s) Dump effectors of the indicated type.
void	dumpEffectors(java.io.PrintStream s) dump all effectors
void	dumpGoalEffectors(int which) dumpGoalEffectors(int, PrintStream) to System.out
void	dumpGoalEffectors(int which, java.io.PrintStream s) Dumps all Joint.RX_TARGET, Joint.RX_POSTURE, or Joint.G_LIMIT effectors.
void	dumpInvariantEffectors() dumpInvariantEffectors(PrintStream) to System.out
void	dumpInvariantEffectors(java.io.PrintStream s) dumps all invariantEffectors, in order
void	dumpLimitEffectors() dumpLimitEffectors(PrintStream) to System.out
void	dumpLimitEffectors(java.io.PrintStream s) dump all limitEffectors, in order
void	dumpLockEffectors() dumpLockEffectors(PrintStream) to System.out
void	dumpLockEffectors(java.io.PrintStream s) dumps all lockEffectors, in order
void	dumpNumericDetails() dumpNumericDetails(PrintStream) to System.out
void	dumpNumericDetails(java.io.PrintStream s) Adds Jacobian structure info to PDLS.dumpNumericDetails(PrintStream).
void	dumpPostureEffectors() dumpPostureEffectors(PrintStream) to System.out
void	dumpPostureEffectors(java.io.PrintStream s) dump all postureEffectors, in order
void	dumpPotentialEffectors() dumpPotentialEffectors(PrintStream) to System.out
void	dumpPotentialEffectors(java.io.PrintStream s) Dumps all potentialEffectors.
void	dumpStats(java.io.PrintStream s) Prepends counts and internal metrics to superclass dump.
void	dumpStructure(java.io.PrintStream s) Prepends name to superclass dump.
void	dumpSubJacobian(Joint effector, Joint actuator) dumpSubJacobian(Joint, Joint, PrintStream) to Systemout.
void	dumpSubJacobian(Joint effector, Joint actuator, java.io.PrintStream s) Dumps actuator theta, effector residual(s), and intersection of jacobian rows and cols for given effector, actuator pair.
void	dumpTargetEffectors() dumpTargetEffectors(PrintStream) to System.out
void	dumpTargetEffectors(java.io.PrintStream s) dump all targetEffectors, in order
java.lang.String	getConstraintName(int constraint, int width) Get a semantic human-readable name for a constraint.
int	getCumulativeNumEffectors(int which) Get the total number of effectors of the given type and higher priority types.
java.lang.String	getName() returns name
int	getNumActuators() get the number of actuating Joints
int	getNumChainClosures() get the number of chain closure Joints
int	getNumConstraints() get the number of constraints
int	getNumEffectors() get the total number of effectors
int	getNumEffectors(int which) Get the number of effectors of the indicated type.
int	getNumLevels() get the total number of levels
int	getNumLevels(int which) Get the number of solve levels of the indicated type.
int	getNumRows() get the total number of rows
int	getNumRows(int which) Get the number of Jacobian rows of the indicated type.
int	getNumVariables() get the number of variables
java.lang.String	getPriorityLevelName(int level) Get a semantic human-readable name for a priority level.
java.lang.String	getVariableName(int variable, int width) Get a semantic human-readable name for a variable.
void	init() Collect and cons all other state given chainClosures.
protected vona.math.PDLS.TimeMetrics	makeMetrics(java.lang.String name) This impl chains to PDLS.makeMetrics(String, String) and then adds the new metrics object to allMetrics.
static double[]	mmult334x41(double[] r, double[][] p, double[] q) computes tensor product r [3x3] = (p [[3x3]x4])(q [4x1])
static double[]	mmult33Tx31(double[] r, double[] p, double[] q) computes row-major r = (p^T)(q)
static double[]	mmult33x31(double[] r, double[] p, double[] q) computes row-major r = (p)(q)
static double[]	mmult33x33T(double[] r, double[] p, double[] q) computes row-major r = (p)(q^T)
static double[]	mmult44x41(double[] r, double[] p, double[] q) computes row-major r = (p)(q)
protected void	numericSubJacobian(double[] jacobian, Joint e, Joint a, boolean aInverted, int goal, vona.math.RX eCToA, vona.math.RX aToEP, int asc, int[] aoi, int isr, int[] eii, int osr, int[] eoi) Compute partial derivatives of effector e TXEM components with respect to actuator a TXEM components by numeric perturbation.
protected void	numericSubJacobian(Joint e, Joint a, boolean aInverted, int goal, vona.math.RX eCToA, vona.math.RX aToEP, int asc, int[] aoi, int isr, int[] eii, int osr, int[] eoi) Covers numericSubJacobian(double[], Joint, Joint, boolean, int, RX, RX, int, int[], int, int[], int, int[]), stores results in numericJacobian.
static double[]	qmmQmmStar(double[] r, double[] p, double[] q) Computes row-major r = PQ^* where P and Q^* are quat mult matrices corresponding to left- and right-multiply by quats p and q, respectively.
protected void	recomputeClosures() Joint.recomputeInverse(int, boolean) all chainClosures
void	resetStats() chains to superclass impl and then resets additional stats
void	resetStepsize() Reset PDLS.residualMaxMag, default impl sets nan.
java.lang.String	toString() getName()
void	updateActuators() Sets actuators DoF to the current PDLS.theta.
protected void	updateCMTs() LinkageNode.updateCMTs() allSupportNodes in order, then all chainClosures.
void	updateJacobian(int convergenceIteration) Updates PDLS.jacobian.
void	updatePostureVariation(int convergenceIteration) Sets PDLS.postureVariation for actuators to the shortest TXEM distance to their Joint.RX_POSTURE goal, if any.
void	updateResidual(int convergenceIteration) Computes residual; constraint order given in header doc.
void	updateTheta(int convergenceIteration) On first iteration copies current actuators DoF to PDLS.theta, chains to super on later iterations (which just adds PDLS.thetaDiff to PDLS.theta).

Methods inherited from class vona.math.PDLS

checkConvergence, clamp, clamp, clearDebugFlags, dbgEnabled, dump, dumpForLevelsMaybe, dumpForLevelsMaybe, dumpForLevelsMaybe, dumpGlobalStats, dumpGlobalStats, dumpJacobian, dumpJacobian, dumpMatrix, dumpStats, dumpStructure, getAlwaysDampSigma, getAutoLambdaSlowdownThreshold, getAutoLambdaSpeedupThreshold, getAutoLambdaThetaDiffMaxMagScale, getDivergenceFactor, getDivergenceTol, getLevelLambda, getLevelParam, getLevelParam, getMaxConvergenceIterations, getMaxConvergenceMS, getMaxLevelSize, getNeverDampSigma, getResidualMaxMag, getResidualTol, getSlowdownFactor, getSlowdownLimit, getSlowdownThreshold, getSpeedupFactor, getSpeedupLimit, getSpeedupThreshold, getSVDTol, getThetaDiffMaxMag, getThetaDiffTol, grow, grow, mag, mag, main, makeMetrics, makePriorityMetrics, makeSolveMetrics, markEndMaybe, markEndMaybe, markStartMaybe, markStartMaybe, resetGlobalStats, resizePriorityMetrics, revertTheta, saveCurrentResidual, setAlwaysDampSigma, setAutoLambdaSlowdownThreshold, setAutoLambdaSpeedupThreshold, setAutoLambdaThetaDiffMaxMagScale, setDebugFlags, setDivergenceFactor, setDivergenceTol, setLambda, setLevelLambda, setMaxConvergenceIterations, setMaxConvergenceMS, setNeverDampSigma, setResidualMaxMag, setResidualMaxMag, setResidualTol, setResidualTol, setSlowdownFactor, setSlowdownLimit, setSlowdownLimit, setSlowdownThreshold, setSpeedupFactor, setSpeedupLimit, setSpeedupLimit, setSpeedupThreshold, setSVDTol, setThetaDiffMaxMag, setThetaDiffTol, solve, updateLambda, updateProblem

Methods inherited from class java.lang.Object

clone, equals, finalize, getClass, hashCode, notify, notifyAll, wait, wait, wait

Field Detail

cvsid

private static final java.lang.String cvsid

See Also:

Constant Field Values

DEF_THETA_DIFF_MAX_MAG

public static final double DEF_THETA_DIFF_MAX_MAG

initial value for PDLS.thetaDiffMaxMag

See Also:

Constant Field Values

DEF_THETA_DIFF_TOL

public static final double DEF_THETA_DIFF_TOL

initial value of PDLS.thetaDiffTol

See Also:

Constant Field Values

DEF_RESIDUAL_MAX_MAG

public static final double DEF_RESIDUAL_MAX_MAG

initial value for all PDLS.residualMaxMag

See Also:

Constant Field Values

DEF_AUTO_LAMBDA_SLOWDOWN_THRESHOLD

public static final double DEF_AUTO_LAMBDA_SLOWDOWN_THRESHOLD

initial value for all PDLS.autoLambdaSlowdownThreshold

See Also:

Constant Field Values

DEF_AUTO_LAMBDA_SPEEDUP_THRESHOLD

public static final double DEF_AUTO_LAMBDA_SPEEDUP_THRESHOLD

initial value for all PDLS.autoLambdaSpeedupThreshold

See Also:

Constant Field Values

DEF_SLOWDOWN_LIMIT

public static final double DEF_SLOWDOWN_LIMIT

initial value for all PDLS.slowdownLimit

See Also:

Constant Field Values

DEF_SPEEDUP_LIMIT

public static final double DEF_SPEEDUP_LIMIT

initial value for all PDLS.speedupLimit

See Also:

Constant Field Values

DEF_RESIDUAL_TOL

public static final double DEF_RESIDUAL_TOL

initial value for all PDLS.residualTol

See Also:

Constant Field Values

DEF_SVD_TOL

public static final double DEF_SVD_TOL

initial value for all PDLS.svdTol

See Also:

Constant Field Values

DEF_ALWAYS_DAMP_SIGMA

public static final double DEF_ALWAYS_DAMP_SIGMA

initial value for all PDLS.alwaysDampSigma

See Also:

Constant Field Values

DEF_AUTO_LAMBDA_THETA_DIFF_MAX_MAG_SCALE

public static final double DEF_AUTO_LAMBDA_THETA_DIFF_MAX_MAG_SCALE

initial value for all PDLS.autoLambdaThetaDiffMaxMagScale

See Also:

Constant Field Values

DEF_DIVERGENCE_FACTOR

public static final double DEF_DIVERGENCE_FACTOR

initial value for all PDLS.divergenceFactor

See Also:

Constant Field Values

DEF_DIVERGENCE_TOL

public static final double DEF_DIVERGENCE_TOL

initial value for all PDLS.divergenceTol

See Also:

Constant Field Values

DEF_MAX_CONVERGENCE_ITERATIONS

public static final int DEF_MAX_CONVERGENCE_ITERATIONS

maximum number of convergence loop iterations per PDLS.solve()

See Also:

Constant Field Values

DEF_MAX_CONVERGENCE_MS

public static final int DEF_MAX_CONVERGENCE_MS

maximum number of ms spent in convergence iterations PDLS.solve()

See Also:

Constant Field Values

FMT_WIDTH

public static final int FMT_WIDTH

field width for FMT

See Also:

Constant Field Values

FMT

public static final java.lang.String FMT

decimal printf format

See Also:

Constant Field Values

FMT_S

public static final java.lang.String FMT_S

string printf format same width as FMT

See Also:

Constant Field Values

NSJ_TX_H

public static final double NSJ_TX_H

default perturbation magnitude for numeric partial derivative

See Also:

Constant Field Values

NSJ_EM_H

public static final double NSJ_EM_H

default perturbation magnitude for numeric partial derivative

See Also:

Constant Field Values

DBG_CHECK_JACOBIAN_EPS

public static final double DBG_CHECK_JACOBIAN_EPS

slop factors allowed in compareAnalyticToNumeric(double, int, msim.model.Joint, msim.model.Joint, int, int)

See Also:

Constant Field Values

DBG_CHECK_JACOBIAN_POTENTIAL_EPS

public static final double DBG_CHECK_JACOBIAN_POTENTIAL_EPS

slop factors allowed in compareAnalyticToNumeric(double, int, msim.model.Joint, msim.model.Joint, int, int)

See Also:

Constant Field Values

dbgCheckJacobian

public static final boolean dbgCheckJacobian

Whether to compare analytic and numeric Jacobians for debugging.

Current impl requires this to be final b/c numericJacobian is not allocated if false.

See Also:

Constant Field Values

GOAL_PRIORITY_COMPARATOR

public static final java.util.Comparator<Joint> GOAL_PRIORITY_COMPARATOR

Orders Joints based on their Joint.softGoalPriority.

Note this only suffices to order Joints which all have the same type of goal.

A collection of Joints sorted with this comparitor will end up in decreasing priority order.

nextSolverNumber

protected static int nextSolverNumber

next unique Solver number

name

protected final java.lang.String name

name of this Solver, auto-generated

actuators

protected final java.util.Set<Joint> actuators

Actuating Joints (see header doc), in order, collected in init().

chainClosures

protected final java.util.Set<Joint> chainClosures

Chain closure Joints (see header doc), in order, collected in Controller.analyze().

allSupportNodes

protected final java.util.List<LinkageNode> allSupportNodes

All supporting nodes between (and including) the most ancestral actuators and any chainClosures.

Sorted in LinkageNode.depth order.

Note that this can be a superset of actuators because it also includes Links and locked tree Joints. It is used to optimize updateCMTs().

solveRound

protected int solveRound

Solve ordering, see compareTo(msim.control.Solver)

invariantEffectors

protected final java.util.List<Joint> invariantEffectors

Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().

limitEffectors

protected final java.util.List<Joint> limitEffectors

Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().

lockEffectors

protected final java.util.List<Joint> lockEffectors

Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().

potentialEffectors

protected final java.util.List<Joint> potentialEffectors

Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().

targetEffectors

protected final java.util.List<Joint> targetEffectors

Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().

postureEffectors

protected final java.util.List<Joint> postureEffectors

Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().

treeEffectors

protected final java.util.List<Joint> treeEffectors

Joints corresponding to the indicated kind of effector (see header doc), in order, collected in init().

numInvariantRows

protected int numInvariantRows

total number of rows in indicated part of Jacobian

numLimitRows

protected int numLimitRows

total number of rows in indicated part of Jacobian

numLockRows

protected int numLockRows

total number of rows in indicated part of Jacobian

numPotentialRows

protected int numPotentialRows

total number of rows in indicated part of Jacobian

numTargetRows

protected int numTargetRows

total number of rows in indicated part of Jacobian

numPostureRows

protected int numPostureRows

total number of rows in indicated part of Jacobian

numInvariantLevels

protected int numInvariantLevels

total number of priority levels in indicated part of Jacobian

numLimitLevels

protected int numLimitLevels

total number of priority levels in indicated part of Jacobian

numLockLevels

protected int numLockLevels

total number of priority levels in indicated part of Jacobian

numPotentialLevels

protected int numPotentialLevels

total number of priority levels in indicated part of Jacobian

numTargetLevels

protected int numTargetLevels

total number of priority levels in indicated part of Jacobian

numPostureLevels

protected int numPostureLevels

total number of priority levels in indicated part of Jacobian

aToEP

protected final vona.math.RX aToEP

Actuator mobility to effector parent frame or effector child frame to actuator mobility transform.

Used in updateJacobian(int).

eCToA

protected final vona.math.RX eCToA

Actuator mobility to effector parent frame or effector child frame to actuator mobility transform.

Used in updateJacobian(int).

aToEPRM

protected final double[] aToEPRM

Actuator mobility to effector parent frame or effector child frame to actuator mobility quaternion/rotmatrix.

Used in updateJacobian(int).

aToEPQ

protected final double[] aToEPQ

Actuator mobility to effector parent frame or effector child frame to actuator mobility quaternion/rotmatrix.

Used in updateJacobian(int).

eCToAQ

protected final double[] eCToAQ

Actuator mobility to effector parent frame or effector child frame to actuator mobility quaternion/rotmatrix.

Used in updateJacobian(int).

tempM

protected final double[] tempM

temp storage for updateJacobian(int)

tempM2

protected final double[] tempM2

temp storage for updateJacobian(int)

tempV

protected final double[] tempV

temp storage for updateJacobian(int)

tempV2

protected final double[] tempV2

temp storage for updateJacobian(int)

linkCoM

protected final double[] linkCoM

temp storage for updateJacobian(int)

gravity

protected final double[] gravity

temp storage for updateJacobian(int)

limitFactor

protected final double[] limitFactor

temp storage for updateJacobian(int)

nsjA

protected final vona.math.RX nsjA

workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])

nsjAOrig

protected final vona.math.RX nsjAOrig

workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])

nsjE

protected final vona.math.RX nsjE

workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])

nsjEOrig

protected final vona.math.RX nsjEOrig

workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])

nsjCol

protected final double[] nsjCol

workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])

dEGravitydAemc

protected final double[] dEGravitydAemc

workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])

nsjLPOrig

protected final double[] nsjLPOrig

workspace for numericSubJacobian(double[], msim.model.Joint, msim.model.Joint, boolean, int, vona.math.RX, vona.math.RX, int, int[], int, int[], int, int[])

numericJacobian

protected double[] numericJacobian

numeric mirror of PDLS.jacobian when dbgCheckJacobian

variableName

protected java.lang.String[] variableName

cached names for indicated element

constraintName

protected java.lang.String[] constraintName

cached names for indicated element

priorityLevelName

protected java.lang.String[] priorityLevelName

cached names for indicated element

allMetrics

protected final java.util.List<vona.math.PDLS.TimeMetrics> allMetrics

collection of all metrics

initMetrics

protected final vona.math.PDLS.TimeMetrics initMetrics

metrics for each call to the indicated method

updateResidualMetrics

protected final vona.math.PDLS.TimeMetrics updateResidualMetrics

metrics for each call to the indicated method

updateThetaMetrics

protected final vona.math.PDLS.TimeMetrics updateThetaMetrics

metrics for each call to the indicated method

updateJacobianMetrics

protected final vona.math.PDLS.TimeMetrics updateJacobianMetrics

metrics for each call to the indicated method

updatePostureVariationMetrics

protected final vona.math.PDLS.TimeMetrics updatePostureVariationMetrics

metrics for each call to the indicated method

updateActuatorsMetrics

protected final vona.math.PDLS.TimeMetrics updateActuatorsMetrics

metrics for each call to the indicated method

Constructor Detail

Solver

public Solver()

sets name

Method Detail

makeMetrics

protected vona.math.PDLS.TimeMetrics makeMetrics(java.lang.String name)

This impl chains to PDLS.makeMetrics(String, String) and then adds the new metrics object to allMetrics.

init

public void init()

Collect and cons all other state given chainClosures.

Time complexity is linear in the sum of the sizes of all the chainClosures Joint.supports.

compareTo

public int compareTo(Solver o)

Compares this Solver with the specified Solver for order.

Specified by:

compareTo in interface java.lang.Comparable<Solver>

Returns:

a negative integer, zero, or a positive integer as solveRound is less than, equal to, or greater than o.solveRound

resetStepsize

public void resetStepsize()

Reset PDLS.residualMaxMag, default impl sets nan.

this impl sets DEF_RESIDUAL_MAX_MAG.

Overrides:

resetStepsize in class vona.math.PDLS

updateResidual

public void updateResidual(int convergenceIteration)

Computes residual; constraint order given in header doc.

On the first iteration this does not change any model state, it just asks all the effectors for their residuals in their current state. In later iterations it first does updateActuators(), updateCMTs(), and recomputeClosures().

Overrides:

updateResidual in class vona.math.PDLS

updateCMTs

protected void updateCMTs()

LinkageNode.updateCMTs() allSupportNodes in order, then all chainClosures.

recomputeClosures

protected void recomputeClosures()

Joint.recomputeInverse(int, boolean) all chainClosures

updateTheta

public void updateTheta(int convergenceIteration)

On first iteration copies current actuators DoF to PDLS.theta, chains to super on later iterations (which just adds PDLS.thetaDiff to PDLS.theta).

Overrides:

updateTheta in class vona.math.PDLS

updateJacobian

public void updateJacobian(int convergenceIteration)

Updates PDLS.jacobian.

See class header doc for Jacobian layout.

The asymptotic time complexity of Jacobian computation is dominated by the PDLS.numVariables*PDLS.numConstraints time it takes to initially zero the whole matrix. TBD: this can be reduced to the sum of the supports of all effectors by just zeroing the parts that might have been set non-zero previously.

Overrides:

updateJacobian in class vona.math.PDLS

mmult33x33T

public static final double[] mmult33x33T(double[] r,
                   double[] p,
                   double[] q)

computes row-major r = (p)(q^T)

mmult33x31

public static final double[] mmult33x31(double[] r,
                  double[] p,
                  double[] q)

computes row-major r = (p)(q)

mmult33Tx31

public static final double[] mmult33Tx31(double[] r,
                   double[] p,
                   double[] q)

computes row-major r = (p^T)(q)

mmult44x41

public static final double[] mmult44x41(double[] r,
                  double[] p,
                  double[] q)

computes row-major r = (p)(q)

mmult334x41

public static final double[] mmult334x41(double[] r,
                   double[][] p,
                   double[] q)

computes tensor product r [3x3] = (p [[3x3]x4])(q [4x1])

qmmQmmStar

public static final double[] qmmQmmStar(double[] r,
                  double[] p,
                  double[] q)

Computes row-major r = PQ^* where P and Q^* are quat mult matrices corresponding to left- and right-multiply by quats p and q, respectively.

numericSubJacobian

protected final void numericSubJacobian(double[] jacobian,
                      Joint e,
                      Joint a,
                      boolean aInverted,
                      int goal,
                      vona.math.RX eCToA,
                      vona.math.RX aToEP,
                      int asc,
                      int[] aoi,
                      int isr,
                      int[] eii,
                      int osr,
                      int[] eoi)

Compute partial derivatives of effector e TXEM components with respect to actuator a TXEM components by numeric perturbation.

Parameters:

jacobian - the destination

e - the effector

a - the actuator, a=e for tree joint effectors

aInverted - whether a is topologically inverted wrt e

goal - the limit or potential goal if any, else negative

eCToA - transform taking e child mobility frame to a mobility frame, or null for tree joint effectors (identity)

aToEP - transform taking a mobility frame to e parent mobility frame, or null for tree joint effectors (identity)

asc - actuator start column in PDLS.jacobian

aoi - TXEM indices of actuator DoF, null if none

isr - effector DoI start row in PDLS.jacobian

eii - effector DoI TXEM indices, null if none

osr - effector DoF start row in PDLS.jacobian

eoi - effector DoF TXEM indices, null if none

numericSubJacobian

protected final void numericSubJacobian(Joint e,
                      Joint a,
                      boolean aInverted,
                      int goal,
                      vona.math.RX eCToA,
                      vona.math.RX aToEP,
                      int asc,
                      int[] aoi,
                      int isr,
                      int[] eii,
                      int osr,
                      int[] eoi)

Covers numericSubJacobian(double[], Joint, Joint, boolean, int, RX, RX, int, int[], int, int[], int, int[]), stores results in numericJacobian.

compareAnalyticToNumeric

protected final void compareAnalyticToNumeric(double av,
                            int ni,
                            Joint e,
                            Joint a,
                            int ec,
                            int ac)

Helper to compare analytic and numeric Jacobian components.

This is used when debugging to validate the computations against each other.

updatePostureVariation

public void updatePostureVariation(int convergenceIteration)

Sets PDLS.postureVariation for actuators to the shortest TXEM distance to their Joint.RX_POSTURE goal, if any.

Overrides:

updatePostureVariation in class vona.math.PDLS

updateActuators

public void updateActuators()

Sets actuators DoF to the current PDLS.theta.

dumpStructure

public void dumpStructure(java.io.PrintStream s)

Prepends name to superclass dump.

Overrides:

dumpStructure in class vona.math.PDLS

dumpStats

public void dumpStats(java.io.PrintStream s)

Prepends counts and internal metrics to superclass dump.

Overrides:

dumpStats in class vona.math.PDLS

resetStats

public void resetStats()

chains to superclass impl and then resets additional stats

Overrides:

resetStats in class vona.math.PDLS

dumpDetails

public void dumpDetails(java.io.PrintStream s)

prepends paths of all joints to dumpNumericDetails(java.io.PrintStream)

dumpDetails

public void dumpDetails()

dumpDetails(java.io.PrintStream) to System.out

dumpNumericDetails

public void dumpNumericDetails(java.io.PrintStream s)

Adds Jacobian structure info to PDLS.dumpNumericDetails(PrintStream).

Overrides:

dumpNumericDetails in class vona.math.PDLS

dumpNumericDetails

public void dumpNumericDetails()

dumpNumericDetails(PrintStream) to System.out

Overrides:

dumpNumericDetails in class vona.math.PDLS

getName

public java.lang.String getName()

returns name

toString

public java.lang.String toString()

getName()

Overrides:

toString in class java.lang.Object

getNumVariables

public int getNumVariables()

get the number of variables

getNumConstraints

public int getNumConstraints()

get the number of constraints

getNumActuators

public int getNumActuators()

get the number of actuating Joints

getNumChainClosures

public int getNumChainClosures()

get the number of chain closure Joints

getNumEffectors

public int getNumEffectors(int which)

Get the number of effectors of the indicated type.

getNumEffectors

public int getNumEffectors()

get the total number of effectors

getCumulativeNumEffectors

public int getCumulativeNumEffectors(int which)

Get the total number of effectors of the given type and higher priority types.

getNumLevels

public int getNumLevels(int which)

Get the number of solve levels of the indicated type.

getNumLevels

public int getNumLevels()

get the total number of levels

getNumRows

public int getNumRows(int which)

Get the number of Jacobian rows of the indicated type.

getNumRows

public int getNumRows()

get the total number of rows

dumpChainClosures

public void dumpChainClosures(java.io.PrintStream s)

dumps chainClosures, in order

dumpChainClosures

public void dumpChainClosures()

dumpChainClosures(PrintStream) to System.out

dumpActuators

public void dumpActuators(java.io.PrintStream s)

dumps actuators, in order

dumpActuators

public void dumpActuators()

dumpActuators(PrintStream) to System.out

dumpInvariantEffectors

public void dumpInvariantEffectors(java.io.PrintStream s)

dumps all invariantEffectors, in order

dumpInvariantEffectors

public void dumpInvariantEffectors()

dumpInvariantEffectors(PrintStream) to System.out

dumpLockEffectors

public void dumpLockEffectors(java.io.PrintStream s)

dumps all lockEffectors, in order

dumpLockEffectors

public void dumpLockEffectors()

dumpLockEffectors(PrintStream) to System.out

dumpPotentialEffectors

public void dumpPotentialEffectors(java.io.PrintStream s)

Dumps all potentialEffectors.

Note that in the current impl there is at most one as the potential energies are all summed into a single effector in order to minimize their L1 norm.

dumpPotentialEffectors

public void dumpPotentialEffectors()

dumpPotentialEffectors(PrintStream) to System.out

dumpGoalEffectors

public void dumpGoalEffectors(int which,
                     java.io.PrintStream s)

Dumps all Joint.RX_TARGET, Joint.RX_POSTURE, or Joint.G_LIMIT effectors.

dumpGoalEffectors

public void dumpGoalEffectors(int which)

dumpGoalEffectors(int, PrintStream) to System.out

dumpTargetEffectors

public void dumpTargetEffectors(java.io.PrintStream s)

dump all targetEffectors, in order

dumpTargetEffectors

public void dumpTargetEffectors()

dumpTargetEffectors(PrintStream) to System.out

dumpPostureEffectors

public void dumpPostureEffectors(java.io.PrintStream s)

dump all postureEffectors, in order

dumpPostureEffectors

public void dumpPostureEffectors()

dumpPostureEffectors(PrintStream) to System.out

dumpLimitEffectors

public void dumpLimitEffectors(java.io.PrintStream s)

dump all limitEffectors, in order

dumpLimitEffectors

public void dumpLimitEffectors()

dumpLimitEffectors(PrintStream) to System.out

dumpEffectors

public void dumpEffectors(int which,
                 java.io.PrintStream s)

Dump effectors of the indicated type.

dumpEffectors

public void dumpEffectors(int which)

dumpEffectors(int, PrintStream) to System.out

dumpEffectors

public void dumpEffectors(java.io.PrintStream s)

dump all effectors

dumpEffectors

public void dumpEffectors()

dumpEffectors(PrintStream) to System.out

dumpSubJacobian

public void dumpSubJacobian(Joint effector,
                   Joint actuator,
                   java.io.PrintStream s)

Dumps actuator theta, effector residual(s), and intersection of jacobian rows and cols for given effector, actuator pair.

dumpSubJacobian

public void dumpSubJacobian(Joint effector,
                   Joint actuator)

dumpSubJacobian(Joint, Joint, PrintStream) to Systemout.

getVariableName

public java.lang.String getVariableName(int variable,
                               int width)

Get a semantic human-readable name for a variable.

Default impl returns the variable number.

This impl returns the cached variableName.

Overrides:

getVariableName in class vona.math.PDLS

width - the requested maximum width of the name

getConstraintName

public java.lang.String getConstraintName(int constraint,
                                 int width)

Get a semantic human-readable name for a constraint.

Default impl returns the constraint number.

This impl returns the cached constraintName.

Overrides:

getConstraintName in class vona.math.PDLS

width - the requested maximum width of the name

getPriorityLevelName

public java.lang.String getPriorityLevelName(int level)

Get a semantic human-readable name for a priority level.

Default impl returns the level number.

This impl returns the cached priorityLevelName.

Overrides:

getPriorityLevelName in class vona.math.PDLS