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