artisynth.core.probes

Class IKSolver

java.lang.Object

artisynth.core.probes.IKSolver

All Implemented Interfaces:

PostScannable, Scannable

public class IKSolver
extends java.lang.Object
implements PostScannable

Performs inverse kinematics computations to compute rigid body positions from marker data, while enforcing constraints between the bodies imposed by joints and other body connectors. In multibody biomechanics, can be used to filter marker data to determine feasible body trajectories, prior to inverse dynamic calculations.

Constructor Summary

Constructors

Constructor and Description
IKSolver() Creates an empty IKSolver.
IKSolver(IKSolver solver) Creates an IKSolver which is a copy of another IKSolver.
IKSolver(MechModel mech, java.util.Collection<FrameMarker> mkrs) Creates an IKSolver for a set of markers contained within a prescribed MechModel.
IKSolver(MechModel mech, java.util.Collection<FrameMarker> mkrs, VectorNd weights) Creates an IKSolver for a set of markers contained within a prescribed MechModel.

Method Summary

Modifier and Type	Method and Description
double	avgNumIterations() Return the average number of iterations per solve
void	clearSolveCounts() Clears the cummulative solve and iteration counts.
PositionInputProbe	createBodyPoseProbe(java.lang.String name, NumericProbeBase targProbe, RotationRep rotRep, double interval) Creates an input probe for controlling the poses of the bodies associated with this solver.
PositionInputProbe	createBodyPoseProbe(java.lang.String name, java.lang.String targDataFilePath, RotationRep rotRep, double interval) Creates an input probe for controlling the poses of the bodies associated with this solver.
PositionInputProbe	createMarkerPositionProbe(java.lang.String name, NumericProbeBase targProbe, double interval) Creates an input probe for controlling the positions of the markers associated with this solver.
PositionInputProbe	createMarkerPositionProbe(java.lang.String name, java.lang.String targDataFilePath, double interval) Creates an input probe for controlling the positions of the markers associated with this solver.
java.util.ArrayList<RigidBody>	getBodies() Returns the bodies associated with this IKSolver.
java.util.ArrayList<BodyConnector>	getConnectors() Returns the connectors associated with this IKSolver.
double	getConvergenceTol() Queries the convergance tolerance used in each solve step.
java.util.ArrayList<FrameMarker>	getMarkers() Returns the markers used by this IKSolver.
VectorNd	getMarkerWeights() Returns the marker weights used by this IKSolver.
double	getMassRegularization() Queries the mass regulaization coefficient for this solver.
int	getMaxIterations() Queries the maximum number of iterations allowed in each solve step.
MechModel	getMechModel() Returns the MechModel associated with this IKSolver.
boolean	isWritable() Returns true if this component should in fact be written to secondary storage.
int	numBodies() Queries the number of rigid bodies associated with this solver.
int	numIterations() Return the cummulative number of solve iterations.
int	numMarkers() Returns the number of markers used by this IKSolver.
int	numSolves() Return the cummulative number of solves.
void	postscan(java.util.Deque<ScanToken> tokens, CompositeComponent ancestor) Performs any required post-scanning for this component.
void	restoreModelState() Restores the state of the MechModel.
void	saveModelState() Saves the current state of the MechModel.
void	scan(ReaderTokenizer rtok, java.lang.Object ref) Scans this element from a ReaderTokenizer.
void	setBodiesDynamic(boolean dynamic) Sets the dynamic property of all the bodies associated with this solver.
void	setConvergenceTol(double tol) Sets the convergance tolerance tol used in each solve step.
void	setMarkerWeights(VectorNd weights) Sets the marker weights used by this IKSolver.
void	setMassRegularization(double c) Sets the mass regularization coefficent c for this solver.
void	setMaxIterations(int maxi) Set the maximum number of iterations allowed in each solve step.
int	solve(VectorNd mtargs) Updates the positions of the bodies associated with this solver to bring the markers as close as possible to the target positions specified by mtargs.
void	write(java.io.PrintWriter pw, NumberFormat fmt, java.lang.Object ref) Writes a text description of this element to a PrintWriter.

Methods inherited from class java.lang.Object

equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Detail

IKSolver

public IKSolver()

Creates an empty IKSolver.

IKSolver

public IKSolver(MechModel mech,
                java.util.Collection<FrameMarker> mkrs)

Creates an IKSolver for a set of markers contained within a prescribed MechModel. All markers are assumed to have a weight of 1. The rigid bodies and connectors are determined automatically from the markers. The frames associated with each marker must be a rigid body, and the set of all such bodies is known as the marker bodies. The bodies whose positions are updating by the solver includes the marker bodies, plus any other rigid bodies bodies connected to them via BodyConnectors, up to and excluding any rigid body whose grounded property is set to true.

Parameters:

mech - MechModel containing the markers and bodies

mkrs - markers determining which bodies will be controlled

IKSolver

public IKSolver(MechModel mech,
                java.util.Collection<FrameMarker> mkrs,
                VectorNd weights)

Creates an IKSolver for a set of markers contained within a prescribed MechModel. Markers weights can be specified by the optional vector weights. The rigid bodies and connectors are determined automatically from the markers. The frames associated with each marker must be a rigid body, and the set of all such bodies is known as the marker bodies. The bodies whose positions are updating by the solver includes the marker bodies, plus any other rigid bodies bodies connected to them via BodyConnectors, up to and excluding any rigid body whose grounded property is set to true.

Parameters:

mech - MechModel containing the markers and bodies

mkrs - markers determining which bodies will be controlled

weights - if non-null, specifies weights for the markers

IKSolver

public IKSolver(IKSolver solver)

Creates an IKSolver which is a copy of another IKSolver.

Parameters:

solver - IKSolver to copy

Method Detail

getMassRegularization

public double getMassRegularization()

Queries the mass regulaization coefficient for this solver.

Returns:

mass regulaization coefficient

See Also:

setMassRegularization(double)

setMassRegularization

public void setMassRegularization(double c)

Sets the mass regularization coefficent c for this solver. The default value is 0.001.

Each body is associated with a solve matrix defined by J^T J, where J is the matrix mapping differential changes in body pose to changes in marker position. This matrix provides the Hessian for the minimization. If the number of markers is less than 3, or if the markers all lie along a line, the solve matrix will be rank deficient. In that case, regularization terms are added to the matrix in such a way as to make it non-singular while preserving its range space. The size of these terms, relative to the weight of the matrix itself, is given by c.

Parameters:

c - mass regulaization coefficient

See Also:

getMassRegularization()

getMaxIterations

public int getMaxIterations()

Queries the maximum number of iterations allowed in each solve step.

Returns:

maximum number of solve iterations

See Also:

setMaxIterations(int)

setMaxIterations

public void setMaxIterations(int maxi)

Set the maximum number of iterations allowed in each solve step. The default value is 20.

Parameters:

maxi - maximum number of solve iterations

See Also:

getMaxIterations()

getConvergenceTol

public double getConvergenceTol()

Queries the convergance tolerance used in each solve step. #see #setConvergenceTol

Returns:

convergence tolerance

setConvergenceTol

public void setConvergenceTol(double tol)

Sets the convergance tolerance tol used in each solve step. A solve step will be considered converged with the incremental translation and rotation for all bodies is less then tol*bodyLen and tol, respectively, where bodyLen is the charateristic length of the body as inferrd from its spatial inertia. The default value of tol is 1e-8. #see #getConvergenceTol

Parameters:

tol - convergence tolerance

getBodies

public java.util.ArrayList<RigidBody> getBodies()

Returns the bodies associated with this IKSolver.

Returns:

bodies associated with this solver

numBodies

public int numBodies()

Queries the number of rigid bodies associated with this solver.

Returns:

number of bodies associated with this solver

setBodiesDynamic

public void setBodiesDynamic(boolean dynamic)

Sets the dynamic property of all the bodies associated with this solver.

Parameters:

dynamic - setting for each body's dynamic property

getMarkers

public java.util.ArrayList<FrameMarker> getMarkers()

Returns the markers used by this IKSolver.

Returns:

markers used by this solver

numMarkers

public int numMarkers()

Returns the number of markers used by this IKSolver.

Returns:

number of markers used by this solver

getMarkerWeights

public VectorNd getMarkerWeights()

Returns the marker weights used by this IKSolver.

Returns:

marker weights used by this solver

setMarkerWeights

public void setMarkerWeights(VectorNd weights)

Sets the marker weights used by this IKSolver.

Parameters:

weights - new marker weights

getMechModel

public MechModel getMechModel()

Returns the MechModel associated with this IKSolver.

Returns:

MechModel associated with this solver

getConnectors

public java.util.ArrayList<BodyConnector> getConnectors()

Returns the connectors associated with this IKSolver.

Returns:

connectors associated with this solver

solve

public int solve(VectorNd mtargs)

Updates the positions of the bodies associated with this solver to bring the markers as close as possible to the target positions specified by mtargs.

Parameters:

mtargs - target positions for each marker. Should have a size >= 3*numMarkers().

saveModelState

public void saveModelState()

Saves the current state of the MechModel. This allows the MechModel state to be restored after one or more solve steps.

restoreModelState

public void restoreModelState()

Restores the state of the MechModel.

numIterations

public int numIterations()

Return the cummulative number of solve iterations.

numSolves

public int numSolves()

Return the cummulative number of solves.

avgNumIterations

public double avgNumIterations()

Return the average number of iterations per solve

clearSolveCounts

public void clearSolveCounts()

Clears the cummulative solve and iteration counts.

isWritable

public boolean isWritable()

Returns true if this component should in fact be written to secondary storage. This gives subclasses control over whether or not they are actually written out.

Specified by:

isWritable in interface Scannable

Returns:

true if this component should be written to secondary storage.

write

public void write(java.io.PrintWriter pw,
                  NumberFormat fmt,
                  java.lang.Object ref)
           throws java.io.IOException

Writes a text description of this element to a PrintWriter. The text description should be compatable with scan and complete enough to allow full reconstruction of the element.

Specified by:

write in interface Scannable

Parameters:

pw - stream for writing the element

fmt - numeric formating information

ref - optional reference object which can be used for producing references to other objects

Throws:

java.io.IOException - if an I/O error occured

scan

public void scan(ReaderTokenizer rtok,
                 java.lang.Object ref)
          throws java.io.IOException

Scans this element from a ReaderTokenizer. The expected text format is assumed to be compatible with that produced by write.

Specified by:

scan in interface Scannable

Parameters:

rtok - Tokenizer from which to scan the element

ref - optional reference object which can be used for resolving references to other objects

Throws:

java.io.IOException - if an I/O or formatting error occured

postscan

public void postscan(java.util.Deque<ScanToken> tokens,
                     CompositeComponent ancestor)
              throws java.io.IOException

Performs any required post-scanning for this component. This involves handling any information whose processing was deferred during the scan() method and stored in the token queue. The most common use of this method is to resolve the paths of component references, which may not have been created at the time of the initial scan() call.

Specified by:

postscan in interface PostScannable

Parameters:

tokens - token information that was stored during scan().

ancestor - ancestor component with respect to which reference component paths are defined.

Throws:

java.io.IOException - if an error is encountered (such as a reference to a non-existent component)

createBodyPoseProbe

public PositionInputProbe createBodyPoseProbe(java.lang.String name,
                                              java.lang.String targDataFilePath,
                                              RotationRep rotRep,
                                              double interval)
                                       throws java.io.IOException

Creates an input probe for controlling the poses of the bodies associated with this solver. The data is generated by performing an inverse kinematic solve on marker data stored in a specified probe data file.

The vector size of the data in this file must be 3 * numMarkers(). The start and stop times and scale of the created probe is also determined from the data file. Knots are added to the probe at a time interval specified by interval; if this is given as -1, then the knot times are the same as those in the data file.

The state of the MechModel associated with this solver is saved before the solve and restored afterward.

Parameters:

name - if non-null, specifies the probe's name

targDataFilePath - path name of the probe data file containing the target marker positions

rotRep - rotation representation for the body orientations

interval - knot time spacing interval, or -1 if knot times should be determined from the data file

Returns:

the created input probe

Throws:

java.io.IOException - if an I/O error occurs reading the data file

createBodyPoseProbe

public PositionInputProbe createBodyPoseProbe(java.lang.String name,
                                              NumericProbeBase targProbe,
                                              RotationRep rotRep,
                                              double interval)

Creates an input probe for controlling the poses of the bodies associated with this solver. The data is generated by performing an inverse kinematic solve on marker target data containing in a probe targProbe.

The vector size of the data in mkrdata must be 3 * numMarkers(). The start and stop times and scale of the created probe is also determined from mkrdata. Knots are added to the probe at a time interval specified by interval; if this is given as -1, then the knot times are the same as those in mkrdata.

The state of the MechModel associated with this solver is saved before the solve and restored afterward.

Parameters:

name - if non-null, specifies the probe's name

targProbe - probe containing target position data for the markers

rotRep - rotation representation for the body orientations

interval - knot time spacing interval, or -1 if knot times should be determined from mrkdata

Returns:

the created input probe

createMarkerPositionProbe

public PositionInputProbe createMarkerPositionProbe(java.lang.String name,
                                                    java.lang.String targDataFilePath,
                                                    double interval)
                                             throws java.io.IOException

Creates an input probe for controlling the positions of the markers associated with this solver. The data is generated by performing an inverse kinematic solve on marker data stored in a specified probe data file. Effectively, this method takes marker data stored in the data file and filters it for kinematic feasibility.

The vector size of the data in this file must be 3 * numMarkers(). The start and stop times and scale of the created probe is also determined from the data file. Knots are added to the probe at a time interval specified by interval; if this is given as -1, then the knot times are the same as those in the data file.

The state of the MechModel associated with this solver is saved before the solve and restored afterward.

Parameters:

name - if non-null, specifies the probe's name

targDataFilePath - path name of the probe data file containing the target marker positions

interval - knot time spacing interval, or -1 if knot times should be determined from the data file.

Returns:

the created input probe

Throws:

java.io.IOException - if an I/O error occurs reading the data file

createMarkerPositionProbe

public PositionInputProbe createMarkerPositionProbe(java.lang.String name,
                                                    NumericProbeBase targProbe,
                                                    double interval)

Creates an input probe for controlling the positions of the markers associated with this solver. The data is generated by performing an inverse kinematic solve on marker target data containing in a probe targProbe. Effectively, this method takes marker data stored in targProbe and filters it for kinematic feasibility.

The vector size of the data in mkrdata must be 3 * numMarkers(). The start and stop times and scale of the created probe is also determined from mkrdata. Knots are added to the probe at a time interval specified by interval; if this is given as -1, then the knot times are the same as those in mkrdata.

The state of the MechModel associated with this solver is saved before the solve and restored afterward.

Parameters:

name - if non-null, specifies the probe's name

targProbe - probe containing target position data for the markers

interval - knot time spacing interval, or -1 if knot times should be determined from mrkdata

Returns:

the created input probe