@ThreadSafe public class Linkage extends java.lang.Object implements java.lang.Comparable<Linkage>
MSim Linkage model datastructure and mutation algorithms.
A Linkage is fundamentally a tree of Link
s connected by binary
tree Joint
s. Additional chain closure joints may also
be present that connect arbitrary links, closing kinematic chains. Links
represent rigid bodies and joints are structured rigid transforms from a
Joint.childLink
to an Joint.parentLink
.
RootJoint
sThe model tree begins at the groundLink
. For every other link
l there is a corresponding Link.rootJoint
, which, if
connected, attaches l to the groundLink
. Upon initial
creation, a link is a child of the groundLink
through its Link.rootJoint
. As the Linkage topology is mutated a link may be
re-parented, making its Link.rootJoint
a chain closure, but whenever
a link is orphaned it is automatically re-parented to its original Link.rootJoint
. In this way the Linkage topology, considering links as
vertices and joints as edges, is always fully connected: there is always a
path from any link to the groundLink
.
The groundLink
is mutable, see Link.makeGroundLink()
.
The top-level linkage may be broken into a tree of properly-nested
sublinkages as it is constructed. Each sublinkage contains its own
ground Link
, which is the parent of the RootJoint
s of all
other links in the sublinkage.
Every link in the entire linkage is always a member of exactly one
sublinkage. Every Joint
in the linkage either connects two links in
the same sublinkage, and is a member of same, or is a crossing joint
connecting a link in one sublinkage with a link in the enclosing sublinkage;
crossing joints are members of the inner sublinkage. An outcrossing
joint connects its child link in a sublinkage to a parent link in the
enclosing sublinkage, and vice-versa for an incrossing joint.
A sublinkage is created with one of three dispositions: driving,
driven, or simultaneous. The sublinkage's parent and
disposition are immutable, but the sublinkage can always be remove()
d. A driving sublinkage is solved before the enclosing sublinkage,
vice-versa for driven, and a simultaneous sublinkage is solved together with
the enclosing sublinkage. The dispositions can be considered to assign
directions to the edges in the sublinkage tree: the edge connecting a
driving sublinkage points to its enclosing sublinkage, vice-versa for
driven, and the edge to the parent of a simultaneous sublinkage is
bidirected. The sublinkage tree is thus a DAG and defines a partial order
on the solve sequence. Solver
s will now be constructed (in Controller.analyzeStructure(msim.model.Link, int)
so that no solver extends beyond a single SCC
of the sublinkage DAG. There may still be more than one solver per SCC; all
solvers within the same SCC are in the same solve round, which
corresponds to a topological sort of the sublinkage DAG.
There are no topological restrictions on crossing closure joints, but
there is always exactly one outcrossing tree joint (except for the top-level
sublinkage, which has none) connecting the sublinkage ground to a link in
the enclosing linkage. Also, the root joint of the sublinkage ground link
is always a child of the superlinkage ground link. Re-grounding the
sublinkage Link.makeGroundLink()
automatically switches the
outcrossing tree joint, and vice-versa. There are no restrictions on
incrossing tree joints for simultaneous or driving sublinkages, but
incrossing tree joints are disallowed for driven sublinkages.
Kinematic abstraction is achieved by replacing the abstracted sublinkage A by a virtual interface linkage I (typically I is simultaneous), with A a driven sublinkage of I.
A link corresponds to a link frame which is posed with respect to
the ground frame, link frame of the groundLink
.
A joint connects its Joint.childLink
to its Joint.parentLink
and has child and parent coordinate frames
corresponding to their link frames. A joint further contains three
sub-transforms: the Joint.childToMobilityRX
and Joint.mobilityToParentRX
are set by the user and define the pose of the
child mobility frame and parent mobility frame respectively,
relative to the child and parent frames; the Joint.mobilityRX
,
constrained to the transform subspace allowed by the joint limits, gives the
relative pose of the child and parent mobility frames. Manipulations to the
mobility transform effectively manipulate the pose of the Joint.childLink
with respect to Joint.parentLink
. The three
sub-transforms compose to produce a total transform Joint.jointRX
:
jointRX = (mobilityToParentRX)(mobilityRX)(childToMobilityRX)with transforms composing from right to left. Summarizing:
Joint.childLink
Joint.childToMobilityRX
takes child frame to child mobility
frameJoint.mobilityRX
takes child mobility frame to parent mobility
frameJoint.mobilityToParentRX
takes parent mobility frame to parent
frameJoint.parentLink
Joint.jointRX
takes coordinates from the child frame to the
parent frame.This discussion applies to non-Joint.invert()
-ed joints. Inverted
joints are basically the same except Joint.mobilityRX
takes parent
mobility frame to child mobility frame.
The connected tree structure means that it is always possible to resolve
the pose of every link frame relative to the ground frame by following the
corresponding tree path. A LinkageNode.cmt
(Composite Model
Transform) is computed for each LinkageNode
by a pose
analysis DFS in LinkageNode.updateCMTsRecursively()
from groundLink
. The CMT takes coordinates in a canonical frame for each
LinkageNode
type to the ground frame. The canonical frame for a a
link is the link frame, and the canonical frame for a joint is the parent
frame.
MSim is primarily a pure kinematic system, however, provision is included to also model and simulate quasistatic effects of gravity on link mass and linear-spring stiffness on 1-DoF joints.
A single global gravity acceleration vector may be set (in ultimate
ground frame) via MSim.setGravity(double...)
. By default gravity is unset,
which globally disables modeling and simulation of gravitational potential
energy (GPE).
Link masses and center-of-mass locations (in link frame) may also be set
via Link.setMass(double)
and Link.setCenterOfMass(double...)
. By default link
masses are unset (disabling GPE for that link) and CoMs are at link frame
origin. GPE is actually modeled as a property of the link's RootJoint
.
A linear-spring stiffness may be set via Joint.setStiffness(double)
on
any 1-DoF (revolute or prismatic) joint, and the stiffness rest pose may be
set via Joint.setStiffnessRest(double)
. By default spring stiffness is
unset, disabling elastic potential energy (EPE) for that joint, and
stiffness rest is zero (or the nearest DoF limit).
See Joint.getStiffnessPotential(double)
and RootJoint.getGravityPotential(RX)
for the formulas used to model EPE and
GPE, respectively.
Every joint has a mutable Joint.Type
which assigns a disposition
to each of the six translation and exponential map rotation vector
components of the Joint.mobilityRX
. DoI components,
Degrees of Invariance, have a fixed and immutable value. The rest of
the components are DoF, Degrees of Freedom, and have mutable
values that may range within any specified limits.
Each joint may also have target and posture poses assigned. Targets are treated with higher priority than postures.
A chain closure joint cj induces a kinematic cycle through
the least common ancestor (LCA) of its Joint.childLink
and Joint.parentLink
. The LCA is automatically found and stored as the Joint.branchLink
of cj during Controller.analyze()
. The
support of cj is the set of all joints in its cycle, not
including itself.
Tree joints always satisfy their invariants and limits; chain closure
joints are driven to satisfy their invariants, targets, postures, and limits
by adjusting their supports (closure joints with no invariants, targets, or
limits are unconstrained and are topologically skipped in this process).
This constraint satisfaction is imposed by a Solver
, which handles
the simultaneous solution of a set of chain closures with transitively
overlapping supports, called a solve group. Joints are driven to
satisfy a cascade of goals:
The MSim model is designed to be thread-safe. Most aspects of the model
are mutable, including both structure (topology) and state
(geometry), with everything guarded by Controller.LOCK
. All
external mutations are initiated by calls to public mutators on LinkageNode
and subclasses. These calls may be made asynchronously or they
may be run synchronously in the Controller.updateThread
, see Controller.scheduleUpdateTask(java.lang.Runnable)
and Controller.runUpdateTask(java.lang.Runnable)
.
Copyright (C) 2008 Marsette A. Vona, III
Modifier and Type | Class and Description |
---|---|
static class |
Linkage.Disposition
possible
disposition s |
Modifier and Type | Field and Description |
---|---|
protected java.util.Map<java.lang.String,Linkage> |
children
Immediate sublinkages in insertion order.
|
private static java.lang.String |
cvsid |
Linkage.Disposition |
disposition
solve disposition of this linkage in
parent , if any |
static Linkage.Disposition[] |
DISPOSITIONS
all
Linkage.Disposition s |
int |
distanceToPickedLinkage
Distance from this linkage to the currently picked linkage, or 0 if
none.
|
protected Link |
groundLink
the ground
Link |
protected java.util.Set<Joint> |
joints
the set of all
Joint s in this Linkage in insertion order |
protected java.util.Set<Link> |
links
the set of all
Link s in this Linkage in insertion order |
java.lang.String |
name
name of this linkage
|
Linkage |
parent
immediate superlinkage, if any
|
protected java.util.Map<java.lang.String,java.lang.Object> |
propertyMap
the property map or null if none
|
int |
solveRound
Solve ordering, see
compareTo(msim.model.Linkage) |
int |
sublinkageSCC
Index of containing strongly connected component in sublinkage DAG.
|
protected java.util.Collection<Linkage> |
unmodifiableChildren
unmodifiable view of
children |
protected java.util.Set<Joint> |
unmodifiableJoints
unmodifiable view of
joints |
protected java.util.Set<Link> |
unmodifiableLinks
unmodifiable view of
links |
Constructor and Description |
---|
Linkage()
Linkage(Linkage, Disposition, String) , auto-generates name,
default disposition, no parent. |
Linkage(Linkage parent)
Linkage(Linkage, Disposition, String) , auto-generates name,
default disposition. |
Linkage(Linkage parent,
Linkage.Disposition disposition)
Linkage(Linkage, Disposition, String) , auto-generates name |
Linkage(Linkage parent,
Linkage.Disposition disposition,
java.lang.String name)
Constructs a Linkage containing only a
groundLink . |
Modifier and Type | Method and Description |
---|---|
void |
clear()
empty entire linkage
|
void |
clearRecursively()
clear() every linkage in the linkage tree, and remove all
sublinkages. |
int[] |
collectTreeMetrics()
Covers
collectTreeMetrics(int[]) , conses. |
int[] |
collectTreeMetrics(int[] metrics)
|
int |
compareTo(Linkage o)
Compares this Linkage with the specified Linkage for order.
|
boolean |
contains(Joint j)
check if this
Linkage contains Joint j |
boolean |
contains(Link l)
check if this
Linkage contains Link l |
boolean |
contains(Linkage linkage)
Check if linkage is a child of this linkage.
|
boolean |
contains(LinkageNode n)
check if this
Linkage contains LinkageNode n |
void |
dump()
dump(java.io.PrintStream) to System.out |
void |
dump(java.io.PrintStream s)
dump linkage details
|
void |
dumpRecursively()
dumpRecursively(PrintStream) to System.out |
void |
dumpRecursively(java.io.PrintStream s)
Recursively
dump(PrintStream) the linkage sub-tree starting at
this linkage. |
protected void |
dumpRecursively(java.lang.String prefix,
java.io.PrintStream s)
impl of
dumpRecursively(PrintStream) |
void |
dumpTree()
dumpTree(PrintStream) to System.out |
void |
dumpTree(java.io.PrintStream s)
|
void |
dumpTreeMetrics()
dumpTreeMetrics(PrintStream) to System.out |
void |
dumpTreeMetrics(java.io.PrintStream s)
|
Linkage |
findLinkage(java.lang.String namePattern)
findLinkage(String, int) , returning the first linkage with
the given namePattern. |
Linkage |
findLinkage(java.lang.String namePattern,
int nth)
Get the nth linkage with a name matching namePattern in
recursive linkage tree DFS pre-order from this linkage.
|
java.util.Collection<Linkage> |
findLinkages(java.lang.String namePattern)
findLinkages(String, Collection) , consing a new ArrayList . |
java.util.Collection<Linkage> |
findLinkages(java.lang.String namePattern,
java.util.Collection<Linkage> linkages)
Get all linkages with a name matching namePattern in recursive
tree DFS pre-order in the linkage tree from this linkage.
|
Linkage |
getChild(java.lang.String name)
Get the named child linkage of this linkage in the linkage tree.
|
java.util.Collection<Linkage> |
getChildren()
Get an unmodifiable view of the set of child linkages of this linkage
in the linkage tree.
|
Link |
getGroundLink()
get a reference to the
groundLink |
java.util.Set<Joint> |
getJointSet()
get a reference to an immutable view of the set of
Joint s |
java.util.Set<Link> |
getLinkSet()
get a reference to an immutable view of the set of
Link s |
java.lang.Object |
getProperty(java.lang.String key)
get a property
|
boolean |
hasChildren()
check if this linkage has any children in the linkage tree
|
boolean |
hasDescendant(Linkage other)
check if this Linkage is an ancestor of (or the same as) other
|
boolean |
hasProperty(java.lang.String key)
check existence of a property
|
boolean |
isLive()
check if this linkage is actually present in its
parent |
boolean |
isTreeLeaf()
check if this linkage is a leaf in the linkage tree
|
java.util.Iterator<Joint> |
jointIterator()
get an iterator on an immutable view of the set of
Joint s |
java.util.Iterator<Link> |
linkIterator()
get an iterator on an immutable view of the set of
Link s |
int |
recomputeChainClosures()
Joint.recomputeInverse(int, boolean) all chain closures. |
int |
recomputeChainClosuresRecursively()
recomputeChainClosures() every linkage in the linkage tree. |
java.util.Iterator<Joint> |
recursiveJointIterator()
Return an iterator over all the
Joint s in all the linkages in
the linkage tree. |
java.util.Iterator<Linkage> |
recursiveLinkageIterator()
Return an iterator over all the linkages in the linkage tree in DFS
pre-order.
|
java.util.Iterator<Link> |
recursiveLinkIterator()
Return an iterator over all the
Link s in all the linkages in
the linkage tree. |
boolean |
recursivelyContains(Joint j)
Check
contains(Joint) on every linkage in the linkage
tree. |
boolean |
recursivelyContains(Link l)
Check
contains(Link) on every linkage in the linkage
tree. |
boolean |
recursivelyContains(Linkage linkage)
Check if linkage is a descendant of this linkage.
|
boolean |
recursivelyContains(LinkageNode n)
Check
contains(LinkageNode) on every linkage in the linkage
tree. |
void |
remove()
Remove this sublinkage.
|
java.lang.Object |
removeProperty(java.lang.String key)
remove a property
|
java.lang.Object |
setProperty(java.lang.String key,
java.lang.Object value)
covers
setProperty(String, Object, boolean) , replaces |
java.lang.Object |
setProperty(java.lang.String key,
java.lang.Object value,
boolean replace)
Set a property.
|
java.lang.String |
toString()
toString(boolean, boolean) , no stats or details |
java.lang.String |
toString(boolean structureStats)
toString(boolean, boolean) , no solve details |
java.lang.String |
toString(boolean structureStats,
boolean solveDetails)
returns a short human-readable identifying string for this linkage
|
private static final java.lang.String cvsid
protected final java.util.Set<Link> links
Link
s in this Linkage in insertion orderprotected final java.util.Set<Joint> joints
Joint
s in this Linkage in insertion orderprotected final java.util.Set<Joint> unmodifiableJoints
joints
public final java.lang.String name
public final Linkage parent
public static final Linkage.Disposition[] DISPOSITIONS
Linkage.Disposition
spublic final Linkage.Disposition disposition
parent
, if anyprotected java.util.Map<java.lang.String,Linkage> children
Immediate sublinkages in insertion order.
protected final java.util.Collection<Linkage> unmodifiableChildren
children
public int solveRound
Solve ordering, see compareTo(msim.model.Linkage)
public int sublinkageSCC
Index of containing strongly connected component in sublinkage DAG.
public int distanceToPickedLinkage
Distance from this linkage to the currently picked linkage, or 0 if none.
protected java.util.Map<java.lang.String,java.lang.Object> propertyMap
public Linkage(Linkage parent, Linkage.Disposition disposition, java.lang.String name)
Constructs a Linkage containing only a groundLink
.
parent
- the containing superlinkage, or null if nonedisposition
- the disposition
, or null to default to Linkage.Disposition.SIMULTANEOUS
name
- the name of the linkage, must be unique in parent, if
any, or null to auto-generate a unique namepublic Linkage(Linkage parent, Linkage.Disposition disposition)
Linkage(Linkage, Disposition, String)
, auto-generates namepublic Linkage(Linkage parent)
Linkage(Linkage, Disposition, String)
, auto-generates name,
default disposition.
public Linkage()
Linkage(Linkage, Disposition, String)
, auto-generates name,
default disposition, no parent.
public int compareTo(Linkage o)
Compares this Linkage with the specified Linkage for order.
compareTo
in interface java.lang.Comparable<Linkage>
solveRound
is less than, equal to, or greater than o.solveRound
public java.util.Collection<Linkage> getChildren()
Get an unmodifiable view of the set of child linkages of this linkage in the linkage tree.
public Linkage getChild(java.lang.String name)
Get the named child linkage of this linkage in the linkage tree.
public boolean hasChildren()
public boolean isTreeLeaf()
public boolean hasDescendant(Linkage other)
public void remove()
Remove this sublinkage.
The top-level linkage cannot be removed.
All connected incoming crossing tree joints must first be disconnected or converted to chain closures. All crossing joints are detached from the superlinkage.
java.lang.IllegalStateException
- if this is the top-level linkage or if
there are any incoming crossing tree jointspublic boolean isLive()
parent
public java.util.Iterator<Linkage> recursiveLinkageIterator()
Return an iterator over all the linkages in the linkage tree in DFS pre-order.
Synchronize on Controller.LOCK
while using the returned
iterator, which does not support remove()
.
public Linkage findLinkage(java.lang.String namePattern, int nth)
Get the nth linkage with a name matching namePattern in recursive linkage tree DFS pre-order from this linkage.
nth
- 0 to return the first matching linkage, 1 to return the 2nd,
etcpublic Linkage findLinkage(java.lang.String namePattern)
findLinkage(String, int)
, returning the first linkage with
the given namePattern.
public java.util.Collection<Linkage> findLinkages(java.lang.String namePattern, java.util.Collection<Linkage> linkages)
Get all linkages with a name matching namePattern in recursive tree DFS pre-order in the linkage tree from this linkage.
public java.util.Collection<Linkage> findLinkages(java.lang.String namePattern)
findLinkages(String, Collection)
, consing a new ArrayList
.
public boolean contains(Linkage linkage)
Check if linkage is a child of this linkage.
public boolean recursivelyContains(Linkage linkage)
Check if linkage is a descendant of this linkage.
public Link getGroundLink()
groundLink
public java.util.Set<Link> getLinkSet()
Link
spublic java.util.Set<Joint> getJointSet()
Joint
spublic java.util.Iterator<Link> linkIterator()
Link
spublic java.util.Iterator<Link> recursiveLinkIterator()
Return an iterator over all the Link
s in all the linkages in
the linkage tree.
The linkages are iterated in DFS pre-order. Within a linkage the links are iterated in insertion order.
Synchronize on Controller.LOCK
while using the returned
iterator, which does not support remove()
.
public java.util.Iterator<Joint> jointIterator()
Joint
spublic java.util.Iterator<Joint> recursiveJointIterator()
Return an iterator over all the Joint
s in all the linkages in
the linkage tree.
The linkages are iterated in DFS pre-order. Within a linkage the joints are iterated in insertion order.
Synchronize on Controller.LOCK
while using the returned
iterator, which does not support remove()
.
public boolean recursivelyContains(Link l)
Check contains(Link)
on every linkage in the linkage
tree.
public boolean recursivelyContains(Joint j)
Check contains(Joint)
on every linkage in the linkage
tree.
public boolean contains(LinkageNode n)
Linkage
contains LinkageNode
npublic boolean recursivelyContains(LinkageNode n)
Check contains(LinkageNode)
on every linkage in the linkage
tree.
public int recomputeChainClosures()
Joint.recomputeInverse(int, boolean)
all chain closures.
public int recomputeChainClosuresRecursively()
recomputeChainClosures()
every linkage in the linkage tree.
public void clear()
public void clearRecursively()
clear()
every linkage in the linkage tree, and remove all
sublinkages.
public java.lang.String toString(boolean structureStats, boolean solveDetails)
public java.lang.String toString(boolean structureStats)
toString(boolean, boolean)
, no solve detailspublic java.lang.String toString()
toString(boolean, boolean)
, no stats or detailstoString
in class java.lang.Object
public int[] collectTreeMetrics(int[] metrics)
public int[] collectTreeMetrics()
Covers collectTreeMetrics(int[])
, conses.
public void dump(java.io.PrintStream s)
public void dump()
dump(java.io.PrintStream)
to System.outpublic void dumpRecursively(java.io.PrintStream s)
Recursively dump(PrintStream)
the linkage sub-tree starting at
this linkage.
public void dumpRecursively()
dumpRecursively(PrintStream)
to System.outprotected void dumpRecursively(java.lang.String prefix, java.io.PrintStream s)
dumpRecursively(PrintStream)
public void dumpTree(java.io.PrintStream s)
public void dumpTree()
dumpTree(PrintStream)
to System.outpublic void dumpTreeMetrics(java.io.PrintStream s)
public void dumpTreeMetrics()
dumpTreeMetrics(PrintStream)
to System.outpublic java.lang.Object setProperty(java.lang.String key, java.lang.Object value, boolean replace)
Set a property.
replace
- whether to replace the value if the key is already presentpublic java.lang.Object setProperty(java.lang.String key, java.lang.Object value)
setProperty(String, Object, boolean)
, replacespublic java.lang.Object removeProperty(java.lang.String key)
public java.lang.Object getProperty(java.lang.String key)
public boolean hasProperty(java.lang.String key)