-
Notifications
You must be signed in to change notification settings - Fork 468
/
SimbodyMatterSubsystem.h
3146 lines (2697 loc) · 153 KB
/
SimbodyMatterSubsystem.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
#define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
/* -------------------------------------------------------------------------- *
* Simbody(tm) *
* -------------------------------------------------------------------------- *
* This is part of the SimTK biosimulation toolkit originating from *
* Simbios, the NIH National Center for Physics-Based Simulation of *
* Biological Structures at Stanford, funded under the NIH Roadmap for *
* Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
* *
* Portions copyright (c) 2006-15 Stanford University and the Authors. *
* Authors: Michael Sherman *
* Contributors: Paul Mitiguy *
* *
* Licensed under the Apache License, Version 2.0 (the "License"); you may *
* not use this file except in compliance with the License. You may obtain a *
* copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
* *
* Unless required by applicable law or agreed to in writing, software *
* distributed under the License is distributed on an "AS IS" BASIS, *
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
* See the License for the specific language governing permissions and *
* limitations under the License. *
* -------------------------------------------------------------------------- */
#include "SimTKcommon.h"
#include "simbody/internal/common.h"
#include "simbody/internal/MobilizedBody.h"
#include <cassert>
#include <vector>
#include <iostream>
class SimbodyMatterSubsystemRep;
namespace SimTK {
class MobilizedBody;
class MultibodySystem;
class Constraint;
class UnilateralContact;
class StateLimitedFriction;
/** This subsystem contains the bodies ("matter") in the multibody system,
the mobilizers (joints) that define the generalized coordinates used to
represent the motion of those bodies, and constraints that must be satisfied
by the values of those coordinates.
There are many methods in the API for this class. For whole-system information
and calculations, the methods here are the right ones to use. For information
associated with individual objects contained in the subsystem, such as
MobilizedBody and Constraint objects, it is generally easier to obtain the
information through the contained objects' APIs instead.
This class is a "handle" containing only an opaque reference to the
underlying implementation class.
<h3>Theory discussion</h3>
The bodies, mobilizers, and constraints are represented mathematically with
the following set of equations:
<pre>
qdot = N u Kinematic differential eqns.
zdot = zdot(t,q,u,z) Auxiliary states
M udot + ~G mult = f(t,q,u,z) Equations of motion
G udot = b(t,q,u)
where
[P] [bp]
G=[V] b=[bv] f = T + ~J*(F-C)
[A] [ba]
pdotdot = P udot - bp(t,q,u) = 0 Acceleration constraints
vdot = V udot - bv(t,q,u) = 0
a(t,q,u,udot) = A udot - ba(t,q,u) = 0
pdot = P u - c(t,q) = 0 Velocity constraints
v(t,q,u) = 0
p(t,q) = 0 Position constraints
n(q) = 0 Normalization constraints
</pre>
where M(q) is the mass matrix, G(t,q,u) the acceleration constraint matrix,
C(q,u) the coriolis and gyroscopic forces, T is user-applied joint mobility
forces, F is user-applied body forces and torques and gravity. J(q) is the
%System Jacobian (partial velocity matrix) whose transpose ~J maps spatial
forces to joint mobility forces. p(t,q) are the holonomic (position)
constraints, v(t,q,u) the non-holonomic (velocity) constraints, and
a(t,q,u,udot) the acceleration-only constraints, which must be linear in udot,
with A(t,q,u) the coefficient matrix for a(). pdot, pdotdot are obtained by
differentiation of p(), vdot by differentiation of v(). P(t,q)=Dpdot/Du
(yes, that's u, not q -- we can get Pq=Dp/Dq when we need it as Pq=P*N^-1) and
V(t,q,u)=Dv/Du. (We use capital "D" to indicate partial derivative.) n(q) is
the set of quaternion normalization constraints, which exist only at the
position level and are uncoupled from everything else.
We calculate the constraint multipliers like this:
<pre>
G M^-1 ~G mult = G udot0 - b
where udot0 = M^-1 f
</pre>
using the pseudo inverse of G M^-1 ~G to give a least squares solution for
mult: mult = pinv(G M^-1 ~G)(G M^-1 f - b). Then the real udot is
udot = udot0 - udotC, with udotC = M^-1 ~G mult. Note: M^-1* is an
O(n) operator that provides the desired result; it <em>does not</em> require
forming or factoring M.
NOTE: only the following constraint matrices have to be formed and factored:
<pre>
[G M^-1 ~G] to calculate multipliers
[P N^-1] for projection onto position manifold (a.k.a. Pq)
[ P ] for projection onto velocity manifold
[ V ]
</pre>
When working in a weighted norm with weights W on the state variables and
weights T (1/tolerance) on the constraint errors, the matrices we need are
actually [Tp Pq Wq^-1], [Tpv [P;V] Wu^-1], etc. with T and W diagonal
weighting matrices. These can then be used to find least squares solutions
in the weighted norms.
In many cases these matrices consist of decoupled blocks which can
be solved independently. (TODO: take advantage of that whenever possible
to solve a set of smaller systems rather than one large one.) Also, in the
majority of biosimulation applications we are likely to have only holonomic
(position) constraints, so there is no V or A and G=P is the whole story.
**/
class SimTK_SIMBODY_EXPORT SimbodyMatterSubsystem : public Subsystem {
public:
//==============================================================================
/** @name Construction, Destruction, Topological information
Methods in this section are used in the extended construction phase for
a %SimbodyMatterSubsystem which we call defining the "topology" of the
multibody system. This includes adding mobilized bodies and constraints.
Topological information is always state-independent since it is kept in
the %SimbodyMatterSubsystem object directly. The construction phase ends
when realizeTopology() is called on the containing System. **/
/**@{**/
/** Create a matter subsystem containing only the Ground body (mobilized
body 0), and add the subsystem to the indicated MultibodySystem. The
MultibodySystem takes over ownership of the subsystem, which is not
copied. The MultibodySystem and this subsystem handle both refer to the
same subsystem after this call. **/
explicit SimbodyMatterSubsystem(MultibodySystem&);
/** Create an orphan matter subsystem containing only the Ground body
(mobilized body 0); normally use the other constructor to place the
subsystem in a MultibodySystem. **/
SimbodyMatterSubsystem();
/** The destructor destroys the subsystem implementation object only if this
handle is the last reference. Normally, there is a MultibodySystem that holds
a reference to the subsystem implementation, so this destruction will do
nothing. **/
~SimbodyMatterSubsystem() {} // invokes ~Subsystem()
/** Given a MobilizedBodyIndex, return a read-only (const) reference to the
corresponding MobilizedBody within this matter subsystem. This method will
fail if the index is invalid or out of range. MobilizedBodyIndex(0) selects
the Ground mobilized body. **/
const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;
/** Given a MobilizedBodyIndex, return a writable reference to the
corresponding MobilizedBody within this matter subsystem. This method will
fail if the index is invalid or out of range. MobilizedBodyIndex(0) selects
the Ground mobilized body. **/
MobilizedBody& updMobilizedBody(MobilizedBodyIndex);
/** Return a read-only (const) reference to the Ground MobilizedBody
within this matter subsystem. **/
const MobilizedBody::Ground& getGround() const;
/** Return a writable reference to the Ground MobilizedBody within this
matter subsystem; you need a writable reference if you're adding a
mobilized body that is directly connected to Ground. **/
MobilizedBody::Ground& updGround();
/** This is a synonym for updGround() that makes for nicer-looking examples.
Note: topology is not marked invalid upon returning a writable reference
here; that will be done only if a non-const method of the returned
MobilizedBody is called. That means it is OK to use Ground() to satisfy
a const argument; it won't have an "invalidate topology" side effect.
@see updGround() **/
MobilizedBody::Ground& Ground() {return updGround();}
/** Given a ConstraintIndex, return a read-only (const) reference to the
corresponding Constraint within this matter subsystem. This method will
fail if the index is invalid or out of range. **/
const Constraint& getConstraint(ConstraintIndex) const;
/** Given a ConstraintIndex, return a writable reference to the corresponding
Constraint within this matter subsystem. This method will
fail if the index is invalid or out of range. **/
Constraint& updConstraint(ConstraintIndex);
/** Normally the matter subsystem will attempt to generate some decorative
geometry as a sketch of the defined multibody system; you can disable that
with this method. **/
void setShowDefaultGeometry(bool show);
/** Get whether this matter subsystem is set to generate default decorative
geometry that can be used to visualize this multibody system. **/
bool getShowDefaultGeometry() const;
/** The number of bodies includes all mobilized bodies \e including Ground,
which is the first mobilized body, at MobilizedBodyIndex 0. (Note: if
special particle handling were implemented, the count here would \e not
include particles.) Bodies and their inboard mobilizers have the same index
since they are grouped together as a MobilizedBody. MobilizedBody numbering
(using unique integer type MobilizedBodyIndex) starts with Ground at
MobilizedBodyIndex(0) with a regular labeling such that children have higher
indices than their parents. Ground does not have a mobilizer (or I suppose
you could think of its mobilizer as the Weld joint that attaches it to the
universe), but otherwise every mobilized body has a unique body and mobilizer.
Example:
- Here we use a model of a planar pendulum with 1 degree-of-freedom.
- The command getNumBodies() will return 2.
- Why? Ground counts as a mobilized body, and so does the pendulum.
- Note that Ground is at MobilizedBodyIndex(0), and the pendulum is at
the MobilizedBodyIndex(1).**/
int getNumBodies() const;
/** This is the total number of defined constraints, each of which may
generate more than one constraint equation. This is the number of Constraint
objects that were defined; in a given State some of these may be disabled. **/
int getNumConstraints() const;
/** The sum of all the mobilizer degrees of freedom. This is also the length
of the state variable vector u and the mobility forces array. **/
int getNumMobilities() const;
/** The sum of all the q vector allocations for each joint. There may be
some that are not in use for particular modeling options. **/
int getTotalQAlloc() const;
/** Attach new matter by attaching it to the indicated parent body (not
normally called by users -- see MobilizedBody). The mobilizer and mass
properties are provided by \a child. A new MobilizedBodyIndex is assigned for
the child; it is guaranteed to be numerically larger than the
MobilizedBodyIndex of the parent. We take over ownership of \a child's
implementation object from the given MobilizedBody handle, leaving that handle
as a reference to the implementation object now owned by the matter subsystem.
It is an error if the given MobilizedBody handle wasn't the owner of the
implementation object to which it refers.
@note
This method is usually called by concrete MobilizedBody constructors;
it does not normally need to be called by end users. **/
MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent,
MobilizedBody& child);
/** Add a new Constraint object to the matter subsystem (not normally called
by users -- see Constraint). The details of the Constraint are opaque here.
A new ConstraintIndex is assigned. We take over ownership of the
implementation object from the given Constraint handle, leaving that handle as
a reference to the implementation object now owned by the matter subsystem. It
is an error if the given Constraint handle wasn't the owner of the
implementation object to which it refers.
@note
This method is usually called by concrete Constraint constructors; it does not
normally need to be called by end users. **/
ConstraintIndex adoptConstraint(Constraint&);
/** (Experimental) **/
UnilateralContactIndex adoptUnilateralContact(UnilateralContact*);
int getNumUnilateralContacts() const;
const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const;
UnilateralContact& updUnilateralContact(UnilateralContactIndex);
/** (Experimental) **/
StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction*);
int getNumStateLimitedFrictions() const;
const StateLimitedFriction&
getStateLimitedFriction(StateLimitedFrictionIndex) const;
StateLimitedFriction&
updStateLimitedFriction(StateLimitedFrictionIndex);
/** Copy constructor is not very useful. **/
SimbodyMatterSubsystem(const SimbodyMatterSubsystem& ss) : Subsystem(ss) {}
/** Copy assignment is not very useful. **/
SimbodyMatterSubsystem& operator=(const SimbodyMatterSubsystem& ss)
{ Subsystem::operator=(ss); return *this; }
//==============================================================================
/** @name Set/get modeling options
Methods in this section involve setting and getting various modeling options
that may be selected. This includes whether to use quaternions or Euler angles
to represent rotations, and enabling/disabling constraints. **/
/**@{**/
/** For all mobilizers offering unrestricted orientation, decide what
method we should use to model their orientations. Choices are:
quaternions (best for dynamics), or rotation angles (1-2-3 Euler
sequence, good for optimization). Changing this flag invalidates Model
stage and above in the supplied \a state, leaving it realized only through
Topology stage, so you must call realizeModel() on the containing
MultibodySystem prior to using this \a state in further calculations. **/
void setUseEulerAngles(State& state, bool useEulerAngles) const;
/** Return the current setting of the "use Euler angles" model variable as
set in the supplied \a state. **/
bool getUseEulerAngles(const State& state) const;
/** Return the number of quaternions in use by the mobilizers of this system,
given the current setting of the "use Euler angles" flag in the supplied
\a state, and the types of mobilizers in the multibody tree.
@see isUsingQuaternion(), getQuaternionPoolIndex() **/
int getNumQuaternionsInUse(const State& state) const;
/** Check whether a given mobilizer is currently using quaternions, based
on the type of mobilizer and the setting of the "use Euler angles" flag in
the supplied \a state.
@see getNumQuaternionsInUse(), getQuaternionPoolIndex() **/
bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;
/** If the given mobilizer is currently using a quaternion to represent
orientation, return the QuaternionPoolIndex (a small integer) assigned to that
quaternion. This is used, for example, to find which normalization constraint
error is associated with which quaternion.
@see isUsingQuaternion(), getNumQuaternionsInUse() **/
QuaternionPoolIndex getQuaternionPoolIndex(const State& state,
MobilizedBodyIndex mobodIx) const;
/** Disable or enable the Constraint whose ConstraintIndex is supplied within
the supplied \a state. Whether a Constraint is disabled is an Instance-stage
state variable so enabling or disabling invalidates Instance stage and higher
in the given \a state, leaving the \a state realized no higher than Model
stage.
@see isConstraintDisabled() **/
void setConstraintIsDisabled(State& state,
ConstraintIndex constraintIx,
bool shouldDisableConstraint) const;
/** Determine whether a particular Constraint is currently disabled in the
given \a state.
@see setConstraintIsDisabled() **/
bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
/** Given a State which may be modeled using quaternions, copy it to another
State which represents the same configuration using Euler angles instead. If
the \a inputState already uses Euler angles, the output will just be a
duplicate. All continuous and discrete State variables will be copied to the
\a outputState but they will not necessarily have been realized to the same
level as the \a inputState. **/
void convertToEulerAngles(const State& inputState, State& outputState) const;
/** Given a State which may be modeled using Euler angles, copy it to another
State which represents the same configuration using quaternions instead. If
the \a inputState already uses quaternions, the output will just be a
duplicate. All continuous and discrete State variables will be copied to the
\a outputState but they will not necessarily have been realized to the same
level as the \a inputState. **/
void convertToQuaternions(const State& inputState, State& outputState) const;
/** (Advanced) Given a State whose generalized coordinates q have been modified
in some manner that doesn't necessarily keep quaternions normalized, fix them.
Note that all of Simbody's integrators and solvers take care of this
automatically so most users will never need to make this call.
Since we are modifying q's here, Stage::Position is invalidated.
@param[in,out] state **/
void normalizeQuaternions(State& state) const;
/**@}**/
//==============================================================================
/** @name Calculate whole-system properties
These methods perform calculations that yield properties of the system as
a whole. These are \e operators, meaning that they make use of the supplied
State but do not modify the State. They simply calculate a result and return
it to you without storing it internally. Each method requires that the
State has already been realized to at least a particular stage which is
documented with the method. **/
/**@{**/
/** Calculate the total system mass.
@par Required stage
\c Stage::Instance **/
Real calcSystemMass(const State& s) const;
/** Return the position vector p_GC of the system mass center C, measured from
the Ground origin, and expressed in Ground.
@par Required stage
\c Stage::Position **/
Vec3 calcSystemMassCenterLocationInGround(const State& s) const;
/** Return total system mass, mass center location measured from the Ground
origin, and system inertia taken about the Ground origin, expressed in Ground.
@par Required stage
\c Stage::Position **/
MassProperties calcSystemMassPropertiesInGround(const State& s) const;
/** Return the system inertia matrix taken about the system center of mass,
expressed in Ground.
@par Required stage
\c Stage::Position **/
Inertia calcSystemCentralInertiaInGround(const State& s) const;
/** Return the velocity v_GC = d/dt p_GC of the system mass center C in the
Ground frame G, measured from Ground origin and expressed in G.
@par Required stage
\c Stage::Velocity **/
Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;
/** Return the acceleration a_GC = d/dt p_GC of the system mass center C in the
Ground frame G, measured from Ground origin and expressed in G.
@par Required stage
\c Stage::Acceleration **/
Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;
/** Return the momentum of the system as a whole (angular, linear) measured
in the Ground frame, taken about the Ground origin and expressed in Ground.
(The linear component is independent of the "about" point.)
@see calcSystemCentralMomentum()
@par Required stage
\c Stage::Velocity **/
SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;
/** Return the momentum of the system as a whole (angular, linear) measured
in the Ground frame, taken about the current system center of mass
location C and expressed in Ground. (The linear component is independent of the
"about" point.)
@see calcSystemMomentumAboutGroundOrigin()
@par Required stage
\c Stage::Velocity **/
SpatialVec calcSystemCentralMomentum(const State& s) const;
/** Calculate the total kinetic energy of all the mobilized bodies in this
matter subsystem, given the configuration and velocities in \a state.
@par Required stage
\c Stage::Velocity **/
Real calcKineticEnergy(const State& state) const;
/**@}**/
//==============================================================================
/** @name System and Task Space Kinematic Jacobian Operators
The system kinematic Jacobian maps between mobility space (generalized speeds
and generalized forces) and Cartesian body space (mobilized body frame spatial
velocities and spatial forces). A task space Jacobian maps between mobility
space and a specified set of task points or frames fixed to a subset of the
bodies, and generally located away from the body frame. A task space Jacobian
J can be used to construct various task space matrices such as the task space
compliance matrix J M^-1 ~J or its inverse, the task space (or operational
space) inertia matrix.
The system Jacobian J(q) maps n generalized speeds u to spatial velocities V of
each of the nb mobilized bodies (including Ground), measured at the body frame
origin relative to Ground, and expressed in the Ground frame. The transpose ~J
of this matrix maps nb spatial forces to n generalized forces, where the spatial
forces are applied at the body frame origin and expressed in Ground. Similarly,
task space Jacobians map from n generalized speeds to nt task frame spatial
velocities (expressed in Ground), and transposed task space Jacobians map
between task frame spatial forces (or impulses), expressed in Ground, and
generalized forces (or generalized impulses).
Simbody provides fast O(n) methods ("operators") that can form matrix-vector
products like J*u or ~J*F without forming J. The "bias" term Jdot*u (also known
as the Coriolis acceleration) is also available; this arises when working at
the acceleration level because d/dt J*u = J*udot+Jdot*u (where dot means time
derivative). The computational cost of these operators is O(n+nt) so it is
\e much more efficient to work with a group of tasks simultaneously than to
process one at a time, which would have complexity O(n*nt). Alternatively, we
provide methods that will return all or part of J explicitly; in general it
is \e much more efficient computationally to work with the O(n) matrix-vector
multiply operators rather than to form explicit matrices and then perform O(n^2)
matrix-vector products. Performance estimates are given with each method so that
you can determine which methods to use. If you can, you should use the O(n)
methods -- it is a good habit to get into when using an O(n) multibody code like
Simbody!
Note that the Jacobian is associated with an expressed-in frame for the
velocity or force vector and a designated station (point) on each body. We
always use the Ground frame for Jacobians. For the system Jacobian, the body
origin is always the designated station; for task Jacobians different stations
may be specified. We provide three different sets of methods for working with
- the full %System Jacobian: J, nb X n 6-vectors (or 6*nb X n scalars)
- the Station Jacobian for a set of nt task stations (points): JS, nt rows
of n 3-vectors (or a 3*nt X n Matrix of scalars)
- the Frame Jacobian for a set of nt task frames fixed to a body: JF, nt
rows of n 6-vectors (or a 6*nt X n Matrix of scalars)
The rotational part of a Jacobian is the same for any frame fixed to the same
body. So for Frame Jacobians you need specify only a station on the body (the
frame's origin point). That means if you want a 3*nt X n Orientation Jacobian,
you can obtain it from alternate rows of a Frame Jacobian. Using the above
terminology, the complete %System Jacobian is a Frame Jacobian for which the
task frames are the body frames, with each MobilizedBody appearing only once
and in order of MobilizedBodyIndex (starting with Ground).
It is acceptable for the same body to appear more than once in a list of tasks;
these are likely to conflict but that can be dealt with elsewhere. **/
/**@{**/
/** Calculate the product of the %System kinematic Jacobian J (also known as the
partial velocity matrix) and a mobility-space vector u in O(n) time. If the
vector u is a set of generalized speeds, then this produces the body spatial
velocities that result from those generalized speeds. That is, the result is
V_GB = J*u where V_GB[i] is the spatial velocity of the i'th body's body frame
origin (in Ground) that results from the given set of generalized speeds.
@param[in] state
A State compatible with this System that has already been realized to
Stage::Position.
@param[in] u
A mobility-space Vector, such as a set of generalized speeds. The length
and order must match the mobilities of this system (that is n, the number
of generalized speeds u, \e not nq, the number of generalized
coordinates q).
@param[out] Ju
This is the product V=J*u as described above. Each element is a spatial
vector, one per mobilized body, to be indexed by MobilizedBodyIndex.
If the input vector is a set of generalized speeds u, then the results
are nb spatial velocities V_GBi (that is, a pair of vectors w_GBi and v_GBi
giving angular and linear velocity). Note that Ground is body 0 so the 0th
element V_GB0=V_GG=Ju[0] is always zero on return.
The kinematic Jacobian (partial velocity matrix) J is defined as follows:
<pre>
partial(V) T T
J = ----------, V = [V_GB0 V_GB1 ... V_GB nb-1] , u = [u0 u1 ... u n-1]
partial(u)
</pre>
Thus the element J(i,j)=partial(V_GBi)/partial(uj) (each element of J is a
spatial vector). The transpose of this matrix maps spatial forces to
generalized forces; see multiplyBySystemJacobianTranspose().
Note that we're using "monogram" notation for the spatial velocities, where
<pre>
G Bi
V_GBi = V
</pre>
the spatial velocity of body i's body frame Bi (at its origin), measured and
expressed in the Ground frame G.
<h3>Performance discussion</h3>
This is a very fast operator, costing about 12*(nb+n) flops, where nb is the
number of bodies and n the number of mobilities (degrees of freedom) u. In
contrast, even if you have already calculated the entire nbXnX6 matrix J, the
multiplication J*u would cost 12*nb*n flops. As an example, for a 20 body
system with a free flying base and 19 pin joints (25 dofs altogether), this
method takes 12*(20+25)=540 flops while the explicit matrix-vector multiply
would take 12*20*25=6000 flops. So this method is already >10X faster for
that small system; for larger systems the difference grows rapidly.
@see multiplyBySystemJacobianTranspose(), calcSystemJacobian() **/
void multiplyBySystemJacobian( const State& state,
const Vector& u,
Vector_<SpatialVec>& Ju) const;
/** Calculate the acceleration bias term for the %System Jacobian, that is, the
part of the acceleration that is due only to velocities. This term is also
known as the Coriolis acceleration, and it is returned here as a spatial
acceleration of each body frame in Ground.
@param[in] state
A State that has already been realized through Velocity stage.
@param[out] JDotu
The product JDot*u where JDot = d/dt J, and u is the vector of generalized
speeds taken from \a state. This is a Vector of nb SpatialVec elements.
<h3>Theory</h3>
The spatial velocity V_GBi of each body i can be obtained from the generalized
speeds u by V = {V_GBi} = J*u. Taking the time derivative in G gives
<pre>
A = d/dt V = {A_GBi} = J*udot + JDot*u
</pre>
where JDot=JDot(q,u). This method returns JDot*u, which depends only on
configuration q and speeds u. Note that the same u is used to calculate JDot,
which is linear in u, so this term is quadratic in u.
<h3>Implementation</h3>
This method simply extracts the total Coriolis acceleration for each body that
is already available in the \a state cache so there is no computation done
here.
@see getTotalCoriolisAcceleration()
**/
void calcBiasForSystemJacobian(const State& state,
Vector_<SpatialVec>& JDotu) const;
/** Alternate signature that returns the bias as a 6*nb-vector of scalars
rather than as an nb-vector of 2x3 spatial vectors. See the other signature for
documentation. **/
void calcBiasForSystemJacobian(const State& state,
Vector& JDotu) const;
/** Calculate the product of the transposed kinematic Jacobian ~J (==J^T) and
a vector F_G of spatial force-like elements, one per body, in O(n) time to
produce a generalized force-like result f=~J*F. If F_G is actually a set of
spatial forces applied at the body frame origin of each body, and expressed
in the Ground frame, then the result is the equivalent set of generalized
forces f that would produce the same accelerations as F_G.
@param[in] state
A State compatible with this System that has already been realized to
Stage::Position.
@param[in] F_G
This is a vector of SpatialVec elements, one per mobilized body and in
order of MobilizedBodyIndex (with the 0th entry a force on Ground; hence
ignored). Each SpatialVec is a spatial force-like pair of 3-vectors
(torque,force) with the force applied at the body origin and the vectors
expressed in Ground.
@param[out] f
This is the product f=~J*F_G as described above. This result is in the
generalized force space, that is, it has one scalar entry for each of the
n system mobilities (velocity degrees of freedom). Resized if necessary.
The kinematic Jacobian (partial velocity matrix) J is defined as follows:
<pre>
partial(V) T T
J = ----------, V = [V_GB0 V_GB1 ... V_GB nb-1] , u = [u0 u1 ... u n-1]
partial(u)
</pre>
Thus the element J(i,j)=partial(V_GBi)/partial(uj) (each element of J is a
spatial vector). J maps generalized speeds to spatial velocities (see
multiplyBySystemJacobian()); its transpose ~J maps spatial forces
to generalized forces.
Note that we're using "monogram" notation for the spatial velocities, where
<pre>
G Bi
V_GBi = V
</pre>
the spatial velocity of body i's body frame Bi (at its origin), measured and
expressed in the Ground frame G.
<h3>Performance discussion</h3>
This is a very fast operator, costing about 18*nb+11*n flops, where nb is the
number of bodies and n the number of mobilities (degrees of freedom) u. In
contrast, even if you have already calculated the entire 6*nbXnu matrix J, the
multiplication ~J*F would cost 12*nb*n flops. As an example, for a 20 body
system with a free flying base and 19 pin joints (25 dofs altogether), this
method takes 18*20+11*25=635 flops while the explicit matrix-vector multiply
would take 12*20*25=6000 flops. So this method is already >9X faster for
that small system; for larger systems the difference grows rapidly.
@see multiplyBySystemJacobian(), calcSystemJacobian() **/
void multiplyBySystemJacobianTranspose( const State& state,
const Vector_<SpatialVec>& F_G,
Vector& f) const;
/** Explicitly calculate and return the nb x nu whole-system kinematic
Jacobian J_G, with each element a 2x3 spatial vector (SpatialVec). This matrix
maps generalized speeds to the spatial velocities of all the bodies, which
will be at the body origins, measured and expressed
in Ground. That is, if you have a set of n generalized speeds u, you can
find the spatial velocities of all nb bodies as V_G = J_G*u. The transpose of
this matrix maps a set of spatial forces F_G, applied at the body frame
origins and expressed in Ground, to the equivalent set of n generalized
forces f: f = ~J_G*F_G.
@note The 0th row of the returned Jacobian is always zero since it represents
the spatial velocity of Ground.
<h3>Performance discussion</h3>
Before using this method, consider whether you really need to form this
very large matrix which necessarily will take O(n^2) space and time; it will
almost always be \e much faster to use the multiplyBySystemJacobian() method
that directly calculate the matrix-vector product in O(n) time without explicitly
forming the matrix. Here are the details:
As currently implemented, forming the full Jacobian J costs about
12*n*(nb+n) flops. Assuming nb ~= n, this is about 24*n^2 flops. Then
if you want to form a product J*u explicitly, the matrix-vector multiply will
cost about 12*n^2 flops each time you do it. In contrast the J*u product is
calculated using multiplyBySystemJacobian() in about 24*n flops. Even for
very small systems it is cheaper to make repeated calls to
multiplyBySystemJacobian() than to form J explicitly and multiply by it.
See the Performance section for multiplyBySystemJacobian() for more
comparisons.
@see multiplyBySystemJacobian(), multiplyBySystemJacobianTranspose()
@see calcSystemJacobian() alternate signature using scalar elements **/
void calcSystemJacobian(const State& state,
Matrix_<SpatialVec>& J_G) const; // nb X nu
/** Alternate signature that returns a system Jacobian as a 6*nb X n Matrix
of scalars rather than as an nb X n matrix of 2x3 spatial vectors. See
the other signature for documentation and important performance
considerations. **/
void calcSystemJacobian(const State& state,
Matrix& J_G) const; // 6 nb X nu
/** Calculate the Cartesian ground-frame velocities of a set of task stations
(points fixed on bodies) that results from a particular set of generalized
speeds u. The result is the station velocities measured and expressed in Ground.
@param[in] state
A State that has already been realized through Position stage.
@param[in] onBodyB
An array of nt mobilized bodies (one per task) to which the stations of
interest are fixed.
@param[in] stationPInB
The array of nt station points P of interest (one per task), each
corresponding to one of the bodies B from \a onBodyB, given as vectors
from each body B's origin Bo to its station P, expressed in frame B.
@param[in] u
A mobility-space Vector, such as a set of generalized speeds. The length
and order must match the mobilities of this system (that is n, the number
of generalized speeds u, \e not nq, the number of generalized
coordinates q).
@param[out] JSu
The resulting product JS*u, where JS is the station task Jacobian. Resized
to nt if needed.
<h3>Performance discussion</h3>
It is almost always better to use this method than to form an explicit 3*nt X n
station task Jacobian explicitly and then multiply by it. If you have only one
or two tasks, so that the matrix is only 3xn or 6xn, and then perform many
multiplies with that matrix, it might be slightly cheaper to form it. For
example, it is about 4X cheaper to use this method than to form a one-task
Station Jacobian JS explicitly and use it once. However, because this would be
such a skinny matrix (3 X n) explicit multiplication is cheap so if you will
re-use this same Jacobian repeatedly before recalculating (at least 6 times)
then it may be worth calculating and saving it. Here are the details:
A call to this method costs 27*nt + 12*(nb+n) flops. If you assume that
nb ~= n >> 1, you could say this is about 27*nt + 24*n flops. In
contrast, assuming you already have the 3*nt X n station Jacobian JS available,
you can compute the JS*u product in about 6*nt*n flops, 3X faster for one task,
about even for three tasks, and slower for more than three tasks.
However forming JS costs about 40*nt+90*n flops (see calcStationJacobian()).
So to form a one-task Jacobian and use it once is 4X more expensive (96*n vs
24*n), but if you use it more than 5 times it is cheaper to do it
explicitly. Forming a one-task JS and using it 100 times costs about 690*n
flops while calling this method 100 times would cost about 2400*n flops.
@see multiplyByStationJacobianTranspose(), calcStationJacobian() **/
void multiplyByStationJacobian(const State& state,
const Array_<MobilizedBodyIndex>& onBodyB,
const Array_<Vec3>& stationPInB,
const Vector& u,
Vector_<Vec3>& JSu) const;
/** Alternate signature for when you just have a single station task.
@returns JS*u, where JS is the station task Jacobian. **/
Vec3 multiplyByStationJacobian(const State& state,
MobilizedBodyIndex onBodyB,
const Vec3& stationPInB,
const Vector& u) const
{
ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
Vector_<Vec3> JSu(1);
multiplyByStationJacobian(state, bodies, stations, u, JSu);
return JSu[0];
}
/** Calculate the generalized forces resulting from a single force applied
to a set of nt station tasks (points fixed to bodies) P. The applied forces
f_GP should be 3-vectors expressed in Ground. This is considerably faster than
forming the Jacobian explicitly and then performing the matrix-vector multiply.
<h3>Performance discussion</h3>
Cost is about 30*nt + 18*nb + 11*n. Assuming nb ~= n, this is roughly
30*(n+nt). In contrast, forming the complete 3*nt X n matrix would cost about
90*(n+nt/2), and subsequent explicit matrix-vector multiplies would cost
about 6*nt*n each.
@see multiplyByStationJacobian(), calcStationJacobian() **/
void multiplyByStationJacobianTranspose
(const State& state,
const Array_<MobilizedBodyIndex>& onBodyB,
const Array_<Vec3>& stationPInB,
const Vector_<Vec3>& f_GP,
Vector& f) const;
/** Alternate signature for when you just have a single station task. **/
void multiplyByStationJacobianTranspose
(const State& state,
MobilizedBodyIndex onBodyB,
const Vec3& stationPInB,
const Vec3& f_GP,
Vector& f) const
{
ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
Vector_<Vec3> forces(1, f_GP);
multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
}
/** Explicitly calculate and return the 3*nt x n kinematic Jacobian JS for a
set of nt station tasks P (a station is a point fixed on a particular mobilized
body). This matrix maps generalized speeds to the Cartesian velocity of each
station, measured and expressed in Ground. That is, if you have a set of n
generalized speeds u, you can find the Cartesian velocities of stations P as
v_GP = JS*u, where v_GP is a 3*nt column vector. The transpose of this
matrix maps a 3*nt vector of forces f_GP (expressed in Ground and applied
to P) to the equivalent set of n generalized forces f: f = ~JS*f_GP.
@note It is almost always far more efficient to use multiplyByStationJacobian()
or multiplyByStationJacobianTranspose() to form matrix-vector products rather
than to use this method to form the Jacobian explicitly. See the performance
discussions there.
Overloaded signatures of this method are available to allow you to obtain the
Jacobian either as an nt X n Matrix with Vec3 elements, or as 3*nt X n Matrix
with scalar elements.
@param[in] state
A State that has already been realized through Position stage.
@param[in] onBodyB
An array of nt mobilized bodies (one per task) to which the stations of
interest are fixed.
@param[in] stationPInB
The array of nt station points P of interest (one per task), each
corresponding to one of the bodies B from \a onBodyB, given as vectors
from each body B's origin Bo to its station P, expressed in frame B.
@param[out] JS
The resulting nt X n station task Jacobian. Resized if necessary.
<h3>Performance discussion</h3>
The cost of a call to this method is about 42*nt + 54*nb + 33*n flops. If we
assume that nb ~= n >> 1, this is roughly 90*(n+nt/2) flops. Then once the
Station Jacobian JS has been formed, each JS*u matrix-vector product costs
6*nt*n flops to form. When nt is small enough (say one or two tasks), and you
plan to re-use it a lot, this can be computationally efficient; but for single
use or more than a few tasks you can do much better with
multiplyByStationJacobian() or multiplyByStationJacobianTranspose().
@see multiplyByStationJacobian(), multiplyByStationJacobianTranspose() **/
void calcStationJacobian(const State& state,
const Array_<MobilizedBodyIndex>& onBodyB,
const Array_<Vec3>& stationPInB,
Matrix_<Vec3>& JS) const;
/** Alternate signature for when you just have a single station task. **/
void calcStationJacobian(const State& state,
MobilizedBodyIndex onBodyB,
const Vec3& stationPInB,
RowVector_<Vec3>& JS) const
{
ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
calcStationJacobian(state, bodies, stations, JS);
}
/** Alternate signature that returns a station Jacobian as a 3*nt x n Matrix
rather than as a Matrix of Vec3 elements. See the other signature for
documentation and important performance considerations. **/
void calcStationJacobian(const State& state,
const Array_<MobilizedBodyIndex>& onBodyB,
const Array_<Vec3>& stationPInB,
Matrix& JS) const;
/** Alternate signature for when you just have a single station task. **/
void calcStationJacobian(const State& state,
MobilizedBodyIndex onBodyB,
const Vec3& stationPInB,
Matrix& JS) const
{
ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
calcStationJacobian(state, bodies, stations, JS);
}
/** Calculate the acceleration bias term for a station Jacobian, that is, the
part of the station's acceleration that is due only to velocities. This term
is also known as the Coriolis acceleration, and it is returned here as a linear
acceleration of the station in Ground.
@param[in] state
A State that has already been realized through Velocity stage.
@param[in] onBodyB
An array of nt mobilized bodies (one per task) to which the stations of
interest are fixed.
@param[in] stationPInB
The array of nt station points P of interest (one per task), each
corresponding to one of the bodies B from \a onBodyB, given as vectors
from each body B's origin Bo to its station P, expressed in frame B.
@param[out] JSDotu
The resulting product JSDot*u, where JSDot is the time derivative of JS,
the station task Jacobian. Resized to nt if needed.
<h3>Theory</h3>
The velocity v_GP of a station point P in the Ground frame G can be obtained
from the generalized speeds u using the station Jacobian for P, as <pre>
v_GP = JS_P*u
</pre> Taking the time derivative in G gives <pre>
a_GP = JS_P*udot + JSDot_P*u
</pre>
This method returns JSDot_P*u, which depends only on configuration and
velocities. We allow for a set of task points P so that all their bias terms
can be calculated in a single sweep of the multibody tree. Note that u is taken
from the \a state and that the same u shown above is also used to calculate
JSDot_P, which is linear in u, so the bias term is quadratic in u.
<h3>Implementation</h3>
This method just obtains body B's total Coriolis acceleration already available
in the \a state cache and shifts it to station point P. Cost is 48*nt flops.
@see getTotalCoriolisAcceleration(), shiftAccelerationBy()
**/
void calcBiasForStationJacobian(const State& state,
const Array_<MobilizedBodyIndex>& onBodyB,
const Array_<Vec3>& stationPInB,
Vector_<Vec3>& JSDotu) const;
/** Alternate signature that returns the bias as a 3*nt-vector of scalars
rather than as an nt-vector of Vec3s. See the other signature for
documentation. **/
void calcBiasForStationJacobian(const State& state,
const Array_<MobilizedBodyIndex>& onBodyB,
const Array_<Vec3>& stationPInB,
Vector& JSDotu) const;
/** Alternate signature for when you just have a single station task.
@returns JSDot*u, where JSDot is the station Jacobian time derivative. **/
Vec3 calcBiasForStationJacobian(const State& state,
MobilizedBodyIndex onBodyB,
const Vec3& stationPInB) const
{
ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
Vector_<Vec3> JSDotu(1);
calcBiasForStationJacobian(state, bodies, stations, JSDotu);
return JSDotu[0];
}
/** Calculate the spatial velocities of a set of nt task frames A={Ai} fixed to
nt bodies B={Bi}, that result from a particular set of n generalized speeds u.
The result is each task frame's angular and linear velocity measured and
expressed in Ground. Using this method is considerably faster than forming the
6*nt X n Frame Jacobian explicitly and then performing the matrix-vector
multiply. See the performance analysis below for details.
There is a simplified signature of this method available if you have only a
single frame task.
@param[in] state
A State that has already been realized through Position stage.
@param[in] onBodyB
An array of nt mobilized bodies (one per task) to which the task frames of
interest are fixed. These may be in any order and the same body may appear
more than once if there are multiple task frames on it.
@param[in] originAoInB
An array of nt frame origin points Ao for the task frames interest (one
per task), each corresponding to one of the bodies B from \a onBodyB, given
as vectors from each body B's origin Bo to its task frame origin Ao,
expressed in frame B.
@param[in] u
A mobility-space Vector, such as a set of generalized speeds. The length
and order must match the mobilities of this system (that is n, the number
of generalized speeds u, \e not nq, the number of generalized
coordinates q).
@param[out] JFu
The resulting product JF*u, where JF is the frame task Jacobian. Resized
if needed to a Vector of nt SpatialVec entries.
@note All frames A fixed to a given body B have the same angular velocity so
we do not actually need to know the task frames' orientations here, just the
location on B of their origin points Ao. If you have a Transform X_BA giving
the pose of frame A in the body frame B, you can extract the position vector
for the origin point Ao using X_BA.p() and pass that as the \a originAoInB
parameter here.
<h3>Performance discussion</h3>
A call to this method costs 27*nt + 12*(nb+n) flops. If you assume that
nb ~= n >> 1, you could say this is about 25*(nt+n) flops. In contrast, assuming
you already have the 6*nt X n Frame Jacobian JF available, you can compute the
JF*u product in about 12*nt*n flops. If you have just one task (nt==1) this
explicit multiplication is about twice as fast; at two tasks it is about even
and for more than two it is more expensive. However forming JF costs about
180*(n+nt/4) flops (see calcFrameJacobian()). So to form a one-task Jacobian
and use it once is almost 8X more expensive (192*n vs 25*n), but if you use it
more than 16 times it is (marginally) cheaper to do it explicitly (for one
task). For example, forming a one-task JF and using it 100 times costs 1392*n
flops while calling this method 100 times would cost about 2500*n flops.
Conclusion: in almost all practical cases you are better off using this operator
rather than forming JF, even if you have only a single frame task and certainly
if you have more than two tasks.
@see multiplyByFrameJacobianTranspose(), calcFrameJacobian() **/
void multiplyByFrameJacobian
(const State& state,
const Array_<MobilizedBodyIndex>& onBodyB,
const Array_<Vec3>& originAoInB,
const Vector& u,
Vector_<SpatialVec>& JFu) const;
/** Simplified signature for when you just have a single frame task; see the
main signature for documentation.
@returns JF*u, where JF is the single frame task Jacobian. **/
SpatialVec multiplyByFrameJacobian( const State& state,
MobilizedBodyIndex onBodyB,
const Vec3& originAoInB,