1 | // |
2 | // Redistribution and use in source and binary forms, with or without |
3 | // modification, are permitted provided that the following conditions |
4 | // are met: |
5 | // * Redistributions of source code must retain the above copyright |
6 | // notice, this list of conditions and the following disclaimer. |
7 | // * Redistributions in binary form must reproduce the above copyright |
8 | // notice, this list of conditions and the following disclaimer in the |
9 | // documentation and/or other materials provided with the distribution. |
10 | // * Neither the name of NVIDIA CORPORATION nor the names of its |
11 | // contributors may be used to endorse or promote products derived |
12 | // from this software without specific prior written permission. |
13 | // |
14 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY |
15 | // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
16 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR |
17 | // PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR |
18 | // CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, |
19 | // EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, |
20 | // PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR |
21 | // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY |
22 | // OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT |
23 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE |
24 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. |
25 | // |
26 | // Copyright (c) 2008-2021 NVIDIA Corporation. All rights reserved. |
27 | // Copyright (c) 2004-2008 AGEIA Technologies, Inc. All rights reserved. |
28 | // Copyright (c) 2001-2004 NovodeX AG. All rights reserved. |
29 | |
30 | |
31 | #ifndef PX_PHYSICS_NX_ARTICULATION_RC |
32 | #define PX_PHYSICS_NX_ARTICULATION_RC |
33 | /** \addtogroup physics |
34 | @{ */ |
35 | |
36 | #include "PxArticulationBase.h" |
37 | #include "foundation/PxVec3.h" |
38 | #include "foundation/PxTransform.h" |
39 | #include "solver/PxSolverDefs.h" |
40 | |
41 | #if !PX_DOXYGEN |
42 | namespace physx |
43 | { |
44 | #endif |
45 | |
46 | PX_ALIGN_PREFIX(16) |
47 | struct PxSpatialForce |
48 | { |
49 | PxVec3 force; |
50 | PxReal pad0; |
51 | PxVec3 torque; |
52 | PxReal pad1; |
53 | } |
54 | PX_ALIGN_SUFFIX(16); |
55 | |
56 | PX_ALIGN_PREFIX(16) |
57 | struct PxSpatialVelocity |
58 | { |
59 | PxVec3 linear; |
60 | PxReal pad0; |
61 | PxVec3 angular; |
62 | PxReal pad1; |
63 | } |
64 | PX_ALIGN_SUFFIX(16); |
65 | |
66 | class PxJoint; |
67 | |
68 | struct PxArticulationRootLinkData |
69 | { |
70 | PxTransform transform; |
71 | PxVec3 worldLinVel; |
72 | PxVec3 worldAngVel; |
73 | PxVec3 worldLinAccel; |
74 | PxVec3 worldAngAccel; |
75 | }; |
76 | |
77 | class PxArticulationCache |
78 | { |
79 | public: |
80 | enum Enum |
81 | { |
82 | eVELOCITY = (1 << 0), //!< The joint velocities this frame. Note, this is the accumulated joint velocities, not change in joint velocity. |
83 | eACCELERATION = (1 << 1), //!< The joint accelerations this frame. Delta velocity can be computed from acceleration * dt. |
84 | ePOSITION = (1 << 2), //!< The joint positions this frame. Note, this is the accumulated joint positions over frames, not change in joint position. |
85 | eFORCE = (1 << 3), //!< The joint forces this frame. Note, the application should provide these values for the forward dynamic. If the application is using inverse dynamic, this is the joint force returned. |
86 | eLINKVELOCITY = (1 << 4), //!< The link velocities this frame. |
87 | eLINKACCELERATION = (1 << 5), //!< The link accelerations this frame. |
88 | eROOT = (1 << 6), //!< Root link transform, velocity and acceleration. Note, when the application call applyCache with eROOT flag, it won't apply root link's acceleration to the simulation |
89 | eALL = (eVELOCITY | eACCELERATION | ePOSITION| eLINKVELOCITY | eLINKACCELERATION | eROOT ) |
90 | }; |
91 | PxArticulationCache() : |
92 | externalForces (NULL), |
93 | denseJacobian (NULL), |
94 | massMatrix (NULL), |
95 | jointVelocity (NULL), |
96 | jointAcceleration (NULL), |
97 | jointPosition (NULL), |
98 | jointForce (NULL), |
99 | rootLinkData (NULL), |
100 | coefficientMatrix (NULL), |
101 | lambda (NULL), |
102 | scratchMemory (NULL), |
103 | scratchAllocator (NULL), |
104 | version (0) |
105 | {} |
106 | |
107 | PxSpatialForce* externalForces; // N = getNbLinks() |
108 | PxReal* denseJacobian; // N = 6*getDofs()*NumJoints, NumJoints = getNbLinks() - 1 |
109 | PxReal* massMatrix; // N = getDofs()*getDofs() |
110 | PxReal* jointVelocity; // N = getDofs() |
111 | PxReal* jointAcceleration; // N = getDofs() |
112 | PxReal* jointPosition; // N = getDofs() |
113 | PxReal* jointForce; // N = getDofs() |
114 | PxSpatialVelocity* linkVelocity; // N = getNbLinks() |
115 | PxSpatialVelocity* linkAcceleration; // N = getNbLinks() |
116 | PxArticulationRootLinkData* rootLinkData; // root link Data |
117 | |
118 | //application need to allocate those memory and assign them to the cache |
119 | PxReal* coefficientMatrix; |
120 | PxReal* lambda; |
121 | |
122 | //These three members won't be set to zero when zeroCache get called |
123 | void* scratchMemory; //this is used for internal calculation |
124 | void* scratchAllocator; |
125 | PxU32 version; //cache version. If the articulation configuration change, the cache is invalid |
126 | }; |
127 | |
128 | typedef PxFlags<PxArticulationCache::Enum, PxU8> PxArticulationCacheFlags; |
129 | PX_FLAGS_OPERATORS(PxArticulationCache::Enum, PxU8) |
130 | |
131 | /** |
132 | \brief a tree structure of bodies connected by joints that is treated as a unit by the dynamics solver |
133 | |
134 | Articulations are more expensive to simulate than the equivalent collection of |
135 | PxRigidDynamic and PxJoint structures, but because the dynamics solver treats |
136 | each articulation as a single object, they are much less prone to separation and |
137 | have better support for actuation. An articulation may have at most 64 links. |
138 | |
139 | @see PxArticulationJoint PxArticulationLink PxPhysics.createArticulation |
140 | */ |
141 | |
142 | #if PX_VC |
143 | #pragma warning(push) |
144 | #pragma warning(disable : 4435) |
145 | #endif |
146 | |
147 | class PxArticulationReducedCoordinate : public PxArticulationBase |
148 | { |
149 | public: |
150 | |
151 | virtual void release() = 0; |
152 | |
153 | /** |
154 | \brief Sets flags on the articulation |
155 | |
156 | \param[in] flags Articulation flags |
157 | |
158 | */ |
159 | virtual void setArticulationFlags(PxArticulationFlags flags) = 0; |
160 | |
161 | /** |
162 | \brief Raises or clears a flag on the articulation |
163 | |
164 | \param[in] flag The articulation flag |
165 | \param[in] value true/false indicating whether to raise or clear the flag |
166 | |
167 | */ |
168 | virtual void setArticulationFlag(PxArticulationFlag::Enum flag, bool value) = 0; |
169 | |
170 | /** |
171 | \brief return PxArticulationFlags |
172 | */ |
173 | virtual PxArticulationFlags getArticulationFlags() const = 0; |
174 | |
175 | /** |
176 | \brief returns the total Dofs of the articulation |
177 | */ |
178 | virtual PxU32 getDofs() const = 0; |
179 | |
180 | /** |
181 | \brief create an articulation cache |
182 | |
183 | \note this call may only be made on articulations that are in a scene, and may not be made during simulation |
184 | */ |
185 | virtual PxArticulationCache* createCache() const = 0; |
186 | |
187 | /** |
188 | \brief Get the size of the articulation cache |
189 | |
190 | \note this call may only be made on articulations that are in a scene, and may not be made during simulation |
191 | */ |
192 | virtual PxU32 getCacheDataSize() const = 0; |
193 | |
194 | /** |
195 | \brief zero all data in the articulation cache beside the cache version |
196 | |
197 | \note this call may only be made on articulations that are in a scene, and may not be made during simulation |
198 | */ |
199 | virtual void zeroCache(PxArticulationCache& cache) = 0; |
200 | |
201 | /** |
202 | \brief apply the user defined data in the cache to the articulation system |
203 | |
204 | \param[in] cache articulation data. |
205 | \param[in] flag The mode to use when determine which value in the cache will be applied to the articulation |
206 | \param[in] autowake Specify if the call should wake up the articulation if it is currently asleep. If true and the current wake counter value is smaller than #PxSceneDesc::wakeCounterResetValue it will get increased to the reset value. |
207 | |
208 | @see createCache copyInternalStateToCache |
209 | */ |
210 | virtual void applyCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag, bool autowake = true) = 0; |
211 | |
212 | /** |
213 | \brief copy the internal data of the articulation to the cache |
214 | |
215 | \param[in] cache articulation data |
216 | \param[in] flag this indicates what kind of data the articulation system need to copy to the cache |
217 | |
218 | @see createCache applyCache |
219 | */ |
220 | virtual void copyInternalStateToCache(PxArticulationCache& cache, const PxArticulationCacheFlags flag) const = 0; |
221 | |
222 | /** |
223 | \brief release an articulation cache |
224 | |
225 | \param[in] cache the cache to release |
226 | |
227 | @see createCache applyCache copyInternalStateToCache |
228 | */ |
229 | virtual void releaseCache(PxArticulationCache& cache) const = 0; |
230 | |
231 | /** |
232 | \brief reduce the maximum data format to the reduced internal data |
233 | \param[in] maximum joint data format |
234 | \param[out] reduced joint data format |
235 | */ |
236 | virtual void packJointData(const PxReal* maximum, PxReal* reduced) const = 0; |
237 | |
238 | /** |
239 | \brief turn the reduced internal data to maximum joint data format |
240 | \param[in] reduced joint data format |
241 | \param[out] maximum joint data format |
242 | */ |
243 | virtual void unpackJointData(const PxReal* reduced, PxReal* maximum) const = 0; |
244 | |
245 | /** |
246 | \brief initialize all the common data for inverse dynamics |
247 | */ |
248 | virtual void commonInit() const = 0; |
249 | |
250 | /** |
251 | \brief determine the statically balance of the joint force of gravity for entire articulation. External force, joint velocity and joint acceleration |
252 | are set to zero, the joint force returned will be purely determined by gravity. |
253 | |
254 | \param[out] cache return joint forces which can counteract gravity force |
255 | |
256 | @see commonInit |
257 | */ |
258 | virtual void computeGeneralizedGravityForce(PxArticulationCache& cache) const = 0; |
259 | |
260 | /** |
261 | \brief determine coriolise and centrifugal force. External force, gravity and joint acceleration |
262 | are set to zero, the joint force return will be coriolise and centrifugal force for each joint. |
263 | |
264 | \param[in] cache data |
265 | |
266 | @see commonInit |
267 | */ |
268 | virtual void computeCoriolisAndCentrifugalForce(PxArticulationCache& cache) const = 0; |
269 | |
270 | /** |
271 | \brief determine joint force change caused by external force. Gravity, joint acceleration and joint velocity |
272 | are all set to zero. |
273 | |
274 | \param[in] cache data |
275 | |
276 | @see commonInit |
277 | */ |
278 | virtual void computeGeneralizedExternalForce(PxArticulationCache& cache) const = 0; |
279 | |
280 | /** |
281 | \brief determine the joint acceleration for each joint |
282 | This is purely calculates the change in joint acceleration due to change in the joint force |
283 | |
284 | \param[in] cache articulation data |
285 | |
286 | @see commonInit |
287 | */ |
288 | virtual void computeJointAcceleration(PxArticulationCache& cache) const = 0; |
289 | |
290 | /** |
291 | \brief determine the joint force |
292 | This is purely calculates the change in joint force due to change in the joint acceleration |
293 | This means gravity and joint velocity will be zero |
294 | |
295 | \param[in] cache return joint force |
296 | |
297 | @see commonInit |
298 | */ |
299 | virtual void computeJointForce(PxArticulationCache& cache) const = 0; |
300 | |
301 | /** |
302 | \brief compute the dense Jacobian for the entire articulation in world space |
303 | \param[out] cache sets cache.denseJacobian matrix. The matrix is indexed [nCols * row + column]. |
304 | \param[out] nRows set to number of rows in matrix, which corresponds to the number of articulation links times 6. |
305 | \param[out] nCols set to number of columns in matrix, which corresponds to the number of joint DOFs, plus 6 in the case eFIX_BASE is false. |
306 | |
307 | Note that this computes the dense representation of an inherently sparse matrix. Multiplication with this matrix maps |
308 | joint space velocities to 6DOF world space linear and angular velocities. |
309 | */ |
310 | virtual void computeDenseJacobian(PxArticulationCache& cache, PxU32& nRows, PxU32& nCols) const = 0; |
311 | |
312 | |
313 | /** |
314 | \brief compute the coefficient matrix for contact force. |
315 | \param[out] cache returns the coefficient matrix. Each column is the joint force effected by a contact based on impulse strength 1 |
316 | @see commonInit |
317 | */ |
318 | virtual void computeCoefficientMatrix(PxArticulationCache& cache) const = 0; |
319 | |
320 | /** |
321 | \brief compute the lambda value when the test impulse is 1 |
322 | \param[in] initialState the initial state of the articulation system |
323 | \param[in] jointTorque M(q)*qddot + C(q,qdot) + g(q) |
324 | \param[in] maxIter maximum number of solver iterations to run. If the system converges, fewer iterations may be used. |
325 | \param[out] cache returns the coefficient matrix. Each column is the joint force effected by a contact based on impulse strength 1 |
326 | @see commonInit |
327 | */ |
328 | virtual bool computeLambda(PxArticulationCache& cache, PxArticulationCache& initialState, const PxReal* const jointTorque, const PxU32 maxIter) const = 0; |
329 | |
330 | /** |
331 | \brief compute the joint-space inertia matrix |
332 | \param[in] cache articulation data |
333 | |
334 | @see commonInit |
335 | */ |
336 | virtual void computeGeneralizedMassMatrix(PxArticulationCache& cache) const = 0; |
337 | |
338 | /** |
339 | \brief add loop joint to the articulation system for inverse dynamic |
340 | \param[in] joint required to add loop joint |
341 | |
342 | @see commonInit |
343 | */ |
344 | virtual void addLoopJoint(PxJoint* joint) = 0; |
345 | |
346 | /** |
347 | \brief remove loop joint from the articulation system |
348 | \param[in] joint required to remove loop joint |
349 | |
350 | @see commonInit |
351 | */ |
352 | virtual void removeLoopJoint(PxJoint* joint) = 0; |
353 | |
354 | /** |
355 | \brief returns the number of loop joints in the articulation |
356 | \return number of loop joints |
357 | */ |
358 | virtual PxU32 getNbLoopJoints() const = 0; |
359 | |
360 | /** |
361 | \brief returns the set of loop constraints in the articulation |
362 | |
363 | \param[in] userBuffer buffer into which to write an array of constraints pointers |
364 | \param[in] bufferSize the size of the buffer. If this is not large enough to contain all the pointers to links, |
365 | only as many as will fit are written. |
366 | \param[in] startIndex Index of first link pointer to be retrieved |
367 | |
368 | \return the number of links written into the buffer. |
369 | |
370 | @see ArticulationLink |
371 | */ |
372 | virtual PxU32 getLoopJoints(PxJoint** userBuffer, PxU32 bufferSize, PxU32 startIndex = 0) const = 0; |
373 | |
374 | /** |
375 | \brief returns the required size of coeffient matrix in the articulation. The coefficient matrix is number of constraint(loop joints) by total dofs. Constraint Torque = transpose(K) * lambda(). Lambda is a vector of number of constraints |
376 | \return bite size of the coefficient matrix(nc * n) |
377 | */ |
378 | virtual PxU32 getCoefficientMatrixSize() const = 0; |
379 | |
380 | /** |
381 | \brief teleport root link to a new location |
382 | \param[in] pose the new location of the root link |
383 | \param[in] autowake wake up the articulation system |
384 | |
385 | @see commonInit |
386 | */ |
387 | virtual void teleportRootLink(const PxTransform& pose, bool autowake) = 0; |
388 | |
389 | |
390 | /** |
391 | \brief return the link velocity in world space with the associated low-level link index(getLinkIndex()). |
392 | \param[in] linkId low-level link index |
393 | |
394 | @see getLinkIndex() in PxArticulationLink |
395 | */ |
396 | virtual PxSpatialVelocity getLinkVelocity(const PxU32 linkId) = 0; |
397 | |
398 | |
399 | /** |
400 | \brief return the link acceleration in world space with the associated low-level link index(getLinkIndex()) |
401 | \param[in] linkId low-level link index |
402 | |
403 | @see getLinkIndex() in PxArticulationLink |
404 | */ |
405 | virtual PxSpatialVelocity getLinkAcceleration(const PxU32 linkId) = 0; |
406 | |
407 | protected: |
408 | PX_INLINE PxArticulationReducedCoordinate(PxType concreteType, PxBaseFlags baseFlags) : PxArticulationBase(concreteType, baseFlags) {} |
409 | PX_INLINE PxArticulationReducedCoordinate(PxBaseFlags baseFlags) : PxArticulationBase(baseFlags) {} |
410 | virtual ~PxArticulationReducedCoordinate() {} |
411 | }; |
412 | |
413 | #if PX_VC |
414 | #pragma warning(pop) |
415 | #endif |
416 | |
417 | #if !PX_DOXYGEN |
418 | } // namespace physx |
419 | #endif |
420 | |
421 | /** @} */ |
422 | #endif |
423 | |