1 /*
2 *_________________________________________________________________________*
3 * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
4 * DESCRIPTION: SEE READ-ME *
5 * FILE NAME: body23joint.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
19 #include "body23joint.h"
20 #include "point.h"
21 #include "matrixfun.h"
22 #include "body.h"
23 #include "fastmatrixops.h"
24 #include "norm.h"
25 #include "eulerparameters.h"
26 #include "matrices.h"
27 #include <iomanip>
28
29
Body23Joint()30 Body23Joint::Body23Joint(){
31 DimQandU(4,2);
32 }
~Body23Joint()33 Body23Joint::~Body23Joint(){
34 }
35
GetType()36 JointType Body23Joint::GetType(){
37 return BODY23JOINT;
38 }
39
ReadInJointData(std::istream & in)40 bool Body23Joint::ReadInJointData(std::istream& in){
41 return true;
42 }
43
WriteOutJointData(std::ostream & out)44 void Body23Joint::WriteOutJointData(std::ostream& out){
45 }
46
GetForward_sP()47 Matrix Body23Joint::GetForward_sP(){
48 Vect3 temp = -(point2->position); // This is vector from joint to CM
49 Matrix sP(6,2);
50 sP.Zeros();
51
52 sP(2,1) =1.0;
53 sP(3,2) =1.0;
54 sP(5,2) = temp(1);
55 sP(6,1) = -temp(1);
56 return sP;
57 }
58
UpdateForward_sP(Matrix & sP)59 void Body23Joint::UpdateForward_sP( Matrix& sP){
60 // sP is constant, do nothing.
61 // linear part is not constant
62 }
63
GetBackward_sP()64 Matrix Body23Joint::GetBackward_sP(){
65 cout<<" -----------"<<endl;
66 cout<<"Am I here "<<endl;
67 cout<<" -----------"<<endl;
68 Vect3 temp = (point2->position); // This is vector from CM to joint
69 Matrix sP(6,2);
70 sP.Zeros();
71 sP(2,1) =1.0;
72 sP(3,2) =1.0;
73
74 sP(5,2) = temp(1);
75 sP(6,1) = -temp(1);
76
77 return sP;
78 }
79
UpdateBackward_sP(Matrix & sP)80 void Body23Joint::UpdateBackward_sP( Matrix& sP){
81 // sP is constant, do nothing.
82 }
83
ComputeLocalTransform()84 void Body23Joint::ComputeLocalTransform(){
85 Mat3x3 ko_C_k;
86 // Obtain the transformation matrix from euler parameters
87 EP_Transformation(q, ko_C_k);
88 FastMult(pk_C_ko,ko_C_k,pk_C_k);
89 }
90
91
ForwardKinematics()92 void Body23Joint::ForwardKinematics(){
93 Vect3 result1,result2,result3,result4,result5;
94 Vect3 pk_w_k;
95
96
97 //cout<<"Check in spherical "<<q<<" "<<qdot<<endl;
98 EP_Normalize(q);
99
100
101 // orientations
102 ComputeForwardTransforms();
103
104
105 //----------------------------------//
106 // COMPUTE POSITION VECTOR R12 aka GAMMA
107
108 FastNegMult(pk_C_k,point2->position,result1); // parents basis
109 FastAdd(result1,point1->position,r12);
110
111 // compute position vector r21
112 FastNegMult(k_C_pk,r12,r21);
113
114
115 //----------------------------------//
116 // COMPUTE GLOBAL LOCATION
117 FastMult(body1->n_C_k,(body1->GetPoint(2))->position,result1);
118 FastAdd(result1,body1->r,result1);
119 FastNegMult(body2->n_C_k,(body2->GetPoint(1))->position,result2);
120 FastAdd(result1,result2,body2->r);
121
122 ColMatrix temp_u(3);
123 qdot_to_u(q, temp_u, qdot);
124
125 temp_u(1)=0.0;
126 u(1) = temp_u(2);
127 u(2) = temp_u(3);
128
129
130 //-----------------------------------
131 // angular velocities
132
133 FastAssign(temp_u,pk_w_k);
134 FastTMult(pk_C_k,body1->omega_k,result1);
135 FastAdd(result1,pk_w_k,body2->omega_k);
136 FastMult(body2->n_C_k,body2->omega_k,body2->omega); // June 1 checked with Lammps
137 //-----------------------------------
138
139 // compute velocities
140 FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
141 FastAdd(body1->v_k,result1,result2);
142 FastTMult(pk_C_k,result2,result1); // In body basis
143
144 FastCross((body2->GetPoint(1))->position,body2->omega_k,result2);
145 FastAdd(result1,result2,body2->v_k); // In body basis
146 FastMult(body2->n_C_k,body2->v_k,body2->v);
147
148 //------------------------------------------
149 //Compute the KE
150 Matrix tempke;
151 tempke = T(body2->v_k)*(body2->v_k);
152 double ke = 0.0;
153 ke = body2->mass*tempke(1,1);
154 FastMult(body2->inertia,body2->omega_k,result1);
155 tempke= T(body2->omega_k)*result1;
156 ke = 0.5*ke + 0.5*tempke(1,1);
157 body2->KE=ke;
158
159 //-----------------------------------
160 // compute state explicit angular acceleration // Has to be in body basis
161 FastTMult(pk_C_k,body1->alpha_t,result2);
162 FastCross(body2->omega_k,pk_w_k,result1);
163 FastAdd(result1,result2,body2->alpha_t);
164
165 //-----------------------------------
166 // compute state explicit acceleration
167 // NEED TO DO THIS COMPLETELY IN BODY BASIS
168
169 FastCross(body1->omega_k,(body1->GetPoint(2))->position,result1);
170 FastCross(body1->omega_k,result1,result2);
171 FastTMult(pk_C_k,result2,result1);
172
173 //FastCross(body2->omega_k,-(body2->GetPoint(1))->position,result3);
174 FastCross((body2->GetPoint(1))->position,body2->omega_k,result3);
175 FastCross(body2->omega_k,result3,result2);
176 FastAdd(result1,result2,result3); //wxwxr in body basis
177
178 FastCross(body1->alpha_t,(body1->GetPoint(2))->position,result4);
179
180 FastTMult(pk_C_k,result4,result5);
181 FastAssign(result5,result4);
182
183 FastCross((body2->GetPoint(1))->position,body2->alpha_t,result2);
184 FastAdd(result2,result4,result1); //alphaxr in body basis
185
186 FastTMult(pk_C_k,body1->a_t,result2);
187 FastTripleSum(result3,result1,result2,body2->a_t); // in body basis
188
189
190 //-----------------------------------
191 }
192
193 // NOTE: NOT USING BACKWARDKINEMATICS AT PRESENT
BackwardKinematics()194 void Body23Joint::BackwardKinematics(){
195 cout<<"what about here "<<endl;
196 Vect3 result1,result2,result3,result4,result5;
197 Vect3 k_w_pk;
198
199 // orientations
200 ComputeBackwardTransforms();
201
202
203 // compute position vector r21
204 //r21 = point2->position - k_C_pk * point1->position;
205 FastMult(k_C_pk,point1->position,result1);
206 FastSubt(point2->position,result1,r21);
207
208
209 // compute position vector r21
210 FastNegMult(pk_C_k,r21,r12);
211
212 // compute global location
213 // body1->r = body2->r + body2->n_C_k * r21;
214 FastMult(body2->n_C_k,r21,result1);
215 FastAdd(body2->r,result1,body1->r);
216
217 // compute qdot (for revolute joint qdot = u)
218 // qdot = u
219 ColMatrix us(3);
220 /*us(1)=0;
221 us(2)=u(1);
222 us(3)=u(2);*/
223 EP_Derivatives(q,u,qdot);
224
225 // angular velocities
226
227 FastMult(body2->n_C_k,u,result2);
228 FastAdd(body2->omega,result2,body1->omega);
229 FastAssign(u,k_w_pk);
230 FastMult(pk_C_k,body2->omega_k,result1);
231 FastSubt(result1,k_w_pk,body1->omega_k);
232 cout<<"The program was here"<<endl;
233
234 // compute velocities
235 FastCross(body2->omega_k,r21,result1);
236 FastCross(point1->position,k_w_pk,result2);
237 FastAdd(body2->v_k,result1,result3);
238 FastMult(pk_C_k,result3,result4);
239 FastAdd(result2,result4,body1->v_k);
240 FastMult(body1->n_C_k,body1->v_k,body1->v);
241
242 // compute state explicit angular acceleration
243 FastCross(body1->omega_k,k_w_pk,result1);
244 FastMult(pk_C_k,body2->alpha_t,result2);
245 FastAdd(result1,result2,body1->alpha_t);
246
247 // compute state explicit acceleration
248 FastCross(body2->alpha_t,point2->position,result1);
249 FastCross(body2->omega_k,point2->position,result2);
250 FastCross(body2->omega_k,result2,result3);
251 FastTripleSum(body2->a_t,result1,result3,result4);
252 FastMult(pk_C_k,result4,result5);
253
254 FastCross(point1->position,body1->alpha_t,result1);
255 FastCross(point1->position,body1->omega_k,result2);
256 FastCross(body1->omega_k,result2,result3);
257
258 FastTripleSum(result5,result1,result3,body1->a_t);
259
260 }
261