Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F78665192
system.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
Thu, Aug 22, 06:57
Size
14 KB
Mime Type
text/x-c
Expires
Sat, Aug 24, 06:57 (2 d)
Engine
blob
Format
Raw Data
Handle
20078409
Attached To
rLAMMPS lammps
system.cpp
View Options
/*
*_________________________________________________________________________*
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME *
* FILE NAME: system.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 "system.h"
#include "body.h"
#include "joint.h"
#include <math.h>
System
::
System
(){
mappings
=
NULL
;
}
System
::~
System
(){
Delete
();
}
void
System
::
Delete
(){
delete
[]
mappings
;
bodies
.
DeleteValues
();
joints
.
DeleteValues
();
}
int
System
::
GetNumBodies
(){
return
bodies
.
GetNumElements
();
}
int
*
System
::
GetMappings
()
{
return
mappings
;
}
void
System
::
AddBody
(
Body
*
body
){
bodies
.
Append
(
body
);
}
void
System
::
AddJoint
(
Joint
*
joint
){
joints
.
Append
(
joint
);
}
void
System
::
SetTime
(
double
t
){
time
=
t
;
}
double
System
::
GetTime
(){
return
time
;
}
void
System
::
ComputeForces
(){
// NOT DOING ANYTHING AT THIS TIME
}
bool
System
::
ReadIn
(
istream
&
in
){
int
numbodies
;
Body
*
body
;
int
bodytype
;
char
bodyname
[
256
];
int
index
;
// get number of bodies
in
>>
numbodies
;
// bodies loop
for
(
int
i
=
0
;
i
<
numbodies
;
i
++
){
// error check
in
>>
index
;
if
(
index
!=
i
){
cerr
<<
"Error reading bodies"
<<
endl
;
return
false
;
}
in
>>
bodytype
>>
bodyname
;
body
=
NewBody
(
bodytype
);
// type check
if
(
!
body
){
cerr
<<
"Unrecognized body type '"
<<
bodytype
<<
"'"
<<
endl
;
return
false
;
}
// add the body
AddBody
(
body
);
// set generic body info
body
->
ChangeName
(
bodyname
);
// read in the rest of its data
if
(
!
body
->
ReadIn
(
in
))
return
false
;
}
// create a temporary array for fast indexed access
Body
**
bodyarray
=
bodies
.
CreateArray
();
int
numjoints
;
int
jointtype
;
char
jointname
[
256
];
Joint
*
joint
;
int
body1
,
body2
;
int
point1
,
point2
;
// get number of joints
in
>>
numjoints
;
// joints loop
for
(
int
i
=
0
;
i
<
numjoints
;
i
++
){
// error check
in
>>
index
;
if
(
index
!=
i
){
cerr
<<
"Error reading joints"
<<
endl
;
return
false
;
}
in
>>
jointtype
>>
jointname
;
joint
=
NewJoint
(
jointtype
);
// joint type check
if
(
!
joint
){
cerr
<<
"Unrecognized joint type '"
<<
jointtype
<<
"'"
<<
endl
;
return
false
;
}
// add the joint
AddJoint
(
joint
);
// set the generic joint info
joint
->
ChangeName
(
jointname
);
in
>>
body1
>>
body2
;
if
(
!
(
body1
<
numbodies
)
||
!
(
body2
<
numbodies
)
){
cerr
<<
"Body index out of range"
<<
endl
;
delete
[]
bodyarray
;
return
false
;
}
joint
->
SetBodies
(
bodyarray
[
body1
],
bodyarray
[
body2
]);
bodyarray
[
body1
]
->
AddJoint
(
joint
);
bodyarray
[
body2
]
->
AddJoint
(
joint
);
in
>>
point1
>>
point2
;
joint
->
SetPoints
(
bodyarray
[
body1
]
->
GetPoint
(
point1
),
bodyarray
[
body2
]
->
GetPoint
(
point2
));
// read in the rest of its data
if
(
!
joint
->
ReadIn
(
in
)){
delete
[]
bodyarray
;
return
false
;
}
}
// delete the temporary array
delete
[]
bodyarray
;
return
true
;
}
void
System
::
WriteOut
(
ostream
&
out
){
// number of bodies
out
<<
bodies
.
GetNumElements
()
<<
endl
;
// bodies loop
int
i
=
0
;
Body
*
body
;
ListElement
<
Body
>*
b_ele
=
bodies
.
GetHeadElement
();
while
(
b_ele
!=
0
){
out
<<
i
<<
' '
;
body
=
b_ele
->
value
;
// set the body ID for later identification
body
->
SetID
(
i
);
// write out the data
body
->
WriteOut
(
out
);
i
++
;
b_ele
=
b_ele
->
next
;
}
// number of joints
out
<<
joints
.
GetNumElements
()
<<
endl
;
// joints loop
i
=
0
;
Joint
*
joint
;
ListElement
<
Joint
>*
j_ele
=
joints
.
GetHeadElement
();
while
(
j_ele
!=
0
){
out
<<
i
<<
' '
;
joint
=
j_ele
->
value
;
// set the joint ID for later identification
joint
->
SetID
(
i
);
// write out the data
joint
->
WriteOut
(
out
);
i
++
;
j_ele
=
j_ele
->
next
;
}
}
void
System
::
ClearBodyIDs
(){
ListElement
<
Body
>*
current
=
bodies
.
GetHeadElement
();
while
(
current
){
current
->
value
->
SetID
(
0
);
current
=
current
->
next
;
}
}
void
System
::
ClearJointIDs
(){
ListElement
<
Joint
>*
current
=
joints
.
GetHeadElement
();
while
(
current
){
current
->
value
->
SetID
(
0
);
current
=
current
->
next
;
}
}
void
System
::
Create_DegenerateSystem
(
int
&
nfree
,
int
*
freelist
,
double
*&
masstotal
,
double
**&
inertia
,
double
**&
xcm
,
double
**&
vcm
,
double
**&
omega
,
double
**&
ex_space
,
double
**&
ey_space
,
double
**&
ez_space
){
//-------------------------------------------------------------------------//
// Declaring Temporary Entities
//-------------------------------------------------------------------------//
Body
*
body
=
NULL
;
Body
*
prev
;
Body
*
Inertial
;
Point
*
origin
;
Joint
*
joint
;
Point
*
point_CM
;
Point
*
point_p
;
Point
*
point_k
;
Point
*
point_ch
=
NULL
;
Vect3
r1
,
r2
,
r3
,
v1
,
v2
,
v3
;
Mat3x3
IM
,
N
,
PKCK
,
PKCN
;
ColMatrix
qo
,
uo
,
q
,
qdot
,
w
;
mappings
=
new
int
[
nfree
];
for
(
int
i
=
0
;
i
<
nfree
;
i
++
)
{
mappings
[
i
]
=
freelist
[
i
];
}
qo
.
Dim
(
4
);
uo
.
Dim
(
3
);
q
.
Dim
(
4
);
qdot
.
Dim
(
4
);
PKCN
.
Identity
();
PKCK
.
Identity
();
w
.
Dim
(
3
);
//-------------------------------------------------------------------------//
// Setting up Inertial Frame, gravity and Origin
//-------------------------------------------------------------------------//
Inertial
=
new
InertialFrame
;
AddBody
(
Inertial
);
Vect3
temp1
;
temp1
.
Zeros
();
((
InertialFrame
*
)
Inertial
)
->
SetGravity
(
temp1
);
origin
=
new
FixedPoint
(
temp1
);
Inertial
->
AddPoint
(
origin
);
//-------------------------------------------------------------------------//
double
**
xh1
=
new
double
*
[
nfree
];
double
**
xh2
=
new
double
*
[
nfree
];
for
(
int
i
=
0
;
i
<
nfree
;
i
++
){
xh1
[
i
]
=
new
double
[
3
];
xh2
[
i
]
=
new
double
[
3
];
}
for
(
int
i
=
0
;
i
<
nfree
;
i
++
){
for
(
int
j
=
0
;
j
<
3
;
j
++
){
xh1
[
i
][
j
]
=
xcm
[
mappings
[
i
]
-
1
][
j
];
}
}
//-------------------------------------------------------------------------//
// Begin looping over each body for recursive kinematics
//-------------------------------------------------------------------------//
for
(
int
i
=
0
;
i
<
nfree
;
i
++
){
prev
=
Inertial
;
point_p
=
origin
;
body
=
new
RigidBody
;
body
->
mass
=
masstotal
[
mappings
[
i
]
-
1
];
IM
(
1
,
1
)
=
inertia
[
mappings
[
i
]
-
1
][
0
];
IM
(
2
,
2
)
=
inertia
[
mappings
[
i
]
-
1
][
1
];
IM
(
3
,
3
)
=
inertia
[
mappings
[
i
]
-
1
][
2
];
IM
(
1
,
2
)
=
0.0
;
IM
(
1
,
3
)
=
0.0
;
IM
(
2
,
3
)
=
0.0
;
IM
(
2
,
1
)
=
IM
(
1
,
2
);
IM
(
3
,
1
)
=
IM
(
1
,
3
);
IM
(
3
,
2
)
=
IM
(
2
,
3
);
body
->
inertia
=
IM
;
//-------------------------------------------------------//
for
(
int
k
=
0
;
k
<
3
;
k
++
){
r1
(
k
+
1
)
=
xh1
[
i
][
k
]
-
xcm
[
mappings
[
i
]
-
1
][
k
];
r3
(
k
+
1
)
=
xcm
[
mappings
[
i
]
-
1
][
k
];
r3
(
k
+
1
)
=
xh2
[
i
][
k
]
-
xcm
[
mappings
[
i
]
-
1
][
k
];
}
r2
.
Zeros
();
for
(
int
k
=
1
;
k
<=
3
;
k
++
){
N
(
k
,
1
)
=
ex_space
[
mappings
[
i
]
-
1
][
k
-
1
];
N
(
k
,
2
)
=
ey_space
[
mappings
[
i
]
-
1
][
k
-
1
];
N
(
k
,
3
)
=
ez_space
[
mappings
[
i
]
-
1
][
k
-
1
];
}
PKCK
=
T
(
N
);
PKCN
=
T
(
N
);
q
.
Zeros
();
EP_FromTransformation
(
q
,
N
);
r1
=
PKCN
*
r1
;
r3
=
PKCN
*
r3
;
for
(
int
k
=
1
;
k
<=
3
;
k
++
){
w
(
k
)
=
omega
[
mappings
[
i
]
-
1
][
k
-
1
];
}
Vect3
cart_r
,
cart_v
;
for
(
int
k
=
1
;
k
<=
3
;
k
++
){
cart_r
(
k
)
=
xcm
[
mappings
[
i
]
-
1
][
k
-
1
];
cart_v
(
k
)
=
vcm
[
mappings
[
i
]
-
1
][
k
-
1
];
}
w
=
PKCN
*
w
;
EP_Derivatives
(
q
,
w
,
qdot
);
//-------------------------------------------------------------------------//
// Create bodies and joints with associated properties for POEMS
//-------------------------------------------------------------------------//
point_CM
=
new
FixedPoint
(
r2
);
point_k
=
new
FixedPoint
(
r1
);
point_ch
=
new
FixedPoint
(
r3
);
body
->
AddPoint
(
point_CM
);
body
->
AddPoint
(
point_k
);
body
->
AddPoint
(
point_ch
);
AddBody
(
body
);
Mat3x3
One
;
One
.
Identity
();
ColMatrix
qq
=
Stack
(
q
,
cart_r
);
ColMatrix
vv
=
Stack
(
qdot
,
cart_v
);
joint
=
new
FreeBodyJoint
;
AddJoint
(
joint
);
joint
->
SetBodies
(
prev
,
body
);
body
->
AddJoint
(
joint
);
prev
->
AddJoint
(
joint
);
joint
->
SetPoints
(
point_p
,
point_k
);
joint
->
SetZeroOrientation
(
One
);
joint
->
DimQandU
(
7
,
6
);
joint
->
SetInitialState
(
qq
,
vv
);
joint
->
ForwardKinematics
();
}
for
(
int
i
=
0
;
i
<
nfree
;
i
++
)
{
delete
[]
xh1
[
i
];
delete
[]
xh2
[
i
];
}
delete
[]
xh1
;
delete
[]
xh2
;
}
void
System
::
Create_System_LAMMPS
(
int
numbodies
,
double
*
mass
,
double
**
inertia
,
double
**
xcm
,
double
**
xjoint
,
double
**
vcm
,
double
**
omega
,
double
**
ex_space
,
double
**
ey_space
,
double
**
ez_space
,
int
b
,
int
*
mapping
,
int
count
){
//-------------------------------------------------------------------------//
// Declaring Temporary Entities
//-------------------------------------------------------------------------//
Body
*
body
=
NULL
;
Body
*
prev
;
Body
*
Inertial
;
Point
*
origin
;
Joint
*
joint
;
Point
*
point_CM
;
Point
*
point_p
;
Point
*
point_k
;
Point
*
point_ch
=
NULL
;
Vect3
r1
,
r2
,
r3
,
v1
,
v2
,
v3
;
Mat3x3
IM
,
N
,
PKCK
,
PKCN
;
ColMatrix
qo
,
uo
,
q
,
qdot
,
w
;
Vect3
cart_r
,
cart_v
;
mappings
=
new
int
[
b
];
for
(
int
i
=
0
;
i
<
b
;
i
++
){
mappings
[
i
]
=
mapping
[
i
];
}
qo
.
Dim
(
4
);
uo
.
Dim
(
3
);
q
.
Dim
(
4
);
qdot
.
Dim
(
4
);
PKCN
.
Identity
();
PKCK
.
Identity
();
w
.
Dim
(
3
);
//-------------------------------------------------------------------------//
// Setting up Inertial Frame, gravity and Origin
//-------------------------------------------------------------------------//
Inertial
=
new
InertialFrame
;
AddBody
(
Inertial
);
Vect3
temp1
;
temp1
.
Zeros
();
((
InertialFrame
*
)
Inertial
)
->
SetGravity
(
temp1
);
origin
=
new
FixedPoint
(
temp1
);
Inertial
->
AddPoint
(
origin
);
//-------------------------------------------------------------------------//
double
**
xh1
;
double
**
xh2
;
xh1
=
new
double
*
[
b
];
xh2
=
new
double
*
[
b
];
for
(
int
i
=
0
;
i
<
b
;
i
++
){
xh1
[
i
]
=
new
double
[
3
];
xh2
[
i
]
=
new
double
[
3
];
}
for
(
int
j
=
0
;
j
<
3
;
j
++
){
xh1
[
0
][
j
]
=
xcm
[
mapping
[
0
]
-
1
][
j
];
xh2
[
b
-
1
][
j
]
=
xcm
[
mapping
[
b
-
1
]
-
1
][
j
];
}
for
(
int
i
=
0
;
i
<
b
-
1
;
i
++
){
for
(
int
j
=
0
;
j
<
3
;
j
++
){
xh1
[
i
+
1
][
j
]
=
xjoint
[
mapping
[
i
]
-
count
-
1
][
j
];
}
}
for
(
int
i
=
0
;
i
<
b
-
1
;
i
++
){
for
(
int
j
=
0
;
j
<
3
;
j
++
){
xh2
[
i
][
j
]
=
xjoint
[
mapping
[
i
]
-
count
-
1
][
j
];
}
}
//-------------------------------------------------------------------------//
// Begin looping over each body for recursive kinematics
//-------------------------------------------------------------------------//
for
(
int
i
=
0
;
i
<
b
;
i
++
){
if
(
i
==
0
){
prev
=
Inertial
;
point_p
=
origin
;
}
else
{
prev
=
body
;
point_p
=
point_ch
;
}
body
=
new
RigidBody
;
body
->
mass
=
mass
[
mapping
[
i
]
-
1
];
IM
(
1
,
1
)
=
inertia
[
mapping
[
i
]
-
1
][
0
];
IM
(
2
,
2
)
=
inertia
[
mapping
[
i
]
-
1
][
1
];
IM
(
3
,
3
)
=
inertia
[
mapping
[
i
]
-
1
][
2
];
IM
(
1
,
2
)
=
0.0
;
IM
(
1
,
3
)
=
0.0
;
IM
(
2
,
3
)
=
0.0
;
IM
(
2
,
1
)
=
IM
(
1
,
2
);
IM
(
3
,
1
)
=
IM
(
1
,
3
);
IM
(
3
,
2
)
=
IM
(
2
,
3
);
body
->
inertia
=
IM
;
//-------------------------------------------------------//
for
(
int
k
=
0
;
k
<
3
;
k
++
){
r1
(
k
+
1
)
=
xh1
[
i
][
k
]
-
xcm
[
mapping
[
i
]
-
1
][
k
];
r3
(
k
+
1
)
=
xh2
[
i
][
k
]
-
xcm
[
mapping
[
i
]
-
1
][
k
];
}
r2
.
Zeros
();
for
(
int
k
=
1
;
k
<=
3
;
k
++
){
N
(
k
,
1
)
=
ex_space
[
mapping
[
i
]
-
1
][
k
-
1
];
N
(
k
,
2
)
=
ey_space
[
mapping
[
i
]
-
1
][
k
-
1
];
N
(
k
,
3
)
=
ez_space
[
mapping
[
i
]
-
1
][
k
-
1
];
}
if
(
i
==
0
){
PKCK
=
T
(
N
);
PKCN
=
T
(
N
);
q
.
Zeros
();
EP_FromTransformation
(
q
,
N
);
r1
=
PKCN
*
r1
;
r3
=
PKCN
*
r3
;
for
(
int
k
=
1
;
k
<=
3
;
k
++
){
w
(
k
)
=
omega
[
mappings
[
i
]
-
1
][
k
-
1
];
}
for
(
int
k
=
1
;
k
<=
3
;
k
++
){
cart_r
(
k
)
=
xcm
[
mappings
[
i
]
-
1
][
k
-
1
];
cart_v
(
k
)
=
vcm
[
mappings
[
i
]
-
1
][
k
-
1
];
}
w
=
PKCN
*
w
;
EP_Derivatives
(
q
,
w
,
qdot
);
}
else
{
PKCK
=
PKCN
*
N
;
PKCN
=
T
(
N
);
q
.
Zeros
();
EP_FromTransformation
(
q
,
PKCK
);
r1
=
PKCN
*
r1
;
r3
=
PKCN
*
r3
;
for
(
int
k
=
1
;
k
<=
3
;
k
++
){
w
(
k
)
=
omega
[
mapping
[
i
]
-
1
][
k
-
1
]
-
omega
[
mapping
[
i
-
1
]
-
1
][
k
-
1
];
}
w
=
PKCN
*
w
;
EP_Derivatives
(
q
,
w
,
qdot
);
}
//-------------------------------------------------------------------------//
// Create bodies and joints with associated properties for POEMS
//-------------------------------------------------------------------------//
point_CM
=
new
FixedPoint
(
r2
);
point_k
=
new
FixedPoint
(
r1
);
point_ch
=
new
FixedPoint
(
r3
);
body
->
AddPoint
(
point_CM
);
body
->
AddPoint
(
point_k
);
body
->
AddPoint
(
point_ch
);
AddBody
(
body
);
Mat3x3
One
;
One
.
Identity
();
if
(
i
==
0
){
ColMatrix
qq
=
Stack
(
q
,
cart_r
);
ColMatrix
vv
=
Stack
(
qdot
,
cart_v
);
joint
=
new
FreeBodyJoint
;
AddJoint
(
joint
);
joint
->
SetBodies
(
prev
,
body
);
body
->
AddJoint
(
joint
);
prev
->
AddJoint
(
joint
);
joint
->
SetPoints
(
point_p
,
point_k
);
joint
->
SetZeroOrientation
(
One
);
joint
->
DimQandU
(
7
,
6
);
joint
->
SetInitialState
(
qq
,
vv
);
joint
->
ForwardKinematics
();
}
else
{
joint
=
new
SphericalJoint
;
AddJoint
(
joint
);
joint
->
SetBodies
(
prev
,
body
);
body
->
AddJoint
(
joint
);
prev
->
AddJoint
(
joint
);
joint
->
SetPoints
(
point_p
,
point_k
);
joint
->
SetZeroOrientation
(
One
);
joint
->
DimQandU
(
4
,
3
);
joint
->
SetInitialState
(
q
,
qdot
);
joint
->
ForwardKinematics
();
}
}
for
(
int
i
=
0
;
i
<
b
;
i
++
)
{
delete
[]
xh1
[
i
];
delete
[]
xh2
[
i
];
}
delete
[]
xh1
;
delete
[]
xh2
;
}
Event Timeline
Log In to Comment