Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F85438452
onbody.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Sun, Sep 29, 05:51
Size
10 KB
Mime Type
text/x-c
Expires
Tue, Oct 1, 05:51 (2 d)
Engine
blob
Format
Raw Data
Handle
21034723
Attached To
rLAMMPS lammps
onbody.cpp
View Options
/*
*_________________________________________________________________________*
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME *
* FILE NAME: onbody.cpp *
* AUTHORS: See Author List *
* GRANTS: See Grants List *
* COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
* LICENSE: Please see License Agreement *
* DOWNLOAD: Free at www.rpi.edu/~anderk5 *
* ADMINISTRATOR: Prof. Kurt Anderson *
* Computational Dynamics Lab *
* Rensselaer Polytechnic Institute *
* 110 8th St. Troy NY 12180 *
* CONTACT: anderk5@rpi.edu *
*_________________________________________________________________________*/
#include "onbody.h"
#include "body.h"
#include "inertialframe.h"
#include "joint.h"
#include "onfunctions.h"
#include "virtualmatrix.h"
#include "matrixfun.h"
#include <iostream>
#include "norm.h"
#include "eulerparameters.h"
using
namespace
std
;
OnBody
::
OnBody
(){
system_body
=
0
;
system_joint
=
0
;
parent
=
0
;
// these terms have zeros which are NEVER overwritten
sI
.
Zeros
();
sSC
.
Zeros
();
}
OnBody
::~
OnBody
(){
children
.
DeleteValues
();
}
int
OnBody
::
RecursiveSetup
(
InertialFrame
*
basebody
){
int
ID
=
0
;
system_body
=
basebody
;
// record that the traversal algorithm has been here
if
(
system_body
->
GetID
()
)
return
0
;
ID
++
;
system_body
->
SetID
(
ID
);
// setup inertial frame
SetupInertialFrame
();
Joint
*
joint
;
OnBody
*
child
;
int
tempID
;
// loop through children calling the recursive setup function
ListElement
<
Joint
>*
ele
=
system_body
->
joints
.
GetHeadElement
();
while
(
ele
){
joint
=
ele
->
value
;
child
=
new
OnBody
;
tempID
=
child
->
RecursiveSetup
(
ID
,
this
,
joint
);
if
(
tempID
){
children
.
Append
(
child
);
ID
=
tempID
;
}
else
delete
child
;
ele
=
ele
->
next
;
}
return
ID
;
}
int
OnBody
::
RecursiveSetup
(
int
ID
,
OnBody
*
parentbody
,
Joint
*
sys_joint
){
// initialize the topology and system analogs
parent
=
parentbody
;
system_joint
=
sys_joint
;
system_body
=
system_joint
->
OtherBody
(
parentbody
->
system_body
);
// record that the traversal algorithm has been here
if
(
system_body
->
GetID
()
)
return
0
;
ID
++
;
// perform the local setup operations
Setup
();
Joint
*
joint
;
OnBody
*
child
;
int
tempID
;
// loop through children calling the recursive setup function
ListElement
<
Joint
>*
ele
=
system_body
->
joints
.
GetHeadElement
();
while
(
ele
){
joint
=
ele
->
value
;
if
(
joint
!=
sys_joint
){
child
=
new
OnBody
;
tempID
=
child
->
RecursiveSetup
(
ID
,
this
,
joint
);
if
(
tempID
){
children
.
Append
(
child
);
ID
=
tempID
;
}
else
delete
child
;
}
ele
=
ele
->
next
;
}
return
ID
;
}
void
OnBody
::
SetupInertialFrame
(){
// error check
if
(
system_body
->
GetType
()
!=
INERTIALFRAME
){
cerr
<<
"ERROR: attempting to setup inertial frame for non-inertial body"
<<
endl
;
exit
(
1
);
}
// setup gravity
Vect3
neg_gravity
=
-
((
InertialFrame
*
)
system_body
)
->
GetGravity
();
sAhat
.
Zeros
();
Set6DLinearVector
(
sAhat
,
neg_gravity
);
}
void
OnBody
::
Setup
(){
// get the direction of the joint
if
(
system_joint
->
GetBody2
()
==
system_body
)
joint_dir
=
FORWARD
;
else
joint_dir
=
BACKWARD
;
// set the function pointers for the joint direction
if
(
joint_dir
==
FORWARD
){
kinfun
=
&
Joint
::
ForwardKinematics
;
updatesP
=
&
Joint
::
UpdateForward_sP
;
gamma
=
system_joint
->
GetR12
();
pk_C_k
=
system_joint
->
Get_pkCk
();
}
else
{
kinfun
=
&
Joint
::
BackwardKinematics
;
updatesP
=
&
Joint
::
UpdateBackward_sP
;
gamma
=
system_joint
->
GetR21
();
pk_C_k
=
system_joint
->
Get_kCpk
();
}
// initialize variables and dimensions
// sI
OnPopulateSI
(
system_body
->
inertia
,
system_body
->
mass
,
sI
);
// sP
if
(
joint_dir
==
FORWARD
)
sP
=
system_joint
->
GetForward_sP
();
else
sP
=
system_joint
->
GetBackward_sP
();
// dimension these quantities
sM
=
T
(
sP
)
*
sP
;
sMinv
=
sM
;
sPsMinv
=
sP
;
sIhatsP
=
sP
;
// get the state and state derivative column matrix pointers
q
=
system_joint
->
GetQ
();
u
=
system_joint
->
GetU
();
qdot
=
system_joint
->
GetQdot
();
udot
=
system_joint
->
GetUdot
();
qdotdot
=
system_joint
->
GetQdotdot
();
}
void
OnBody
::
RecursiveKinematics
(){
OnBody
*
child
;
// Perform local kinematics recursively outward
ListElement
<
OnBody
>*
ele
=
children
.
GetHeadElement
();
while
(
ele
){
child
=
ele
->
value
;
child
->
LocalKinematics
();
child
->
RecursiveKinematics
();
Mat3x3
result
=*
child
->
pk_C_k
;
ele
=
ele
->
next
;
}
}
void
OnBody
::
RecursiveTriangularization
(){
OnBody
*
child
;
// Perform local triangularization recursively inward
ListElement
<
OnBody
>*
ele
=
children
.
GetHeadElement
();
while
(
ele
){
child
=
ele
->
value
;
child
->
RecursiveTriangularization
();
//child->LocalTriangularization();
ele
=
ele
->
next
;
}
}
void
OnBody
::
RecursiveForwardSubstitution
(){
OnBody
*
child
;
// Perform local forward substitution recursively outward
ListElement
<
OnBody
>*
ele
=
children
.
GetHeadElement
();
while
(
ele
){
child
=
ele
->
value
;
// child->LocalForwardSubstitution();
child
->
RecursiveForwardSubstitution
();
ele
=
ele
->
next
;
}
}
void
OnBody
::
LocalKinematics
(){
// do the directional kinematics
(
system_joint
->*
kinfun
)();
(
system_joint
->*
updatesP
)(
sP
);
OnPopulateSC
(
*
gamma
,
*
pk_C_k
,
sSC
);
}
Mat3x3
OnBody
::
GetN_C_K
(){
Mat3x3
nck
=
system_body
->
n_C_k
;
return
nck
;
}
int
OnBody
::
GetBodyID
(){
int
ID
=
system_body
->
GetID
();
return
ID
;
}
Vect3
OnBody
::
LocalCart
(){
(
system_joint
->*
kinfun
)();
Vect3
RR
=
system_body
->
r
;
return
RR
;
}
void
OnBody
::
LocalTriangularization
(
Vect3
&
Torque
,
Vect3
&
Force
){
Vect3
Iw
,
wIw
,
Ialpha
,
wIwIalpha
,
ma
,
Bforce
,
Bforce_ma
,
Btorque
,
Btorque_wIwIalpha
;
Iw
.
Zeros
();
wIw
.
Zeros
();
wIwIalpha
.
Zeros
();
ma
.
Zeros
();
Bforce
.
Zeros
();
Bforce_ma
.
Zeros
();
Btorque
.
Zeros
();
Btorque_wIwIalpha
.
Zeros
();
FastMult
(
system_body
->
inertia
,
system_body
->
omega_k
,
Iw
);
FastCross
(
Iw
,
system_body
->
omega_k
,
wIw
);
FastMult
(
system_body
->
inertia
,
system_body
->
alpha_t
,
Ialpha
);
FastSubt
(
wIw
,
Ialpha
,
wIwIalpha
);
FastMult
(
-
system_body
->
mass
,
system_body
->
a_t
,
ma
);
Mat3x3
k_C_n
=
T
(
system_body
->
n_C_k
);
Bforce
=
k_C_n
*
Force
;
Btorque
=
k_C_n
*
Torque
;
FastAdd
(
Bforce
,
ma
,
Bforce_ma
);
FastAdd
(
Btorque
,
wIwIalpha
,
Btorque_wIwIalpha
);
OnPopulateSVect
(
Btorque_wIwIalpha
,
Bforce_ma
,
sF
);
sIhat
=
sI
;
sFhat
=
sF
;
Mat6x6
sPsMinvsPT
;
Mat6x6
sIhatsPsMsPT
;
Mat6x6
sUsIhatsPsMsPT
;
Mat6x6
sTsIhat
;
Mat6x6
sTsIhatsSCT
;
Vect6
sTsFhat
;
Mat6x6
sU
;
sU
.
Identity
();
OnBody
*
child
;
ListElement
<
OnBody
>*
ele
=
children
.
GetHeadElement
();
while
(
ele
){
child
=
ele
->
value
;
FastMultT
(
child
->
sIhatsP
,
child
->
sPsMinv
,
sIhatsPsMsPT
);
FastSubt
(
sU
,
sIhatsPsMsPT
,
sUsIhatsPsMsPT
);
FastMult
(
child
->
sSC
,
sUsIhatsPsMsPT
,
child
->
sT
);
FastMult
(
child
->
sT
,
child
->
sIhat
,
sTsIhat
);
FastMultT
(
sTsIhat
,
child
->
sSC
,
sTsIhatsSCT
);
FastAdd
(
sIhat
,
sTsIhatsSCT
,
sIhat
);
FastMult
(
child
->
sT
,
child
->
sFhat
,
sTsFhat
);
FastAdd
(
sFhat
,
sTsFhat
,
sFhat
);
ele
=
ele
->
next
;
}
FastMult
(
sIhat
,
sP
,
sIhatsP
);
FastTMult
(
sP
,
sIhatsP
,
sM
);
sMinv
=
SymInverse
(
sM
);
FastMult
(
sP
,
sMinv
,
sPsMinv
);
}
void
OnBody
::
LocalForwardSubstitution
(){
Vect6
sSCTsAhat
;
Vect6
sIhatsSCTsAhat
;
Vect6
sFsIhatsSCTsAhat
;
Vect6
sPudot
;
int
type
=
system_joint
->
GetType
();
if
(
type
==
1
){
FastTMult
(
sSC
,
parent
->
sAhat
,
sSCTsAhat
);
FastMult
(
sIhat
,
sSCTsAhat
,
sIhatsSCTsAhat
);
FastSubt
(
sFhat
,
sIhatsSCTsAhat
,
sFsIhatsSCTsAhat
);
FastTMult
(
sPsMinv
,
sFsIhatsSCTsAhat
,
*
udot
);
ColMatrix
result1
=*
udot
;
ColMatrix
result2
=*
qdot
;
ColMatrix
result3
=*
q
;
int
num
=
result1
.
GetNumRows
();
ColMatrix
result4
(
num
+
1
);
result4
.
Zeros
();
EPdotdot_udot
(
result1
,
result2
,
result3
,
result4
);
FastAssign
(
result4
,
*
qdotdot
);
FastMult
(
sP
,
*
udot
,
sPudot
);
FastAdd
(
sSCTsAhat
,
sPudot
,
sAhat
);
}
else
if
(
type
==
4
){
FastTMult
(
sSC
,
parent
->
sAhat
,
sSCTsAhat
);
FastMult
(
sIhat
,
sSCTsAhat
,
sIhatsSCTsAhat
);
FastSubt
(
sFhat
,
sIhatsSCTsAhat
,
sFsIhatsSCTsAhat
);
FastTMult
(
sPsMinv
,
sFsIhatsSCTsAhat
,
*
udot
);
ColMatrix
result1
=*
udot
;
ColMatrix
result2
=*
qdot
;
ColMatrix
result3
=*
q
;
int
num
=
result1
.
GetNumRows
();
ColMatrix
result4
(
num
+
1
);
result4
.
Zeros
();
EPdotdot_udot
(
result1
,
result2
,
result3
,
result4
);
FastAssign
(
result4
,
*
qdotdot
);
FastMult
(
sP
,
*
udot
,
sPudot
);
FastAdd
(
sSCTsAhat
,
sPudot
,
sAhat
);
}
else
if
(
type
==
5
){
FastTMult
(
sSC
,
parent
->
sAhat
,
sSCTsAhat
);
FastMult
(
sIhat
,
sSCTsAhat
,
sIhatsSCTsAhat
);
FastSubt
(
sFhat
,
sIhatsSCTsAhat
,
sFsIhatsSCTsAhat
);
FastTMult
(
sPsMinv
,
sFsIhatsSCTsAhat
,
*
udot
);
ColMatrix
temp_u
=
*
udot
;
ColMatrix
result1
(
3
);
result1
(
1
)
=
0.0
;
result1
(
2
)
=
temp_u
(
1
);
result1
(
3
)
=
temp_u
(
2
);
ColMatrix
result2
=*
qdot
;
ColMatrix
result3
=*
q
;
int
num
=
result1
.
GetNumRows
();
ColMatrix
result4
(
num
+
1
);
result4
.
Zeros
();
EPdotdot_udot
(
result1
,
result2
,
result3
,
result4
);
FastAssign
(
result4
,
*
qdotdot
);
FastMult
(
sP
,
*
udot
,
sPudot
);
FastAdd
(
sSCTsAhat
,
sPudot
,
sAhat
);
}
else
if
(
type
==
6
){
FastTMult
(
sSC
,
parent
->
sAhat
,
sSCTsAhat
);
FastMult
(
sIhat
,
sSCTsAhat
,
sIhatsSCTsAhat
);
FastSubt
(
sFhat
,
sIhatsSCTsAhat
,
sFsIhatsSCTsAhat
);
FastTMult
(
sPsMinv
,
sFsIhatsSCTsAhat
,
*
udot
);
ColMatrix
temp_u
=
*
udot
;
int
tt
=
temp_u
.
GetNumRows
()
+
1
;
ColMatrix
result1
(
tt
);
result1
(
1
)
=
0.0
;
for
(
int
k
=
2
;
k
<=
tt
;
k
++
){
result1
(
k
)
=
temp_u
(
k
-
1
);
}
ColMatrix
result2
=*
qdot
;
ColMatrix
result3
=*
q
;
int
num
=
result1
.
GetNumRows
();
ColMatrix
result4
(
num
+
1
);
result4
.
Zeros
();
EPdotdot_udot
(
result1
,
result2
,
result3
,
result4
);
FastAssign
(
result4
,
*
qdotdot
);
FastMult
(
sP
,
*
udot
,
sPudot
);
FastAdd
(
sSCTsAhat
,
sPudot
,
sAhat
);
}
else
{
cout
<<
"Joint type not recognized in onbody.cpp LocalForwardSubsitution() "
<<
type
<<
endl
;
exit
(
-
1
);
}
CalculateAcceleration
();
}
void
OnBody
::
CalculateAcceleration
(){
}
Event Timeline
Log In to Comment