1 /*
2 *_________________________________________________________________________*
3 * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
4 * DESCRIPTION: SEE READ-ME *
5 * FILE NAME: freebodyjoint.cpp *
6 * AUTHORS: See Author List *
7 * GRANTS: See Grants List *
8 * COPYRIGHT: (C) 2005 by Authors as listed in Author's List *
9 * LICENSE: Please see License Agreement *
10 * DOWNLOAD: Free at www.rpi.edu/~anderk5 *
11 * ADMINISTRATOR: Prof. Kurt Anderson *
12 * Computational Dynamics Lab *
13 * Rensselaer Polytechnic Institute *
14 * 110 8th St. Troy NY 12180 *
15 * CONTACT: anderk5@rpi.edu *
16 *_________________________________________________________________________*/
17
18 #include "freebodyjoint.h"
19 #include "point.h"
20 #include "matrixfun.h"
21 #include "body.h"
22 #include "fastmatrixops.h"
23 #include "norm.h"
24 #include "eulerparameters.h"
25 #include "matrices.h"
26 #include <iomanip>
27
28
FreeBodyJoint()29 FreeBodyJoint::FreeBodyJoint(){
30 DimQandU(7,6);
31 }
32
~FreeBodyJoint()33 FreeBodyJoint::~FreeBodyJoint(){
34 }
35
GetType()36 JointType FreeBodyJoint::GetType(){
37 return FREEBODYJOINT;
38 }
39
ReadInJointData(std::istream & in)40 bool FreeBodyJoint::ReadInJointData(std::istream& in){
41 return true;
42 }
43
WriteOutJointData(std::ostream & out)44 void FreeBodyJoint::WriteOutJointData(std::ostream& out){
45 }
46
ComputeLocalTransform()47 void FreeBodyJoint::ComputeLocalTransform(){
48 Mat3x3 ko_C_k;
49 EP_Transformation(q, ko_C_k);
50 FastMult(pk_C_ko,ko_C_k,pk_C_k);
51 }
52
GetForward_sP()53 Matrix FreeBodyJoint::GetForward_sP(){
54 Mat6x6 sP;
55 //sP.Identity();
56
57 sP.Zeros();
58 Mat3x3 temp0=T(pk_C_k);
59 for(int i=1;i<4;i++){
60 sP(i,i)=1.0;
61 for(int j=1;j<4;j++){
62 sP(3+i,3+j)=temp0(i,j);
63 }
64 }
65 return sP;
66 }
67
GetBackward_sP()68 Matrix FreeBodyJoint::GetBackward_sP(){
69 Mat6x6 sP;
70 sP.Identity();
71 sP =-1.0*sP;
72 cout<<"Did I come here in "<<endl;
73 return sP;
74 }
75
76
UpdateForward_sP(Matrix & sP)77 void FreeBodyJoint::UpdateForward_sP( Matrix& sP){
78 // do nothing
79 }
80
UpdateBackward_sP(Matrix & sP)81 void FreeBodyJoint::UpdateBackward_sP( Matrix& sP){
82 // do nothing
83 }
84
ForwardKinematics()85 void FreeBodyJoint::ForwardKinematics(){
86 //cout<<"Check in freebody "<<q<<" "<<qdot<<endl;
87 EP_Normalize(q);
88
89 // COMMENT STEP1: CALCULATE ORIENTATIONS
90 ComputeForwardTransforms();
91
92
93 //COMMENT STEP2: CALCULATE POSITION VECTORS
94 Vect3 result1, result2, result3, result4;
95
96 result1.BasicSet(0,q.BasicGet(4));
97 result1.BasicSet(1,q.BasicGet(5));
98 result1.BasicSet(2,q.BasicGet(6));
99
100 FastAssign(result1,r12);
101 FastNegMult(k_C_pk,r12,r21);
102
103 FastAssign(r12,body2->r);
104
105 //COMMENT STEP3: CALCULATE QDOT
106 qdot_to_u(q, u, qdot);
107
108
109 Vect3 WN;
110 WN.BasicSet(0,u.BasicGet(0));
111 WN.BasicSet(1,u.BasicGet(1));
112 WN.BasicSet(2,u.BasicGet(2));
113
114 Vect3 VN;
115 VN.BasicSet(0,u.BasicGet(3));
116 VN.BasicSet(1,u.BasicGet(4));
117 VN.BasicSet(2,u.BasicGet(5));
118
119 FastAssign(WN,body2->omega_k);
120
121 Vect3 pk_w_k;
122 FastMult(body2->n_C_k,WN,pk_w_k);
123 FastAssign(pk_w_k,body2->omega);
124
125
126
127 //COMMENT STEP5: CALCULATE VELOCITES
128 FastAssign(VN,body2->v);
129 FastTMult(body2->n_C_k,body2->v,body2->v_k);
130
131
132 //CALCULATE KE
133
134 Matrix tempke;
135 tempke = T(body2->v)*(body2->v);
136 double ke = 0.0;
137 ke = body2->mass*tempke(1,1);
138 FastMult(body2->inertia,body2->omega_k,result1);
139 tempke= T(body2->omega_k)*result1;
140 ke = 0.5*ke + 0.5*tempke(1,1);
141 body2->KE=ke;
142
143
144 //COMMENT STEP6: CALCULATE STATE EXPLICIT ANGULAR ACCELERATIONS
145 body2->alpha_t.Zeros();
146
147
148 //COMMENT STEP7: CALCULATE STATE EXPLICIT ACCELERATIONS
149 body2->a_t.Zeros();
150
151 }
152
BackwardKinematics()153 void FreeBodyJoint::BackwardKinematics(){
154 cout<<"Did I come here "<<endl;
155
156 }
157