Assembly< DataType, dim > Class Template Reference

#include <Assembly.h>

Public Types
typedef Part< DataType, dim >	PartType
typedef ads::FixedArray< dim, DataType >	point_type
typedef ads::FixedArray< dim, int >	multi_index_type
Public Member Functions
	Assembly ()
	Assembly (const base &v)
	~Assembly ()
virtual void	setDim ()
virtual std::vector< int > &	IdTag ()
virtual void	setIdTag (std::vector< int > val)
virtual std::string	Name ()
virtual void	setName (std::string val)
virtual base *	GetWorld ()
virtual void	setDHMat (MType val)
virtual MType	GetDHMat ()
virtual void	AddPart (PartType *v)
virtual void	AddPart (PartType &v)
virtual int	GetNumParts ()
virtual PartType *	GetNthPart (int n)
virtual void	AddSubAssembly (base *v)
virtual void	AddSubAssembly (base &v)
virtual int	GetNumSubAssemblies ()
virtual base *	GetNthSubAssembly (int n)
virtual void	setAssemblyTmatrix ()
virtual void	setAssemblyTmatrix (const MType ttemp)
virtual MType *	getAssemblyTmatrix ()
void	setAssemblyMotion (MType &Tmatrix)
void	updateAssembly (DataType dt, DataType time, MType transform, int level)
void	resetAssembly ()
void	partPressureForces (multi_index_type cons, DataType press, DataType scale)
virtual void	logPartLoads (std::ofstream &ofs, DataType time_, int steps)
void	send2CPT (int &nump, int &numc, point_type vertices, multi_index_type connections, DataType scale)
void	vel2CPT (int &totPoints, point_type *velocities, DataType scale)
int	GetNumPoints ()
void	GetNumPC (int &pcount, int &ccount)
void	print (int level)
void	printSummary (int level)
virtual void	outputAssembly_Brep3D (DataType time, int pc, int pointCount[], int &pp, FILE *fout)
virtual void	outputAssembly_STL (DataType time, FILE *fout)
virtual void	Restart (std::ifstream &ifs, int &pos, double &t, double &dt)
virtual void	Checkpointing (std::ofstream &ofs)
virtual PType *	GetVerticies ()
virtual multi_index_type *	GetConnections ()
virtual void	makeVerticies (int n)
virtual void	makeConnections (int n)
virtual PType	surfacePoint (PType I_A, DataType zmin, int flag)
virtual PType	surfacePoint (PType I_A, DataType zmin, int flag, DataType off)
Protected Attributes
std::vector< int >	idTag
std::string	name
bool	mobile
MType	AssemblyTmatrix
MType	MotionTmatrix
std::vector< base >	subAssemblies
std::vector< PartType * >	parts
int	num_subAssemblies
int	num_parts
MType	DHMat
MType	DHMat_old
base *	parent
std::vector< int >	edges
int	component
int	backedge
int	gdistance
PType *	verticies
multi_index_type *	connections

Detailed Description

template<class DataType, int dim>
class Assembly< DataType, dim >

Assemblies are groups of parts and sub-assemblies.
Revolute, prismatic, or spherical wrist joints can be located in the coordinate frames of assemblies and parts.
Links connecting 2 joints are connected to form kinematic chains from the scene coordinate frame, $Oxyz_{Scene}$ , to
the native coordinate frame, $Oxyz_{Native}$ , of a part or assembly.
The pose of the links in a kinematic chain are described using the Denavit-Hartenberg (DH) Kinematic Chain Convention.
Kinematic chains can be static or mobile.

http://en.wikipedia.org/wiki/Denavit–Hartenberg_parameters
http://elvis.rowan.edu/~kay/papers/kinematics.pdf
www.site.uottawa.ca/~petriu/generalrobotics.org-ppp-Kinematics_final.ppt‎

Geometry Solver USAGE:
initialize: parts and assemblies are read or created from input files.
set up:

define links, attachments, and joints in kinematic chain
http://www.cs.duke.edu/brd/Teaching/Bio/asmb/current/Papers/chap3-forward-kinematics.pdf pg 82
- Part: use nearestFacet() to specify Parent/Child points and zaxis_Parent/Child (not yet updated for DH implementation)
- Assembly: specify Parent/Child points and zaxis_Parent/Child
Denavit-Hartenberg parameters are calculated
Static parts are posed.

getGeometry: called at each coupling time step

Deformable Parts are updated in their native coordinate frame based on prescribed local transformations or on the fly based on
output from a coupled (kinetic) solver.
Kinematic update (mobile chains only)
1. Denavit-Hartenberg joint parameters are updated by prescription, based on part deformation,
  from a coupled (kinetic) solver, or any combination there in.
2. Denavit-Hartenberg matrixes are updated for parts and assemblies.
All mobile parts are updated to their pose relative to the Scene's coordinate frame by using the following Denavit-Hartenberg matrixes to tranform the part's cPoints.
- $Part^{Scene}_{Native} = \left[T^{Scene}_0\right] \, \left[T^0_{Native}\right] \times Part.cPoints$
1. Part boundaries (2D:curves, 3D:surfaces) are sent to AMROC-FSI
2. If this is an output time step part boundaries are output along with selected simulation variables
3. If kinematicOutput is selected Denavit-Hartenberg parameters are output
4. If constructionOutput is selcted the curves, splines, points, and vectors, used in constructing parts and assemblies will be output

Kinematic solver
Motions and deformations can be prescribed a priori or determined on the fly from a coupled kinetic solver.

Part: Updates Denavit-Hartenberg parameters for part deformation or for precribed
changes in joint position and/or orientation.
- part deformation can cause changes in due to:
  - $\Delta\,orientation :=$ angle3D(change in facet normal) in 's frame
  - $\Delta\,position :=$ change in facet centroids in 's frame
Part and Assembly: Updates DH parameters for prescribed joint motions
(joint axis revolution/translation $:= q_{pre}$ )

CAPABILITY / STRUCTURE NOTATION
Attachment class functionality deprecated by Denavit-Hartenberg Link and Joint classes.
DH_Link class

Link reference frame, two joints, and DH parameters that define
how a Part or Assembly with its own frame of reference is posed as a link in a
kinematic chain
Part / Assembly idTag
Joint with its kinematic parent actuates at a point and direction (in part's native coordinates)
Joint with a kinematic child actuates at a point and direction (in part's natives coordinates)
Initial DH link pose DataType $d_{ref}, r_{ref}, \alpha_{ref}, \theta_{ref}$
Current DH link pose DataType
- $\theta_n =$ rotation about $z_{n-1}$ (link $\hat{z}$ axis) from $x_{n-1}$ to
- $\alpha_n =$ rotation about (link $\hat{x}$ axis) from $z_{n-1}$ to
- distance from link origin to intersection of $z_{n-1}$ and
- distance from link origin to intersection of and

Joint class

actuationType enum {revolute, prismatic, spherical wrist}
DataType $q_{pre}$ prescribed articulation
Part:
- int id = -1
- PType origin = point, axis = vector
  OR
- int id = connectionId (segment or facet Id)
- int connectionOrientation
  - 2D: $,\, \left[ \, 0 := \overrightarrow{0-1}, 1 := \overrightarrow{1-0} \,\right]$
  - 3D: $,\, \left[ \, 0 := \overrightarrow{0-1}\times \overrightarrow{0-2}, 1 := \overrightarrow{0-2}\times \overrightarrow{0-1} \,\right]$
- PType origin = facet centroid, axis = facet normal
Assembly:
- int id = -1
- PType origin = point, axis = vector

Kinematic Chain Class

contain a std::vector of $\mathcal{N}$ connected link pointers.
can be static (not checked for kinematic updates) or mobile (checked for kinematic updates).
the final link in the kinematic chain from a base, , to the terminal Part / Assembly, $\mathcal{M}$ ,
connects link $\mathcal{N}-1$ 's child joint to a joint at and aligned with the origin of $\mathcal{M}$ 's native coordinate system.
$T^{\mathcal{N}-1}_{\mathcal{M}\,Native}$

================================================================================================

Scene:

contains a std::vector of Assemblies
- contain a std::vector of parts and a std::vector of Denavit-Hartenberg matrixes for them.
- contain a std::vector of assemblies and a std::vector of Denavit-Hartenberg matrixes for them.
contains a Joint at the Scene origin
contains a std::vector of DH_Links
- used to position the base of a Kinematic chain
contain a std::vector of Kineitc Chains
- To pose a Part / Assembly, , attached to joint in a kinematic chain relative to
  another coordinate frame, , form the Denavit-Hartenberg matrix:
  - $T^{\mathcal{K}}_{G\,Native} = T^{\mathcal{K}}_i \, T^i_{G\,Native}$
- Branching chains can be formed by ending one chain at an assembly that is the base of 2 or more chains.
  Parent chains are updated before their children.

================================================================================================

Denavit-Hartenberg kinematic chain

Link

Joint to Parent

Joint to Child

$\alpha$

$\theta$

$q_{pre}$

$\vec{X}$

$\vec{Y}$

$\vec{Z}$

idTag

$\vec{ParentN}$

facet Id

facet Orientation

jointType

idTag

$\vec{ChildN}$

facet Id

facet Orientation

jointType

1

Assembly

Base

Base $\vec{Z}_0$

NONE

Part / Assembly

$\vec{Z}_1$

outward

2

Part / Assembly

$\mapsto \, \vec{ChildN}_1$

inward

Part / Assembly

outward

$\vdots$

Part / Assembly

$\mapsto \, \vec{ChildN}_{i-1}$

inward

Part / Assembly

outward

$\vdots$

$\mathcal{N}$

Part / Assembly

$\mapsto \, \vec{ChildN}_{\mathcal{N}-1}$

inward

Part / Assembly

$O_{Native}$

$\vec{Z}_{Native}$

NONE

================================================================================================

Denavit-Hartenberg convention and algorithm:
The four parameters $r_i, \alpha_i, d_i$ , and $\theta_i$ in $(\star)$ are generally
given the names link length, link twist, link offset, and joint angle, respectively.

: Locate and label the joint axes .
- Joint positions and axii can be specified in the native coordinate frame of a Part or Assembly.
- Joint positions and axii can be determined from Part Facet s.
: Establish the base frame. Set the origin anywhere on the axis.
The and axes are chosen conveniently to form a right-hand frame.
For $i = 1, \dots , n − 1,$ perform Steps 3 to 5 when joint i and i-1 are in the same coordinate frame.
When i and i-1 are in different coordinate frames the Denavit-Hartenberg parameters are specified.
: Locate the origin where the common normal to and $z_{i-1}$
intersects . If intersects $z_{i-1}$ locate at this intersection. If
and $z_{i-1}$ are parallel, locate in any convenient position along .
: Establish the $\hat{x}_i$ axis along the common normal between $z_{i-1}$ and through
, or in the direction normal to the $z_{i-1}$ − plane if $z_{i-1}$ and
intersect.
dist3D_Line_to_Line() can set , $\hat{x}_i = \vec{CN}$ , , and
: Establish to complete a right-hand frame.
cross3D() ( $\hat{z}_i$ , $\hat{x}_i$ ) returns $\hat{y}_i$
: Establish the end-effector frame . Assuming the $n^{th}$ joint
is revolute, set $z_n = \hat{a}$ along the direction $z_{n-1}$ . Establish the origin
On conveniently along , preferably at the center of the gripper or at
the tip of any tool that the manipulator may be carrying. Set $y_n = \hat{s}$
in the direction of the gripper closure and set $x_n = \hat{n}$ as $\hat{s} \times \hat{a}$ . If
the tool is not a simple gripper set and conveniently to form a
right-hand frame.
: Create a table of link parameters .
- = distance along from to the intersection of the and $z_{i-1}$
  axes. Can be set by dist3D_Line_to_Line()
- = distance along $z_{i-1}$ from $O_{i-1}$ to the intersection of the and
  $z_{i-1}$ axes from dist3D_Line_to_Line(). is variable if joint is prismatic.
- $\alpha_i =$ the angle between $z_{i-1}$ and measured about (see Figure 3.3).
  angle3D2_axis() ( $z_{i-1}$ , , ) returns $\alpha_i$
- $\theta_i =$ the angle between $x_{i-1}$ and measured about $z_{i-1}$ (see Figure
  3.3). $\theta_i$ is variable if joint is revolute.
  angle3D2_axis() ( $x_{i-1}$ , , $z_{i-1}$ ) returns $\theta_i$
: Form the homogeneous transformation matrices by substituting
the above parameters into $(\star)$ .
: Form
$T^0_\mathcal{N} = A_1 \cdots A_n$ . This then gives the position and orientation
of the $\mathcal{N}^{th}$ link frame expressed in base coordinates.

$A_i = R_{z\,,\,\Theta_i} Trans_{z\,,\,d_i} Trans_{x\,,\,r_i} R_{x\,,\,\alpha_i} \quad (\star)$

$A_i = \left[ \begin{array}{cccc} c \Theta_i & -s \Theta_i & 0 & 0 \\ s \Theta_i & c \Theta_i & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right] \left[ \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & D_i \\ 0 & 0 & 0 & 1 \end{array} \right] \left[ \begin{array}{cccc} 1 & 0 & 0 & r_i \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array} \right] \left[ \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & c \alpha_i & -s \alpha_i & 0 \\ 0 & s \alpha_i & c \alpha_i & 0 \\ 0 & 0 & 0 & 1 \end{array} \right]$

$A_i = \left[ \begin{array}{cccc} c \Theta_i & -s \Theta_i c \alpha_i & s \Theta_i s \alpha_i & r_i c \Theta_i \\ s \Theta_i & c \Theta_i c \alpha_i & -c \Theta_i s \alpha_i & r_i s \Theta_i \\ 0 & s \alpha_i & c \alpha_i & D_i \\ 0 & 0 & 0 & 1 \end{array} \right]$

Where $\Theta_i = \theta_i + q_{pre}$ if joint $ i $ is revolute, $\Theta_i = \theta_i$ otherwise.
And where $D_i = d_i + q_{pre}$ if joint $ i $ is prismatic, $ D_i = d_i $ otherwise.