Page MenuHomec4science
Files F73183756

freebodyjoint.cpp
No OneTemporary
Actions

File Metadata

Created
Fri, Jul 19, 10:55

freebodyjoint.cpp
View Options

/*
*_________________________________________________________________________*
* POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
* DESCRIPTION: SEE READ-ME *
* FILE NAME: freebodyjoint.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 "freebodyjoint.h"
#include "point.h"
#include "matrixfun.h"
#include "body.h"
#include "fastmatrixops.h"
#include "norm.h"
#include "eulerparameters.h"
#include "matrices.h"
#include <iomanip>
FreeBodyJoint::FreeBodyJoint(){
DimQandU(7,6);
}
FreeBodyJoint::~FreeBodyJoint(){
}
JointType FreeBodyJoint::GetType(){
return FREEBODYJOINT;
}
bool FreeBodyJoint::ReadInJointData(std::istream& in){
return true;
}
void FreeBodyJoint::WriteOutJointData(std::ostream& out){
}
void FreeBodyJoint::ComputeLocalTransform(){
Mat3x3 ko_C_k;
EP_Transformation(q, ko_C_k);
FastMult(pk_C_ko,ko_C_k,pk_C_k);
}
Matrix FreeBodyJoint::GetForward_sP(){
Mat6x6 sP;
//sP.Identity();
sP.Zeros();
Mat3x3 temp0=T(pk_C_k);
for(int i=1;i<4;i++){
sP(i,i)=1.0;
for(int j=1;j<4;j++){
sP(3+i,3+j)=temp0(i,j);
}
}
return sP;
}
Matrix FreeBodyJoint::GetBackward_sP(){
Mat6x6 sP;
sP.Identity();
sP =-1.0*sP;
cout<<"Did I come here in "<<endl;
return sP;
}
void FreeBodyJoint::UpdateForward_sP( Matrix& sP){
// do nothing
}
void FreeBodyJoint::UpdateBackward_sP( Matrix& sP){
// do nothing
}
void FreeBodyJoint::ForwardKinematics(){
//cout<<"Check in freebody "<<q<<" "<<qdot<<endl;
EP_Normalize(q);
// COMMENT STEP1: CALCULATE ORIENTATIONS
ComputeForwardTransforms();
//COMMENT STEP2: CALCULATE POSITION VECTORS
Vect3 result1, result2, result3, result4;
result1.BasicSet(0,q.BasicGet(4));
result1.BasicSet(1,q.BasicGet(5));
result1.BasicSet(2,q.BasicGet(6));
FastAssign(result1,r12);
FastNegMult(k_C_pk,r12,r21);
FastAssign(r12,body2->r);
//COMMENT STEP3: CALCULATE QDOT
qdot_to_u(q, u, qdot);
Vect3 WN;
WN.BasicSet(0,u.BasicGet(0));
WN.BasicSet(1,u.BasicGet(1));
WN.BasicSet(2,u.BasicGet(2));
Vect3 VN;
VN.BasicSet(0,u.BasicGet(3));
VN.BasicSet(1,u.BasicGet(4));
VN.BasicSet(2,u.BasicGet(5));
FastAssign(WN,body2->omega_k);
Vect3 pk_w_k;
FastMult(body2->n_C_k,WN,pk_w_k);
FastAssign(pk_w_k,body2->omega);
//COMMENT STEP5: CALCULATE VELOCITES
FastAssign(VN,body2->v);
FastTMult(body2->n_C_k,body2->v,body2->v_k);
//CALCULATE KE
Matrix tempke;
tempke = T(body2->v)*(body2->v);
double ke = 0.0;
ke = body2->mass*tempke(1,1);
FastMult(body2->inertia,body2->omega_k,result1);
tempke= T(body2->omega_k)*result1;
ke = 0.5*ke + 0.5*tempke(1,1);
body2->KE=ke;
//COMMENT STEP6: CALCULATE STATE EXPLICIT ANGULAR ACCELERATIONS
body2->alpha_t.Zeros();
//COMMENT STEP7: CALCULATE STATE EXPLICIT ACCELERATIONS
body2->a_t.Zeros();
}
void FreeBodyJoint::BackwardKinematics(){
cout<<"Did I come here "<<endl;
}

Event Timeline

c4science · Help