1 /*
2 *_________________________________________________________________________*
3 * POEMS: PARALLELIZABLE OPEN SOURCE EFFICIENT MULTIBODY SOFTWARE *
4 * DESCRIPTION: SEE READ-ME *
5 * FILE NAME: revolutejoint.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 "revolutejoint.h"
19 #include "point.h"
20 #include "matrixfun.h"
21 #include "body.h"
22 #include "fastmatrixops.h"
23
RevoluteJoint()24 RevoluteJoint::RevoluteJoint(){
25 DimQandU(1);
26 Vect3 axis;
27 axis.Zeros();
28 axis(3) = 1;
29 SetAxisPK(axis);
30 }
31
~RevoluteJoint()32 RevoluteJoint::~RevoluteJoint(){
33 }
34
GetType()35 JointType RevoluteJoint::GetType(){
36 return REVOLUTEJOINT;
37 }
38
SetAxisK(VirtualMatrix & axis)39 void RevoluteJoint::SetAxisK(VirtualMatrix& axis){
40 axis_k = axis;
41 axis_pk = pk_C_ko*axis_k;
42 }
43
SetAxisPK(VirtualMatrix & axis)44 void RevoluteJoint::SetAxisPK(VirtualMatrix& axis){
45 axis_pk = axis;
46 axis_k = T(pk_C_ko)*axis_pk;
47 }
48
ReadInJointData(std::istream & in)49 bool RevoluteJoint::ReadInJointData(std::istream& in){
50 Vect3 axis;
51 in >> axis;
52 SetAxisPK(axis);
53 return true;
54 }
55
WriteOutJointData(std::ostream & out)56 void RevoluteJoint::WriteOutJointData(std::ostream& out){
57 out << axis_pk;
58 }
59
GetForward_sP()60 Matrix RevoluteJoint::GetForward_sP(){
61 Vect3 v_kk;
62
63 // v_kk = axis x r
64 FastCross(point2->position,axis_k,v_kk);
65
66 // sP = [axis;v_kk]
67 return Stack(axis_k,v_kk);
68 }
69
UpdateForward_sP(Matrix & sP)70 void RevoluteJoint::UpdateForward_sP( Matrix& sP){
71 // sP is constant, do nothing.
72 }
73
GetBackward_sP()74 Matrix RevoluteJoint::GetBackward_sP(){
75 Vect3 v_kk;
76
77 // v_kk = axis x r
78 FastCross(point1->position,axis_pk,v_kk);
79
80 // sP = [axis;v_kk]
81 return -Stack(axis_pk,v_kk);;
82 }
83
UpdateBackward_sP(Matrix & sP)84 void RevoluteJoint::UpdateBackward_sP( Matrix& sP){
85 // sP is constant, do nothing.
86 }
87
ComputeLocalTransform()88 void RevoluteJoint::ComputeLocalTransform(){
89 Mat3x3 ko_C_k;
90 FastSimpleRotation(axis_k,q.BasicGet(0),ko_C_k);
91 // pk_C_k = pk_C_ko * ko_C_k;
92 FastMult(pk_C_ko,ko_C_k,pk_C_k);
93 }
94
ForwardKinematics()95 void RevoluteJoint::ForwardKinematics(){
96 Vect3 result1,result2,result3,result4,result5;
97 Vect3 pk_w_k;
98
99 // orientations
100 ComputeForwardTransforms();
101
102 // compute position vector r12
103 //r12 = point1->position - pk_C_k * point2->position;
104 FastMult(pk_C_k,point2->position,result1);
105 FastSubt(point1->position,result1,r12);// Jacks comment: needs flipping!!!
106
107 // compute position vector r21
108 FastNegMult(k_C_pk,r12,r21);
109
110 // compute global location
111 // body2->r = body1->r + body1->n_C_k * r12;
112 FastMult(body1->n_C_k,r12,result1);
113 FastAdd(body1->r,result1,body2->r);
114
115 // compute qdot (for revolute joint qdot = u)
116 // qdot = u
117 FastAssign(u,qdot);
118
119 // angular velocities
120 //body2->omega = body1->omega + body1->n_C_k * axis_pk * u;
121 //pk_w_k = axis_k * u;
122 //body2->omega_k = T(pk_C_k) * body1->omega_k + pk_w_k;
123 double u_double = u.BasicGet(0);
124 FastMult(u_double,axis_pk,result1);
125 FastMult(body1->n_C_k,result1,result2);
126 FastAdd(body1->omega,result2,body2->omega);
127 FastMult(u_double,axis_k,pk_w_k);
128 FastTMult(pk_C_k,body1->omega_k,result1);
129 FastAdd(result1,pk_w_k,body2->omega_k);
130
131 // compute velocities
132 FastCross(body1->omega_k,r12,result1);
133 FastCross(point2->position,pk_w_k,result2);
134 FastAdd(body1->v_k,result1,result3);
135 FastTMult(pk_C_k,result3,result4);
136 FastAdd(result2,result4,body2->v_k);
137 FastMult(body2->n_C_k,body2->v_k,body2->v);
138
139 // compute state explicit angular acceleration
140 FastCross(body2->omega_k,pk_w_k,result1);
141 FastTMult(pk_C_k,body1->alpha_t,result2);
142 FastAdd(result1,result2,body2->alpha_t);
143
144 // compute state explicit acceleration
145 FastCross(body1->alpha_t,point1->position,result1);
146 FastCross(body1->omega_k,point1->position,result2);
147 FastCross(body1->omega_k,result2,result3);
148 FastTripleSum(body1->a_t,result1,result3,result4);
149 FastTMult(pk_C_k,result4,result5);
150
151 FastCross(point2->position,body2->alpha_t,result1);
152 FastCross(point2->position,body2->omega_k,result2);
153 FastCross(body2->omega_k,result2,result3);
154
155 FastTripleSum(result5,result1,result3,body2->a_t);
156 }
157
BackwardKinematics()158 void RevoluteJoint::BackwardKinematics(){
159 Vect3 result1,result2,result3,result4,result5;
160 Vect3 k_w_pk;
161
162 // orientations
163 ComputeBackwardTransforms();
164
165 // compute position vector r21
166 //r21 = point2->position - k_C_pk * point1->position;
167 FastMult(k_C_pk,point1->position,result1);
168 FastSubt(point2->position,result1,r21);
169
170 // compute position vector r21
171 FastNegMult(pk_C_k,r21,r12);
172
173 // compute global location
174 // body1->r = body2->r + body2->n_C_k * r21;
175 FastMult(body2->n_C_k,r21,result1);
176 FastAdd(body2->r,result1,body1->r);
177
178 // compute qdot (for revolute joint qdot = u)
179 // qdot = u
180 FastAssign(u,qdot);
181
182 // angular velocities
183 //body1->omega = body2->omega - body2->n_C_k * axis_k * u;
184 //k_w_pk = - axis_pk * u;
185 //body1->omega_k = pk_C_k * body2->omega_k + k_w_pk;
186 double u_double = u.BasicGet(0);
187 FastMult(-u_double,axis_k,result1);
188 FastMult(body2->n_C_k,result1,result2);
189 FastAdd(body2->omega,result2,body1->omega);
190 FastMult(-u_double,axis_pk,k_w_pk);
191 FastMult(pk_C_k,body2->omega_k,result1);
192 FastAdd(result1,k_w_pk,body1->omega_k);
193
194 // compute velocities
195 FastCross(body2->omega_k,r21,result1);
196 FastCross(point1->position,k_w_pk,result2);
197 FastAdd(body2->v_k,result1,result3);
198 FastMult(pk_C_k,result3,result4);
199 FastAdd(result2,result4,body1->v_k);
200 FastMult(body1->n_C_k,body1->v_k,body1->v);
201
202 // compute state explicit angular acceleration
203 FastCross(body1->omega_k,k_w_pk,result1);
204 FastMult(pk_C_k,body2->alpha_t,result2);
205 FastAdd(result1,result2,body1->alpha_t);
206
207 // compute state explicit acceleration
208 FastCross(body2->alpha_t,point2->position,result1);
209 FastCross(body2->omega_k,point2->position,result2);
210 FastCross(body2->omega_k,result2,result3);
211 FastTripleSum(body2->a_t,result1,result3,result4);
212 FastMult(pk_C_k,result4,result5);
213
214 FastCross(point1->position,body1->alpha_t,result1);
215 FastCross(point1->position,body1->omega_k,result2);
216 FastCross(body1->omega_k,result2,result3);
217
218 FastTripleSum(result5,result1,result3,body1->a_t);
219 }
220