Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F92079510
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, Nov 17, 04:28
Size
10 KB
Mime Type
text/x-c
Expires
Tue, Nov 19, 04:28 (2 d)
Engine
blob
Format
Raw Data
Handle
22202306
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