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