DART  6.10.1
GenericJoint.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2011-2021, The DART development contributors
3  * All rights reserved.
4  *
5  * The list of contributors can be found at:
6  * https://github.com/dartsim/dart/blob/master/LICENSE
7  *
8  * This file is provided under the following "BSD-style" License:
9  * Redistribution and use in source and binary forms, with or
10  * without modification, are permitted provided that the following
11  * conditions are met:
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19  * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20  * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26  * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  */
32 
33 #ifndef DART_DYNAMICS_DETAIL_GenericJoint_HPP_
34 #define DART_DYNAMICS_DETAIL_GenericJoint_HPP_
35 
37 
38 #include "dart/config.hpp"
42 #include "dart/math/Helpers.hpp"
43 
44 #define GenericJoint_REPORT_DIM_MISMATCH(func, arg) \
45  { \
46  dterr << "[GenericJoint::" #func "] Mismatch beteween size of " \
47  << #arg " [" << arg.size() << "] and the number of " \
48  << "DOFs [" << getNumDofs() << "] for Joint named [" \
49  << this->getName() << "].\n"; \
50  assert(false); \
51  }
52 
53 #define GenericJoint_REPORT_OUT_OF_RANGE(func, index) \
54  { \
55  dterr << "[GenericJoint::" << #func << "] The index [" << index \
56  << "] is out of range for Joint named [" << this->getName() \
57  << "] which has " << this->getNumDofs() << " DOFs.\n"; \
58  assert(false); \
59  }
60 
61 #define GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(func) \
62  { \
63  dterr << "[GenericJoint::" #func "] Unsupported actuator type (" \
64  << Joint::mAspectProperties.mActuatorType << ") for Joint [" \
65  << this->getName() << "].\n"; \
66  assert(false); \
67  }
68 
69 #define GenericJoint_SET_IF_DIFFERENT(mField, value) \
70  if (value == Base::mAspectProperties.mField) \
71  return; \
72  Base::mAspectProperties.mField = value; \
73  Joint::incrementVersion();
74 
75 namespace dart {
76 namespace dynamics {
77 
78 //==============================================================================
79 //
80 // These namespace-level definitions are required to enable ODR-use of static
81 // constexpr member variables.
82 //
83 // See this StackOverflow answer: http://stackoverflow.com/a/14396189/111426
84 //
85 template <class ConfigSpaceT>
87 
88 //==============================================================================
89 template <class ConfigSpaceT>
91 {
92  for (auto i = 0u; i < NumDofs; ++i)
93  delete mDofs[i];
94 }
95 
96 //==============================================================================
97 template <class ConfigSpaceT>
99 {
100  Joint::setProperties(static_cast<const Joint::Properties&>(properties));
101  setProperties(static_cast<const UniqueProperties&>(properties));
102 }
103 
104 //==============================================================================
105 template <class ConfigSpaceT>
108 {
109  setAspectProperties(properties);
110 }
111 
112 //==============================================================================
113 template <class ConfigSpaceT>
115 {
116  setCommands(state.mCommands);
117  setPositionsStatic(state.mPositions);
118  setVelocitiesStatic(state.mVelocities);
119  setAccelerationsStatic(state.mAccelerations);
120  setForces(state.mForces);
121 }
122 
123 //==============================================================================
124 template <class ConfigSpaceT>
127 {
128  for (auto i = 0u; i < NumDofs; ++i)
129  {
130  setDofName(i, properties.mDofNames[i], properties.mPreserveDofNames[i]);
131  setPositionLowerLimit(i, properties.mPositionLowerLimits[i]);
132  setPositionUpperLimit(i, properties.mPositionUpperLimits[i]);
133  setInitialPosition(i, properties.mInitialPositions[i]);
134  setVelocityLowerLimit(i, properties.mVelocityLowerLimits[i]);
135  setVelocityUpperLimit(i, properties.mVelocityUpperLimits[i]);
136  setInitialVelocity(i, properties.mInitialVelocities[i]);
137  setAccelerationLowerLimit(i, properties.mAccelerationLowerLimits[i]);
138  setAccelerationUpperLimit(i, properties.mAccelerationUpperLimits[i]);
139  setForceLowerLimit(i, properties.mForceLowerLimits[i]);
140  setForceUpperLimit(i, properties.mForceUpperLimits[i]);
141  setSpringStiffness(i, properties.mSpringStiffnesses[i]);
142  setRestPosition(i, properties.mRestPositions[i]);
143  setDampingCoefficient(i, properties.mDampingCoefficients[i]);
144  setCoulombFriction(i, properties.mFrictions[i]);
145  }
146 }
147 
148 //==============================================================================
149 template <class ConfigSpaceT>
152 {
154  Joint::mAspectProperties, Base::mAspectProperties);
155 }
156 
157 //==============================================================================
158 template <class ConfigSpaceT>
160 {
161  if (this == &other)
162  return;
163 
164  setProperties(other.getGenericJointProperties());
165 }
166 
167 //==============================================================================
168 template <class ConfigSpaceT>
170 {
171  if (nullptr == other)
172  return;
173 
174  copy(*other);
175 }
176 
177 //==============================================================================
178 template <class ConfigSpaceT>
180  const GenericJoint<ConfigSpaceT>& other)
181 {
182  copy(other);
183  return *this;
184 }
185 
186 //==============================================================================
187 template <class ConfigSpaceT>
189 {
190  if (index < NumDofs)
191  return mDofs[index];
192 
194 
195  return nullptr;
196 }
197 
198 //==============================================================================
199 template <class ConfigSpaceT>
201 {
202  if (index < NumDofs)
203  return mDofs[index];
204 
206 
207  return nullptr;
208 }
209 
210 //==============================================================================
211 template <class ConfigSpaceT>
213 {
214  return NumDofs;
215 }
216 
217 //==============================================================================
218 template <class ConfigSpaceT>
220  size_t index, const std::string& name, bool preserveName)
221 {
222  if (NumDofs <= index)
223  {
224  dterr << "[GenericJoint::setDofName] Attempting to set the name of DOF "
225  << "index " << index << ", which is out of bounds for the Joint ["
226  << this->getName()
227  << "]. We will set the name of DOF index 0 instead.\n";
228  assert(false);
229  index = 0u;
230  }
231 
232  preserveDofName(index, preserveName);
233 
234  std::string& dofName = Base::mAspectProperties.mDofNames[index];
235 
236  if (name == dofName)
237  return dofName;
238 
239  const SkeletonPtr& skel
240  = this->mChildBodyNode ? this->mChildBodyNode->getSkeleton() : nullptr;
241  if (skel)
242  dofName = skel->mNameMgrForDofs.changeObjectName(mDofs[index], name);
243  else
244  dofName = name;
245 
246  return dofName;
247 }
248 
249 //==============================================================================
250 template <class ConfigSpaceT>
252 {
253  if (NumDofs <= index)
254  {
255  GenericJoint_REPORT_OUT_OF_RANGE(preserveDofName, index);
256  return;
257  }
258 
259  GenericJoint_SET_IF_DIFFERENT(mPreserveDofNames[index], preserve);
260 }
261 
262 //==============================================================================
263 template <class ConfigSpaceT>
265 {
266  if (NumDofs <= index)
267  {
269  index = 0;
270  }
271 
272  return Base::mAspectProperties.mPreserveDofNames[index];
273 }
274 
275 //==============================================================================
276 template <class ConfigSpaceT>
277 const std::string& GenericJoint<ConfigSpaceT>::getDofName(size_t index) const
278 {
279  if (NumDofs <= index)
280  {
281  dterr << "[GenericJoint::getDofName] Requested name of DOF index [" << index
282  << "] in Joint [" << this->getName() << "], but that is "
283  << "out of bounds (max " << NumDofs - 1
284  << "). Returning name of DOF 0.\n";
285  assert(false);
286  return Base::mAspectProperties.mDofNames[0];
287  }
288 
289  return Base::mAspectProperties.mDofNames[index];
290 }
291 
292 //==============================================================================
293 template <class ConfigSpaceT>
295 {
296  if (index >= getNumDofs())
297  {
299  return 0;
300  }
301 
302  return mDofs[index]->mIndexInSkeleton;
303 }
304 
305 //==============================================================================
306 template <class ConfigSpaceT>
308 {
309  if (index >= getNumDofs())
310  {
311  GenericJoint_REPORT_OUT_OF_RANGE(getIndexInTree, index);
312  return 0;
313  }
314 
315  return mDofs[index]->mIndexInTree;
316 }
317 
318 //==============================================================================
319 template <class ConfigSpaceT>
320 void GenericJoint<ConfigSpaceT>::setCommand(size_t index, double command)
321 {
322  if (index >= getNumDofs())
324 
325  switch (Joint::mAspectProperties.mActuatorType)
326  {
327  case Joint::FORCE:
328  this->mAspectState.mCommands[index] = math::clip(
329  command,
330  Base::mAspectProperties.mForceLowerLimits[index],
331  Base::mAspectProperties.mForceUpperLimits[index]);
332  break;
333  case Joint::PASSIVE:
334  if (0.0 != command)
335  {
336  dtwarn << "[GenericJoint::setCommand] Attempting to set a non-zero ("
337  << command << ") command for a PASSIVE joint ["
338  << this->getName() << "].\n";
339  }
340  this->mAspectState.mCommands[index] = command;
341  break;
342  case Joint::SERVO:
343  this->mAspectState.mCommands[index] = math::clip(
344  command,
345  Base::mAspectProperties.mVelocityLowerLimits[index],
346  Base::mAspectProperties.mVelocityUpperLimits[index]);
347  break;
348  case Joint::MIMIC:
349  if (0.0 != command)
350  {
351  dtwarn << "[GenericJoint::setCommand] Attempting to set a non-zero ("
352  << command << ") command for a MIMIC joint [" << this->getName()
353  << "].\n";
354  }
355  this->mAspectState.mCommands[index] = math::clip(
356  command,
357  Base::mAspectProperties.mVelocityLowerLimits[index],
358  Base::mAspectProperties.mVelocityUpperLimits[index]);
359  break;
360  case Joint::ACCELERATION:
361  this->mAspectState.mCommands[index] = math::clip(
362  command,
363  Base::mAspectProperties.mAccelerationLowerLimits[index],
364  Base::mAspectProperties.mAccelerationUpperLimits[index]);
365  break;
366  case Joint::VELOCITY:
367  this->mAspectState.mCommands[index] = math::clip(
368  command,
369  Base::mAspectProperties.mVelocityLowerLimits[index],
370  Base::mAspectProperties.mVelocityUpperLimits[index]);
371  // TODO: This possibly makes the acceleration to exceed the limits.
372  break;
373  case Joint::LOCKED:
374  if (0.0 != command)
375  {
376  dtwarn << "[GenericJoint::setCommand] Attempting to set a non-zero ("
377  << command << ") command for a LOCKED joint [" << this->getName()
378  << "].\n";
379  }
380  this->mAspectState.mCommands[index] = command;
381  break;
382  default:
383  assert(false);
384  break;
385  }
386 }
387 
388 //==============================================================================
389 template <class ConfigSpaceT>
391 {
392  if (index >= getNumDofs())
393  {
395  return 0.0;
396  }
397 
398  return this->mAspectState.mCommands[index];
399 }
400 
401 //==============================================================================
402 template <class ConfigSpaceT>
403 void GenericJoint<ConfigSpaceT>::setCommands(const Eigen::VectorXd& commands)
404 {
405  if (static_cast<size_t>(commands.size()) != getNumDofs())
406  {
407  GenericJoint_REPORT_DIM_MISMATCH(setCommands, commands);
408  return;
409  }
410 
411  switch (Joint::mAspectProperties.mActuatorType)
412  {
413  case Joint::FORCE:
414  this->mAspectState.mCommands = math::clip(
415  commands,
416  Base::mAspectProperties.mForceLowerLimits,
417  Base::mAspectProperties.mForceUpperLimits);
418  break;
420  if (Vector::Zero() != commands)
421  {
422  dtwarn << "[GenericJoint::setCommands] Attempting to set a non-zero ("
423  << commands.transpose() << ") command for a PASSIVE joint ["
424  << this->getName() << "].\n";
425  }
426  this->mAspectState.mCommands = commands;
427  break;
428  case Joint::SERVO:
429  this->mAspectState.mCommands = math::clip(
430  commands,
431  Base::mAspectProperties.mVelocityLowerLimits,
432  Base::mAspectProperties.mVelocityUpperLimits);
433  break;
435  if (Vector::Zero() != commands)
436  {
437  dtwarn << "[GenericJoint::setCommands] Attempting to set a non-zero ("
438  << commands.transpose() << ") command for a MIMIC joint ["
439  << this->getName() << "].\n";
440  }
441  this->mAspectState.mCommands = math::clip(
442  commands,
443  Base::mAspectProperties.mVelocityLowerLimits,
444  Base::mAspectProperties.mVelocityUpperLimits);
445  break;
446  case Joint::ACCELERATION:
447  this->mAspectState.mCommands = math::clip(
448  commands,
449  Base::mAspectProperties.mAccelerationLowerLimits,
450  Base::mAspectProperties.mAccelerationUpperLimits);
451  break;
452  case Joint::VELOCITY:
453  this->mAspectState.mCommands = math::clip(
454  commands,
455  Base::mAspectProperties.mVelocityLowerLimits,
456  Base::mAspectProperties.mVelocityUpperLimits);
457  // TODO: This possibly makes the acceleration to exceed the limits.
458  break;
459  case Joint::LOCKED:
460  if (Vector::Zero() != commands)
461  {
462  dtwarn << "[GenericJoint::setCommands] Attempting to set a non-zero ("
463  << commands.transpose() << ") command for a LOCKED joint ["
464  << this->getName() << "].\n";
465  }
466  this->mAspectState.mCommands = commands;
467  break;
468  default:
469  assert(false);
470  break;
471  }
472 }
473 
474 //==============================================================================
475 template <class ConfigSpaceT>
477 {
478  return this->mAspectState.mCommands;
479 }
480 
481 //==============================================================================
482 template <class ConfigSpaceT>
484 {
485  this->mAspectState.mCommands.setZero();
486 }
487 
488 //==============================================================================
489 template <class ConfigSpaceT>
491 {
492  if (index >= getNumDofs())
493  {
495  return;
496  }
497 
498  if (this->mAspectState.mPositions[index] == position)
499  return;
500  // TODO(JS): Above code should be changed something like:
501  // if (ConfigSpaceT::getEuclideanPoint(mPositions, index) == position)
502  // return;
503 
504  // Note: It would not make much sense to use setPositionsStatic() here
505  this->mAspectState.mPositions[index] = position;
506  this->notifyPositionUpdated();
507 }
508 
509 //==============================================================================
510 template <class ConfigSpaceT>
512 {
513  if (index >= getNumDofs())
514  {
516  return 0.0;
517  }
518 
519  return getPositionsStatic()[index];
520 }
521 
522 //==============================================================================
523 template <class ConfigSpaceT>
524 void GenericJoint<ConfigSpaceT>::setPositions(const Eigen::VectorXd& positions)
525 {
526  if (static_cast<size_t>(positions.size()) != getNumDofs())
527  {
528  GenericJoint_REPORT_DIM_MISMATCH(setPositions, positions);
529  return;
530  }
531 
532  setPositionsStatic(positions);
533 }
534 
535 //==============================================================================
536 template <class ConfigSpaceT>
538 {
539  return getPositionsStatic();
540 }
541 
542 //==============================================================================
543 template <class ConfigSpaceT>
545  size_t index, double position)
546 {
547  if (index >= getNumDofs())
548  {
549  GenericJoint_REPORT_OUT_OF_RANGE(setPositionLowerLimit, index);
550  return;
551  }
552 
554 }
555 
556 //==============================================================================
557 template <class ConfigSpaceT>
559 {
560  if (index >= getNumDofs())
561  {
562  GenericJoint_REPORT_OUT_OF_RANGE(getPositionLowerLimit, index);
563  return 0.0;
564  }
565 
566  return Base::mAspectProperties.mPositionLowerLimits[index];
567 }
568 
569 //==============================================================================
570 template <class ConfigSpaceT>
572  const Eigen::VectorXd& lowerLimits)
573 {
574  if (static_cast<size_t>(lowerLimits.size()) != getNumDofs())
575  {
576  GenericJoint_REPORT_DIM_MISMATCH(setPositionLowerLimits, lowerLimits);
577  return;
578  }
579 
580  GenericJoint_SET_IF_DIFFERENT(mPositionLowerLimits, lowerLimits);
581 }
582 
583 //==============================================================================
584 template <class ConfigSpaceT>
586 {
587  return Base::mAspectProperties.mPositionLowerLimits;
588 }
589 
590 //==============================================================================
591 template <class ConfigSpaceT>
593  size_t index, double position)
594 {
595  if (index >= getNumDofs())
596  {
597  GenericJoint_REPORT_OUT_OF_RANGE(setPositionUpperLimit, index);
598  return;
599  }
600 
601  GenericJoint_SET_IF_DIFFERENT(mPositionUpperLimits[index], position);
602 }
603 
604 //==============================================================================
605 template <class ConfigSpaceT>
607 {
608  if (index >= getNumDofs())
609  {
610  GenericJoint_REPORT_OUT_OF_RANGE(getPositionUpperLimit, index);
611  return 0.0;
612  }
613 
614  return Base::mAspectProperties.mPositionUpperLimits[index];
615 }
616 
617 //==============================================================================
618 template <class ConfigSpaceT>
620  const Eigen::VectorXd& upperLimits)
621 {
622  if (static_cast<size_t>(upperLimits.size()) != getNumDofs())
623  {
624  GenericJoint_REPORT_DIM_MISMATCH(setPositionUpperLimits, upperLimits);
625  return;
626  }
627 
628  GenericJoint_SET_IF_DIFFERENT(mPositionUpperLimits, upperLimits);
629 }
630 
631 //==============================================================================
632 template <class ConfigSpaceT>
634 {
635  return Base::mAspectProperties.mPositionUpperLimits;
636 }
637 
638 //==============================================================================
639 template <class ConfigSpaceT>
641 {
642  if (index >= getNumDofs())
643  {
645  return true;
646  }
647 
648  return std::isfinite(Base::mAspectProperties.mPositionUpperLimits[index])
649  || std::isfinite(Base::mAspectProperties.mPositionLowerLimits[index]);
650 }
651 
652 //==============================================================================
653 template <class ConfigSpaceT>
655 {
656  if (index >= getNumDofs())
657  {
658  GenericJoint_REPORT_OUT_OF_RANGE(resetPosition, index);
659  return;
660  }
661 
662  setPosition(index, Base::mAspectProperties.mInitialPositions[index]);
663 }
664 
665 //==============================================================================
666 template <class ConfigSpaceT>
668 {
669  setPositionsStatic(Base::mAspectProperties.mInitialPositions);
670 }
671 
672 //==============================================================================
673 template <class ConfigSpaceT>
675  size_t index, double initial)
676 {
677  if (index >= getNumDofs())
678  {
679  GenericJoint_REPORT_OUT_OF_RANGE(setInitialPosition, index);
680  return;
681  }
682 
683  GenericJoint_SET_IF_DIFFERENT(mInitialPositions[index], initial);
684 }
685 
686 //==============================================================================
687 template <class ConfigSpaceT>
689 {
690  if (index >= getNumDofs())
691  {
692  GenericJoint_REPORT_OUT_OF_RANGE(getInitialPosition, index);
693  return 0.0;
694  }
695 
696  return Base::mAspectProperties.mInitialPositions[index];
697 }
698 
699 //==============================================================================
700 template <class ConfigSpaceT>
702  const Eigen::VectorXd& initial)
703 {
704  if (static_cast<size_t>(initial.size()) != getNumDofs())
705  {
706  GenericJoint_REPORT_DIM_MISMATCH(setInitialPositions, initial);
707  return;
708  }
709 
710  GenericJoint_SET_IF_DIFFERENT(mInitialPositions, initial);
711 }
712 
713 //==============================================================================
714 template <class ConfigSpaceT>
716 {
717  return Base::mAspectProperties.mInitialPositions;
718 }
719 
720 //==============================================================================
721 template <class ConfigSpaceT>
723 {
724  if (this->mAspectState.mPositions == positions)
725  return;
726 
727  this->mAspectState.mPositions = positions;
728  this->notifyPositionUpdated();
729 }
730 
731 //==============================================================================
732 template <class ConfigSpaceT>
735 {
736  return this->mAspectState.mPositions;
737 }
738 
739 //==============================================================================
740 template <class ConfigSpaceT>
742 {
743  if (this->mAspectState.mVelocities == velocities)
744  return;
745 
746  this->mAspectState.mVelocities = velocities;
747  this->notifyVelocityUpdated();
748 }
749 
750 //==============================================================================
751 template <class ConfigSpaceT>
754 {
755  return this->mAspectState.mVelocities;
756 }
757 
758 //==============================================================================
759 template <class ConfigSpaceT>
761 {
762  if (this->mAspectState.mAccelerations == accels)
763  return;
764 
765  this->mAspectState.mAccelerations = accels;
766  this->notifyAccelerationUpdated();
767 }
768 
769 //==============================================================================
770 template <class ConfigSpaceT>
773 {
774  return this->mAspectState.mAccelerations;
775 }
776 
777 //==============================================================================
778 template <class ConfigSpaceT>
780 {
781  if (index >= getNumDofs())
782  {
784  return;
785  }
786 
787  if (this->mAspectState.mVelocities[index] == velocity)
788  return;
789 
790  // Note: It would not make much sense to use setVelocitiesStatic() here
791  this->mAspectState.mVelocities[index] = velocity;
792  this->notifyVelocityUpdated();
793 
794  if (Joint::mAspectProperties.mActuatorType == Joint::VELOCITY)
795  this->mAspectState.mCommands[index] = this->getVelocitiesStatic()[index];
796 }
797 
798 //==============================================================================
799 template <class ConfigSpaceT>
801 {
802  if (index >= getNumDofs())
803  {
805  return 0.0;
806  }
807 
808  return getVelocitiesStatic()[index];
809 }
810 
811 //==============================================================================
812 template <class ConfigSpaceT>
814  const Eigen::VectorXd& velocities)
815 {
816  if (static_cast<size_t>(velocities.size()) != getNumDofs())
817  {
818  GenericJoint_REPORT_DIM_MISMATCH(setVelocities, velocities);
819  return;
820  }
821 
822  setVelocitiesStatic(velocities);
823 
824  if (Joint::mAspectProperties.mActuatorType == Joint::VELOCITY)
825  this->mAspectState.mCommands = this->getVelocitiesStatic();
826 }
827 
828 //==============================================================================
829 template <class ConfigSpaceT>
831 {
832  return getVelocitiesStatic();
833 }
834 
835 //==============================================================================
836 template <class ConfigSpaceT>
838  size_t index, double velocity)
839 {
840  if (index >= getNumDofs())
841  {
842  GenericJoint_REPORT_OUT_OF_RANGE(setVelocityLowerLimit, index);
843  return;
844  }
845 
846  GenericJoint_SET_IF_DIFFERENT(mVelocityLowerLimits[index], velocity);
847 }
848 
849 //==============================================================================
850 template <class ConfigSpaceT>
852 {
853  if (index >= getNumDofs())
854  {
855  GenericJoint_REPORT_OUT_OF_RANGE(getVelocityLowerLimit, index);
856  return 0.0;
857  }
858 
859  return Base::mAspectProperties.mVelocityLowerLimits[index];
860 }
861 
862 //==============================================================================
863 template <class ConfigSpaceT>
865  const Eigen::VectorXd& lowerLimits)
866 {
867  if (static_cast<size_t>(lowerLimits.size()) != getNumDofs())
868  {
869  GenericJoint_REPORT_DIM_MISMATCH(setVelocityLowerLimits, lowerLimits);
870  return;
871  }
872 
873  GenericJoint_SET_IF_DIFFERENT(mVelocityLowerLimits, lowerLimits);
874 }
875 
876 //==============================================================================
877 template <class ConfigSpaceT>
879 {
880  return Base::mAspectProperties.mVelocityLowerLimits;
881 }
882 
883 //==============================================================================
884 template <class ConfigSpaceT>
886  size_t index, double velocity)
887 {
888  if (index >= getNumDofs())
889  {
890  GenericJoint_REPORT_OUT_OF_RANGE(setVelocityUpperLimit, index);
891  return;
892  }
893 
894  GenericJoint_SET_IF_DIFFERENT(mVelocityUpperLimits[index], velocity);
895 }
896 
897 //==============================================================================
898 template <class ConfigSpaceT>
900 {
901  if (index >= getNumDofs())
902  {
903  GenericJoint_REPORT_OUT_OF_RANGE(getVelocityUpperLimit, index);
904  return 0.0;
905  }
906 
907  return Base::mAspectProperties.mVelocityUpperLimits[index];
908 }
909 
910 //==============================================================================
911 template <class ConfigSpaceT>
913  const Eigen::VectorXd& upperLimits)
914 {
915  if (static_cast<size_t>(upperLimits.size()) != getNumDofs())
916  {
917  GenericJoint_REPORT_DIM_MISMATCH(setVelocityUpperLimits, upperLimits);
918  return;
919  }
920 
921  GenericJoint_SET_IF_DIFFERENT(mVelocityUpperLimits, upperLimits);
922 }
923 
924 //==============================================================================
925 template <class ConfigSpaceT>
927 {
928  return Base::mAspectProperties.mVelocityUpperLimits;
929 }
930 
931 //==============================================================================
932 template <class ConfigSpaceT>
934 {
935  if (index >= getNumDofs())
936  {
937  GenericJoint_REPORT_OUT_OF_RANGE(resetVelocity, index);
938  return;
939  }
940 
941  setVelocity(index, Base::mAspectProperties.mInitialVelocities[index]);
942 }
943 
944 //==============================================================================
945 template <class ConfigSpaceT>
947 {
948  setVelocitiesStatic(Base::mAspectProperties.mInitialVelocities);
949 }
950 
951 //==============================================================================
952 template <class ConfigSpaceT>
954  size_t index, double initial)
955 {
956  if (index >= getNumDofs())
957  {
958  GenericJoint_REPORT_OUT_OF_RANGE(setInitialVelocity, index);
959  return;
960  }
961 
962  GenericJoint_SET_IF_DIFFERENT(mInitialVelocities[index], initial);
963 }
964 
965 //==============================================================================
966 template <class ConfigSpaceT>
968 {
969  if (index >= getNumDofs())
970  {
971  GenericJoint_REPORT_OUT_OF_RANGE(getInitialVelocity, index);
972  return 0.0;
973  }
974 
975  return Base::mAspectProperties.mInitialVelocities[index];
976 }
977 
978 //==============================================================================
979 template <class ConfigSpaceT>
981  const Eigen::VectorXd& initial)
982 {
983  if (static_cast<size_t>(initial.size()) != getNumDofs())
984  {
985  GenericJoint_REPORT_DIM_MISMATCH(setInitialVelocities, initial);
986  return;
987  }
988 
989  GenericJoint_SET_IF_DIFFERENT(mInitialVelocities, initial);
990 }
991 
992 //==============================================================================
993 template <class ConfigSpaceT>
995 {
996  return Base::mAspectProperties.mInitialVelocities;
997 }
998 
999 //==============================================================================
1000 template <class ConfigSpaceT>
1002  size_t index, double acceleration)
1003 {
1004  if (index >= getNumDofs())
1005  {
1006  GenericJoint_REPORT_OUT_OF_RANGE(setAcceleration, index);
1007  return;
1008  }
1009 
1010  if (this->mAspectState.mAccelerations[index] == acceleration)
1011  return;
1012 
1013  // Note: It would not make much sense to use setAccelerationsStatic() here
1014  this->mAspectState.mAccelerations[index] = acceleration;
1015  this->notifyAccelerationUpdated();
1016 
1017  if (Joint::mAspectProperties.mActuatorType == Joint::ACCELERATION)
1018  this->mAspectState.mCommands[index] = this->getAccelerationsStatic()[index];
1019 }
1020 
1021 //==============================================================================
1022 template <class ConfigSpaceT>
1024 {
1025  if (index >= getNumDofs())
1026  {
1027  GenericJoint_REPORT_OUT_OF_RANGE(getAcceleration, index);
1028  return 0.0;
1029  }
1030 
1031  return getAccelerationsStatic()[index];
1032 }
1033 
1034 //==============================================================================
1035 template <class ConfigSpaceT>
1037  const Eigen::VectorXd& accelerations)
1038 {
1039  if (static_cast<size_t>(accelerations.size()) != getNumDofs())
1040  {
1041  GenericJoint_REPORT_DIM_MISMATCH(setAccelerations, accelerations);
1042  return;
1043  }
1044 
1045  setAccelerationsStatic(accelerations);
1046 
1047  if (Joint::mAspectProperties.mActuatorType == Joint::ACCELERATION)
1048  this->mAspectState.mCommands = this->getAccelerationsStatic();
1049 }
1050 
1051 //==============================================================================
1052 template <class ConfigSpaceT>
1054 {
1055  return getAccelerationsStatic();
1056 }
1057 
1058 //==============================================================================
1059 template <class ConfigSpaceT>
1061  size_t index, double acceleration)
1062 {
1063  if (index >= getNumDofs())
1064  {
1065  GenericJoint_REPORT_OUT_OF_RANGE(setAccelerationLowerLimit, index);
1066  return;
1067  }
1068 
1069  GenericJoint_SET_IF_DIFFERENT(mAccelerationLowerLimits[index], acceleration);
1070 }
1071 
1072 //==============================================================================
1073 template <class ConfigSpaceT>
1075 {
1076  if (index >= getNumDofs())
1077  {
1078  GenericJoint_REPORT_OUT_OF_RANGE(getAccelerationLowerLimit, index);
1079  return 0.0;
1080  }
1081 
1082  return Base::mAspectProperties.mAccelerationLowerLimits[index];
1083 }
1084 
1085 //==============================================================================
1086 template <class ConfigSpaceT>
1088  const Eigen::VectorXd& lowerLimits)
1089 {
1090  if (static_cast<size_t>(lowerLimits.size()) != getNumDofs())
1091  {
1092  GenericJoint_REPORT_DIM_MISMATCH(setAccelerationLowerLimits, lowerLimits);
1093  return;
1094  }
1095 
1096  GenericJoint_SET_IF_DIFFERENT(mAccelerationLowerLimits, lowerLimits);
1097 }
1098 
1099 //==============================================================================
1100 template <class ConfigSpaceT>
1102 {
1103  return Base::mAspectProperties.mAccelerationLowerLimits;
1104 }
1105 
1106 //==============================================================================
1107 template <class ConfigSpaceT>
1109  size_t index, double acceleration)
1110 {
1111  if (index >= getNumDofs())
1112  {
1113  GenericJoint_REPORT_OUT_OF_RANGE(setAccelerationUpperLimit, index) return;
1114  }
1115 
1116  GenericJoint_SET_IF_DIFFERENT(mAccelerationUpperLimits[index], acceleration);
1117 }
1118 
1119 //==============================================================================
1120 template <class ConfigSpaceT>
1122 {
1123  if (index >= getNumDofs())
1124  {
1125  GenericJoint_REPORT_OUT_OF_RANGE(getAccelerationUpperLimit, index);
1126  return 0.0;
1127  }
1128 
1129  return Base::mAspectProperties.mAccelerationUpperLimits[index];
1130 }
1131 
1132 //==============================================================================
1133 template <class ConfigSpaceT>
1135  const Eigen::VectorXd& upperLimits)
1136 {
1137  if (static_cast<size_t>(upperLimits.size()) != getNumDofs())
1138  {
1139  GenericJoint_REPORT_DIM_MISMATCH(setAccelerationUpperLimits, upperLimits);
1140  return;
1141  }
1142 
1143  GenericJoint_SET_IF_DIFFERENT(mAccelerationUpperLimits, upperLimits);
1144 }
1145 
1146 //==============================================================================
1147 template <class ConfigSpaceT>
1149 {
1150  return Base::mAspectProperties.mAccelerationUpperLimits;
1151 }
1152 
1153 //==============================================================================
1154 template <class ConfigSpaceT>
1156 {
1157  setAccelerationsStatic(Vector::Zero());
1158 }
1159 
1160 //==============================================================================
1161 template <class ConfigSpaceT>
1163 {
1164  if (index >= getNumDofs())
1165  {
1167  return;
1168  }
1169 
1170  this->mAspectState.mForces[index] = force;
1171 
1172  if (Joint::mAspectProperties.mActuatorType == Joint::FORCE)
1173  this->mAspectState.mCommands[index] = this->mAspectState.mForces[index];
1174 }
1175 
1176 //==============================================================================
1177 template <class ConfigSpaceT>
1179 {
1180  if (index >= getNumDofs())
1181  {
1183  return 0.0;
1184  }
1185 
1186  return this->mAspectState.mForces[index];
1187 }
1188 
1189 //==============================================================================
1190 template <class ConfigSpaceT>
1191 void GenericJoint<ConfigSpaceT>::setForces(const Eigen::VectorXd& forces)
1192 {
1193  if (static_cast<size_t>(forces.size()) != getNumDofs())
1194  {
1195  GenericJoint_REPORT_DIM_MISMATCH(setForces, forces);
1196  return;
1197  }
1198 
1199  this->mAspectState.mForces = forces;
1200 
1201  if (Joint::mAspectProperties.mActuatorType == Joint::FORCE)
1202  this->mAspectState.mCommands = this->mAspectState.mForces;
1203 }
1204 
1205 //==============================================================================
1206 template <class ConfigSpaceT>
1208 {
1209  return this->mAspectState.mForces;
1210 }
1211 
1212 //==============================================================================
1213 template <class ConfigSpaceT>
1215 {
1216  if (index >= getNumDofs())
1217  {
1218  GenericJoint_REPORT_OUT_OF_RANGE(setForceLowerLimit, index);
1219  return;
1220  }
1221 
1222  GenericJoint_SET_IF_DIFFERENT(mForceLowerLimits[index], force);
1223 }
1224 
1225 //==============================================================================
1226 template <class ConfigSpaceT>
1228 {
1229  if (index >= getNumDofs())
1230  {
1231  GenericJoint_REPORT_OUT_OF_RANGE(getForceLowerLimit, index);
1232  return 0.0;
1233  }
1234 
1235  return Base::mAspectProperties.mForceLowerLimits[index];
1236 }
1237 
1238 //==============================================================================
1239 template <class ConfigSpaceT>
1241  const Eigen::VectorXd& lowerLimits)
1242 {
1243  if (static_cast<size_t>(lowerLimits.size()) != getNumDofs())
1244  {
1245  GenericJoint_REPORT_DIM_MISMATCH(setForceLowerLimits, lowerLimits);
1246  return;
1247  }
1248 
1249  GenericJoint_SET_IF_DIFFERENT(mForceLowerLimits, lowerLimits);
1250 }
1251 
1252 //==============================================================================
1253 template <class ConfigSpaceT>
1255 {
1256  return Base::mAspectProperties.mForceLowerLimits;
1257 }
1258 
1259 //==============================================================================
1260 template <class ConfigSpaceT>
1262 {
1263  if (index >= getNumDofs())
1264  {
1265  GenericJoint_REPORT_OUT_OF_RANGE(setForceUpperLimit, index);
1266  return;
1267  }
1268 
1269  GenericJoint_SET_IF_DIFFERENT(mForceUpperLimits[index], force);
1270 }
1271 
1272 //==============================================================================
1273 template <class ConfigSpaceT>
1275 {
1276  if (index >= getNumDofs())
1277  {
1278  GenericJoint_REPORT_OUT_OF_RANGE(getForceUpperLimit, index);
1279  return 0.0;
1280  }
1281 
1282  return Base::mAspectProperties.mForceUpperLimits[index];
1283 }
1284 
1285 //==============================================================================
1286 template <class ConfigSpaceT>
1288  const Eigen::VectorXd& upperLimits)
1289 {
1290  if (static_cast<size_t>(upperLimits.size()) != getNumDofs())
1291  {
1292  GenericJoint_REPORT_DIM_MISMATCH(setForceUpperLimits, upperLimits);
1293  return;
1294  }
1295 
1296  GenericJoint_SET_IF_DIFFERENT(mForceUpperLimits, upperLimits);
1297 }
1298 
1299 //==============================================================================
1300 template <class ConfigSpaceT>
1302 {
1303  return Base::mAspectProperties.mForceUpperLimits;
1304 }
1305 
1306 //==============================================================================
1307 template <class ConfigSpaceT>
1309 {
1310  this->mAspectState.mForces.setZero();
1311 
1312  if (Joint::mAspectProperties.mActuatorType == Joint::FORCE)
1313  this->mAspectState.mCommands = this->mAspectState.mForces;
1314 }
1315 
1316 //==============================================================================
1317 template <class ConfigSpaceT>
1319  size_t index, double velocityChange)
1320 {
1321  if (index >= getNumDofs())
1322  {
1323  GenericJoint_REPORT_OUT_OF_RANGE(setVelocityChange, index);
1324  return;
1325  }
1326 
1327  mVelocityChanges[index] = velocityChange;
1328 }
1329 
1330 //==============================================================================
1331 template <class ConfigSpaceT>
1333 {
1334  if (index >= getNumDofs())
1335  {
1336  GenericJoint_REPORT_OUT_OF_RANGE(getVelocityChange, index);
1337  return 0.0;
1338  }
1339 
1340  return mVelocityChanges[index];
1341 }
1342 
1343 //==============================================================================
1344 template <class ConfigSpaceT>
1346 {
1347  mVelocityChanges.setZero();
1348 }
1349 
1350 //==============================================================================
1351 template <class ConfigSpaceT>
1353  size_t index, double impulse)
1354 {
1355  if (index >= getNumDofs())
1356  {
1357  GenericJoint_REPORT_OUT_OF_RANGE(setConstraintImpulse, index);
1358  return;
1359  }
1360 
1361  mConstraintImpulses[index] = impulse;
1362 }
1363 
1364 //==============================================================================
1365 template <class ConfigSpaceT>
1367 {
1368  if (index >= getNumDofs())
1369  {
1370  GenericJoint_REPORT_OUT_OF_RANGE(getConstraintImpulse, index);
1371  return 0.0;
1372  }
1373 
1374  return mConstraintImpulses[index];
1375 }
1376 
1377 //==============================================================================
1378 template <class ConfigSpaceT>
1380 {
1381  mConstraintImpulses.setZero();
1382 }
1383 
1384 //==============================================================================
1385 template <class ConfigSpaceT>
1387 {
1388  const Point& point = math::integratePosition<ConfigSpaceT>(
1389  math::toManifoldPoint<ConfigSpaceT>(getPositionsStatic()),
1390  getVelocitiesStatic(),
1391  dt);
1392 
1393  setPositionsStatic(math::toEuclideanPoint<ConfigSpaceT>(point));
1394 }
1395 
1396 //==============================================================================
1397 template <class ConfigSpaceT>
1399 {
1400  setVelocitiesStatic(math::integrateVelocity<ConfigSpaceT>(
1401  getVelocitiesStatic(), getAccelerationsStatic(), dt));
1402 }
1403 
1404 //==============================================================================
1405 template <class ConfigSpaceT>
1407  const Eigen::VectorXd& q2, const Eigen::VectorXd& q1) const
1408 {
1409  if (static_cast<size_t>(q1.size()) != getNumDofs()
1410  || static_cast<size_t>(q2.size()) != getNumDofs())
1411  {
1412  dterr << "[GenericJoint::getPositionsDifference] q1's size [" << q1.size()
1413  << "] or q2's size [" << q2.size() << "] must both equal the dof ["
1414  << this->getNumDofs() << "] for Joint [" << this->getName() << "].\n";
1415  assert(false);
1416  return Eigen::VectorXd::Zero(getNumDofs());
1417  }
1418 
1419  return getPositionDifferencesStatic(q2, q1);
1420 }
1421 
1422 //==============================================================================
1423 template <class ConfigSpaceT>
1424 typename ConfigSpaceT::Vector
1426  const Vector& q2, const Vector& q1) const
1427 {
1428  return q2 - q1;
1429  // TODO(JS): Move this implementation to each configuration space classes.
1430 }
1431 
1432 //==============================================================================
1433 template <class ConfigSpaceT>
1435 {
1436  if (index >= getNumDofs())
1437  {
1438  GenericJoint_REPORT_OUT_OF_RANGE(setSpringStiffness, index);
1439  return;
1440  }
1441 
1442  assert(k >= 0.0);
1443 
1444  GenericJoint_SET_IF_DIFFERENT(mSpringStiffnesses[index], k);
1445 }
1446 
1447 //==============================================================================
1448 template <class ConfigSpaceT>
1450 {
1451  if (index >= getNumDofs())
1452  {
1453  GenericJoint_REPORT_OUT_OF_RANGE(getSpringStiffness, index);
1454  return 0.0;
1455  }
1456 
1457  return Base::mAspectProperties.mSpringStiffnesses[index];
1458 }
1459 
1460 //==============================================================================
1461 template <class ConfigSpaceT>
1463 {
1464  if (index >= getNumDofs())
1465  {
1466  GenericJoint_REPORT_OUT_OF_RANGE(setRestPosition, index);
1467  return;
1468  }
1469 
1470  GenericJoint_SET_IF_DIFFERENT(mRestPositions[index], q0);
1471 }
1472 
1473 //==============================================================================
1474 template <class ConfigSpaceT>
1476 {
1477  if (index >= getNumDofs())
1478  {
1479  GenericJoint_REPORT_OUT_OF_RANGE(getRestPosition, index);
1480  return 0.0;
1481  }
1482 
1483  return Base::mAspectProperties.mRestPositions[index];
1484 }
1485 
1486 //==============================================================================
1487 template <class ConfigSpaceT>
1489 {
1490  if (index >= getNumDofs())
1491  {
1492  GenericJoint_REPORT_OUT_OF_RANGE(setDampingCoefficient, index);
1493  return;
1494  }
1495 
1496  assert(d >= 0.0);
1497 
1498  GenericJoint_SET_IF_DIFFERENT(mDampingCoefficients[index], d);
1499 }
1500 
1501 //==============================================================================
1502 template <class ConfigSpaceT>
1504 {
1505  if (index >= getNumDofs())
1506  {
1507  GenericJoint_REPORT_OUT_OF_RANGE(getDampingCoefficient, index);
1508  return 0.0;
1509  }
1510 
1511  return Base::mAspectProperties.mDampingCoefficients[index];
1512 }
1513 
1514 //==============================================================================
1515 template <class ConfigSpaceT>
1517  size_t index, double friction)
1518 {
1519  if (index >= getNumDofs())
1520  {
1521  GenericJoint_REPORT_OUT_OF_RANGE(setCoulombFriction, index);
1522  return;
1523  }
1524 
1525  assert(friction >= 0.0);
1526 
1528 }
1529 
1530 //==============================================================================
1531 template <class ConfigSpaceT>
1533 {
1534  if (index >= getNumDofs())
1535  {
1536  GenericJoint_REPORT_OUT_OF_RANGE(getCoulombFriction, index);
1537  return 0.0;
1538  }
1539 
1540  return Base::mAspectProperties.mFrictions[index];
1541 }
1542 
1543 //==============================================================================
1544 template <class ConfigSpaceT>
1546 {
1547  // Spring energy
1548  Vector displacement
1549  = getPositionsStatic() - Base::mAspectProperties.mRestPositions;
1550 
1551  const double pe
1552  = 0.5
1553  * displacement.dot(
1554  Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
1555  displacement));
1556 
1557  return pe;
1558 }
1559 
1560 //==============================================================================
1561 template <class ConfigSpaceT>
1563 {
1564  return getRelativeJacobianStatic();
1565 }
1566 
1567 //==============================================================================
1568 template <class ConfigSpaceT>
1571 {
1572  if (this->mIsRelativeJacobianDirty)
1573  {
1574  this->updateRelativeJacobian(false);
1575  this->mIsRelativeJacobianDirty = false;
1576  }
1577 
1578  return mJacobian;
1579 }
1580 
1581 //==============================================================================
1582 template <class ConfigSpaceT>
1584  const Eigen::VectorXd& positions) const
1585 {
1586  return getRelativeJacobianStatic(positions);
1587 }
1588 
1589 //==============================================================================
1590 template <class ConfigSpaceT>
1592  const
1593 {
1594  return getRelativeJacobianTimeDerivStatic();
1595 }
1596 
1597 //==============================================================================
1598 template <class ConfigSpaceT>
1601 {
1602  if (this->mIsRelativeJacobianTimeDerivDirty)
1603  {
1604  this->updateRelativeJacobianTimeDeriv();
1605  this->mIsRelativeJacobianTimeDerivDirty = false;
1606  }
1607 
1608  return mJacobianDeriv;
1609 }
1610 
1611 //==============================================================================
1612 template <class ConfigSpaceT>
1614  : mVelocityChanges(Vector::Zero()),
1615  mImpulses(Vector::Zero()),
1616  mConstraintImpulses(Vector::Zero()),
1617  mJacobian(JacobianMatrix::Zero()),
1618  mJacobianDeriv(JacobianMatrix::Zero()),
1619  mInvProjArtInertia(Matrix::Zero()),
1620  mInvProjArtInertiaImplicit(Matrix::Zero()),
1621  mTotalForce(Vector::Zero()),
1622  mTotalImpulse(Vector::Zero())
1623 {
1624  for (auto i = 0u; i < NumDofs; ++i)
1625  mDofs[i] = this->createDofPointer(i);
1626 
1627  // Joint and GenericJoint Aspects must be created by the most derived class.
1628  this->mAspectState.mPositions = properties.mInitialPositions;
1629  this->mAspectState.mVelocities = properties.mInitialVelocities;
1630 }
1631 
1632 //==============================================================================
1633 template <class ConfigSpaceT>
1635 {
1636  const SkeletonPtr& skel = this->mChildBodyNode->getSkeleton();
1637  for (auto i = 0u; i < NumDofs; ++i)
1638  {
1639  Base::mAspectProperties.mDofNames[i]
1640  = skel->mNameMgrForDofs.issueNewNameAndAdd(
1641  mDofs[i]->getName(), mDofs[i]);
1642  }
1643 }
1644 
1645 //==============================================================================
1646 template <class ConfigSpaceT>
1648 {
1649  assert(this->mChildBodyNode);
1650  return this->mChildBodyNode->getBodyForce()
1651  - this->getRelativeJacobianStatic() * this->mAspectState.mForces;
1652 }
1653 
1654 //==============================================================================
1655 template <class ConfigSpaceT>
1657 {
1658  this->mSpatialVelocity
1659  = this->getRelativeJacobianStatic() * this->getVelocitiesStatic();
1660 }
1661 
1662 //==============================================================================
1663 template <class ConfigSpaceT>
1665 {
1666  this->mSpatialAcceleration = this->getRelativePrimaryAcceleration()
1667  + this->getRelativeJacobianTimeDerivStatic()
1668  * this->getVelocitiesStatic();
1669 }
1670 
1671 //==============================================================================
1672 template <class ConfigSpaceT>
1674 {
1675  this->mPrimaryAcceleration
1676  = this->getRelativeJacobianStatic() * this->getAccelerationsStatic();
1677 }
1678 
1679 //==============================================================================
1680 template <class ConfigSpaceT>
1682 {
1683  // Add joint velocity to _vel
1684  vel.noalias() += getRelativeJacobianStatic() * getVelocitiesStatic();
1685 
1686  // Verification
1687  assert(!math::isNan(vel));
1688 }
1689 
1690 //==============================================================================
1691 template <class ConfigSpaceT>
1693  Eigen::Vector6d& partialAcceleration, const Eigen::Vector6d& childVelocity)
1694 {
1695  // ad(V, S * dq) + dS * dq
1696  partialAcceleration
1697  = math::ad(
1698  childVelocity, getRelativeJacobianStatic() * getVelocitiesStatic())
1699  + getRelativeJacobianTimeDerivStatic() * getVelocitiesStatic();
1700  // Verification
1701  assert(!math::isNan(partialAcceleration));
1702 }
1703 
1704 //==============================================================================
1705 template <class ConfigSpaceT>
1707 {
1708  // Add joint acceleration to _acc
1709  acc.noalias() += getRelativeJacobianStatic() * getAccelerationsStatic();
1710 
1711  // Verification
1712  assert(!math::isNan(acc));
1713 }
1714 
1715 //==============================================================================
1716 template <class ConfigSpaceT>
1718  Eigen::Vector6d& velocityChange)
1719 {
1720  // Add joint velocity change to velocityChange
1721  velocityChange.noalias() += getRelativeJacobianStatic() * mVelocityChanges;
1722 
1723  // Verification
1724  assert(!math::isNan(velocityChange));
1725 }
1726 
1727 //==============================================================================
1728 template <class ConfigSpaceT>
1729 const typename GenericJoint<ConfigSpaceT>::Matrix&
1731 {
1733 
1734  return mInvProjArtInertia;
1735 }
1736 
1737 //==============================================================================
1738 template <class ConfigSpaceT>
1739 const typename GenericJoint<ConfigSpaceT>::Matrix&
1741 {
1743 
1744  return mInvProjArtInertiaImplicit;
1745 }
1746 
1747 //==============================================================================
1748 template <class ConfigSpaceT>
1750  Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
1751 {
1752  switch (Joint::mAspectProperties.mActuatorType)
1753  {
1754  case Joint::FORCE:
1755  case Joint::PASSIVE:
1756  case Joint::SERVO:
1757  case Joint::MIMIC:
1758  addChildArtInertiaToDynamic(parentArtInertia, childArtInertia);
1759  break;
1760  case Joint::ACCELERATION:
1761  case Joint::VELOCITY:
1762  case Joint::LOCKED:
1763  addChildArtInertiaToKinematic(parentArtInertia, childArtInertia);
1764  break;
1765  default:
1766  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaTo);
1767  break;
1768  }
1769 }
1770 
1771 //==============================================================================
1772 template <class ConfigSpaceT>
1774  Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
1775 {
1776  // Child body's articulated inertia
1777  JacobianMatrix AIS = childArtInertia * getRelativeJacobianStatic();
1778  Eigen::Matrix6d PI = childArtInertia;
1779  PI.noalias() -= AIS * mInvProjArtInertia * AIS.transpose();
1780  assert(!math::isNan(PI));
1781 
1782  // Add child body's articulated inertia to parent body's articulated inertia.
1783  // Note that mT should be updated.
1784  parentArtInertia
1785  += math::transformInertia(this->getRelativeTransform().inverse(), PI);
1786 }
1787 
1788 //==============================================================================
1789 template <class ConfigSpaceT>
1791  Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
1792 {
1793  // Add child body's articulated inertia to parent body's articulated inertia.
1794  // Note that mT should be updated.
1795  parentArtInertia += math::transformInertia(
1796  this->getRelativeTransform().inverse(), childArtInertia);
1797 }
1798 
1799 //==============================================================================
1800 template <class ConfigSpaceT>
1802  Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
1803 {
1804  switch (Joint::mAspectProperties.mActuatorType)
1805  {
1806  case Joint::FORCE:
1807  case Joint::PASSIVE:
1808  case Joint::SERVO:
1809  case Joint::MIMIC:
1810  addChildArtInertiaImplicitToDynamic(parentArtInertia, childArtInertia);
1811  break;
1812  case Joint::ACCELERATION:
1813  case Joint::VELOCITY:
1814  case Joint::LOCKED:
1815  addChildArtInertiaImplicitToKinematic(parentArtInertia, childArtInertia);
1816  break;
1817  default:
1818  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(addChildArtInertiaImplicitTo);
1819  break;
1820  }
1821 }
1822 
1823 //==============================================================================
1824 template <class ConfigSpaceT>
1826  Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
1827 {
1828  // Child body's articulated inertia
1829  JacobianMatrix AIS = childArtInertia * getRelativeJacobianStatic();
1830  Eigen::Matrix6d PI = childArtInertia;
1831  PI.noalias() -= AIS * mInvProjArtInertiaImplicit * AIS.transpose();
1832  assert(!math::isNan(PI));
1833 
1834  // Add child body's articulated inertia to parent body's articulated inertia.
1835  // Note that mT should be updated.
1836  parentArtInertia
1837  += math::transformInertia(this->getRelativeTransform().inverse(), PI);
1838 }
1839 
1840 //==============================================================================
1841 template <class ConfigSpaceT>
1843  Eigen::Matrix6d& parentArtInertia, const Eigen::Matrix6d& childArtInertia)
1844 {
1845  // Add child body's articulated inertia to parent body's articulated inertia.
1846  // Note that mT should be updated.
1847  parentArtInertia += math::transformInertia(
1848  this->getRelativeTransform().inverse(), childArtInertia);
1849 }
1850 
1851 //==============================================================================
1852 template <class ConfigSpaceT>
1854  const Eigen::Matrix6d& artInertia)
1855 {
1856  switch (Joint::mAspectProperties.mActuatorType)
1857  {
1858  case Joint::FORCE:
1859  case Joint::PASSIVE:
1860  case Joint::SERVO:
1861  case Joint::MIMIC:
1862  updateInvProjArtInertiaDynamic(artInertia);
1863  break;
1864  case Joint::ACCELERATION:
1865  case Joint::VELOCITY:
1866  case Joint::LOCKED:
1867  updateInvProjArtInertiaKinematic(artInertia);
1868  break;
1869  default:
1870  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateInvProjArtInertia);
1871  break;
1872  }
1873 }
1874 
1875 //==============================================================================
1876 template <class ConfigSpaceT>
1878  const Eigen::Matrix6d& artInertia)
1879 {
1880  // Projected articulated inertia
1881  const JacobianMatrix& Jacobian = getRelativeJacobianStatic();
1882  const Matrix projAI = Jacobian.transpose() * artInertia * Jacobian;
1883 
1884  // Inversion of projected articulated inertia
1885  mInvProjArtInertia = math::inverse<ConfigSpaceT>(projAI);
1886 
1887  // Verification
1888  assert(!math::isNan(mInvProjArtInertia));
1889 }
1890 
1891 //==============================================================================
1892 template <class ConfigSpaceT>
1894  const Eigen::Matrix6d& /*_artInertia*/)
1895 {
1896  // Do nothing
1897 }
1898 
1899 //==============================================================================
1900 template <class ConfigSpaceT>
1902  const Eigen::Matrix6d& artInertia, double timeStep)
1903 {
1904  switch (Joint::mAspectProperties.mActuatorType)
1905  {
1906  case Joint::FORCE:
1907  case Joint::PASSIVE:
1908  case Joint::SERVO:
1909  case Joint::MIMIC:
1910  updateInvProjArtInertiaImplicitDynamic(artInertia, timeStep);
1911  break;
1912  case Joint::ACCELERATION:
1913  case Joint::VELOCITY:
1914  case Joint::LOCKED:
1915  updateInvProjArtInertiaImplicitKinematic(artInertia, timeStep);
1916  break;
1917  default:
1918  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateInvProjArtInertiaImplicit);
1919  break;
1920  }
1921 }
1922 
1923 //==============================================================================
1924 template <class ConfigSpaceT>
1926  const Eigen::Matrix6d& artInertia, double timeStep)
1927 {
1928  // Projected articulated inertia
1929  const JacobianMatrix& Jacobian = getRelativeJacobianStatic();
1930  Matrix projAI = Jacobian.transpose() * artInertia * Jacobian;
1931 
1932  // Add additional inertia for implicit damping and spring force
1933  projAI += (timeStep * Base::mAspectProperties.mDampingCoefficients
1934  + timeStep * timeStep * Base::mAspectProperties.mSpringStiffnesses)
1935  .asDiagonal();
1936 
1937  // Inversion of projected articulated inertia
1938  mInvProjArtInertiaImplicit = math::inverse<ConfigSpaceT>(projAI);
1939 
1940  // Verification
1941  assert(!math::isNan(mInvProjArtInertiaImplicit));
1942 }
1943 
1944 //==============================================================================
1945 template <class ConfigSpaceT>
1947  const Eigen::Matrix6d& /*artInertia*/, double /*timeStep*/)
1948 {
1949  // Do nothing
1950 }
1951 
1952 //==============================================================================
1953 template <class ConfigSpaceT>
1955  Eigen::Vector6d& parentBiasForce,
1956  const Eigen::Matrix6d& childArtInertia,
1957  const Eigen::Vector6d& childBiasForce,
1958  const Eigen::Vector6d& childPartialAcc)
1959 {
1960  switch (Joint::mAspectProperties.mActuatorType)
1961  {
1962  case Joint::FORCE:
1963  case Joint::PASSIVE:
1964  case Joint::SERVO:
1965  case Joint::MIMIC:
1966  addChildBiasForceToDynamic(
1967  parentBiasForce, childArtInertia, childBiasForce, childPartialAcc);
1968  break;
1969  case Joint::ACCELERATION:
1970  case Joint::VELOCITY:
1971  case Joint::LOCKED:
1972  addChildBiasForceToKinematic(
1973  parentBiasForce, childArtInertia, childBiasForce, childPartialAcc);
1974  break;
1975  default:
1976  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(addChildBiasForceTo);
1977  break;
1978  }
1979 }
1980 
1981 //==============================================================================
1982 template <class ConfigSpaceT>
1984  Eigen::Vector6d& parentBiasForce,
1985  const Eigen::Matrix6d& childArtInertia,
1986  const Eigen::Vector6d& childBiasForce,
1987  const Eigen::Vector6d& childPartialAcc)
1988 {
1989  // Compute beta
1990  const Eigen::Vector6d beta
1991  = childBiasForce
1992  + childArtInertia
1993  * (childPartialAcc
1994  + getRelativeJacobianStatic() * getInvProjArtInertiaImplicit()
1995  * mTotalForce);
1996 
1997  // Eigen::Vector6d beta
1998  // = _childBiasForce;
1999  // beta.noalias() += _childArtInertia * _childPartialAcc;
2000  // beta.noalias() += _childArtInertia * mJacobian *
2001  // getInvProjArtInertiaImplicit() * mTotalForce;
2002 
2003  // Verification
2004  assert(!math::isNan(beta));
2005 
2006  // Add child body's bias force to parent body's bias force. Note that mT
2007  // should be updated.
2008  parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta);
2009 }
2010 
2011 //==============================================================================
2012 template <class ConfigSpaceT>
2014  Eigen::Vector6d& _parentBiasForce,
2015  const Eigen::Matrix6d& childArtInertia,
2016  const Eigen::Vector6d& childBiasForce,
2017  const Eigen::Vector6d& childPartialAcc)
2018 {
2019  // Compute beta
2020  const Eigen::Vector6d beta
2021  = childBiasForce
2022  + childArtInertia
2023  * (childPartialAcc
2024  + getRelativeJacobianStatic() * getAccelerationsStatic());
2025 
2026  // Eigen::Vector6d beta
2027  // = _childBiasForce;
2028  // beta.noalias() += _childArtInertia * _childPartialAcc;
2029  // beta.noalias() += _childArtInertia * mJacobian *
2030  // getInvProjArtInertiaImplicit() * mTotalForce;
2031 
2032  // Verification
2033  assert(!math::isNan(beta));
2034 
2035  // Add child body's bias force to parent body's bias force. Note that mT
2036  // should be updated.
2037  _parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta);
2038 }
2039 
2040 //==============================================================================
2041 template <class ConfigSpaceT>
2043  Eigen::Vector6d& parentBiasImpulse,
2044  const Eigen::Matrix6d& childArtInertia,
2045  const Eigen::Vector6d& childBiasImpulse)
2046 {
2047  switch (Joint::mAspectProperties.mActuatorType)
2048  {
2049  case Joint::FORCE:
2050  case Joint::PASSIVE:
2051  case Joint::SERVO:
2052  case Joint::MIMIC:
2053  addChildBiasImpulseToDynamic(
2054  parentBiasImpulse, childArtInertia, childBiasImpulse);
2055  break;
2056  case Joint::ACCELERATION:
2057  case Joint::VELOCITY:
2058  case Joint::LOCKED:
2059  addChildBiasImpulseToKinematic(
2060  parentBiasImpulse, childArtInertia, childBiasImpulse);
2061  break;
2062  default:
2063  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(addChildBiasImpulseTo);
2064  break;
2065  }
2066 }
2067 
2068 //==============================================================================
2069 template <class ConfigSpaceT>
2071  Eigen::Vector6d& _parentBiasImpulse,
2072  const Eigen::Matrix6d& childArtInertia,
2073  const Eigen::Vector6d& childBiasImpulse)
2074 {
2075  // Compute beta
2076  const Eigen::Vector6d beta = childBiasImpulse
2077  + childArtInertia * getRelativeJacobianStatic()
2078  * getInvProjArtInertia() * mTotalImpulse;
2079 
2080  // Verification
2081  assert(!math::isNan(beta));
2082 
2083  // Add child body's bias force to parent body's bias force. Note that mT
2084  // should be updated.
2085  _parentBiasImpulse += math::dAdInvT(this->getRelativeTransform(), beta);
2086 }
2087 
2088 //==============================================================================
2089 template <class ConfigSpaceT>
2091  Eigen::Vector6d& parentBiasImpulse,
2092  const Eigen::Matrix6d& /*childArtInertia*/,
2093  const Eigen::Vector6d& childBiasImpulse)
2094 {
2095  // Add child body's bias force to parent body's bias force. Note that mT
2096  // should be updated.
2097  parentBiasImpulse
2098  += math::dAdInvT(this->getRelativeTransform(), childBiasImpulse);
2099 }
2100 
2101 //==============================================================================
2102 template <class ConfigSpaceT>
2104  const Eigen::Vector6d& bodyForce, double timeStep)
2105 {
2106  assert(timeStep > 0.0);
2107 
2108  switch (Joint::mAspectProperties.mActuatorType)
2109  {
2110  case Joint::FORCE:
2111  this->mAspectState.mForces = this->mAspectState.mCommands;
2112  updateTotalForceDynamic(bodyForce, timeStep);
2113  break;
2114  case Joint::PASSIVE:
2115  case Joint::SERVO:
2116  case Joint::MIMIC:
2117  this->mAspectState.mForces.setZero();
2118  updateTotalForceDynamic(bodyForce, timeStep);
2119  break;
2120  case Joint::ACCELERATION:
2121  setAccelerationsStatic(this->mAspectState.mCommands);
2122  updateTotalForceKinematic(bodyForce, timeStep);
2123  break;
2124  case Joint::VELOCITY:
2125  setAccelerationsStatic(
2126  (this->mAspectState.mCommands - getVelocitiesStatic()) / timeStep);
2127  updateTotalForceKinematic(bodyForce, timeStep);
2128  break;
2129  case Joint::LOCKED:
2130  setVelocitiesStatic(Vector::Zero());
2131  setAccelerationsStatic(Vector::Zero());
2132  updateTotalForceKinematic(bodyForce, timeStep);
2133  break;
2134  default:
2135  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateTotalForce);
2136  break;
2137  }
2138 }
2139 
2140 //==============================================================================
2141 template <class ConfigSpaceT>
2143  const Eigen::Vector6d& bodyForce, double timeStep)
2144 {
2145  // Spring force
2146  const Vector springForce
2147  = -Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
2148  getPositionsStatic() - Base::mAspectProperties.mRestPositions
2149  + getVelocitiesStatic() * timeStep);
2150 
2151  // Damping force
2152  const Vector dampingForce
2153  = -Base::mAspectProperties.mDampingCoefficients.cwiseProduct(
2154  getVelocitiesStatic());
2155 
2156  //
2157  mTotalForce = this->mAspectState.mForces + springForce + dampingForce
2158  - getRelativeJacobianStatic().transpose() * bodyForce;
2159 }
2160 
2161 //==============================================================================
2162 template <class ConfigSpaceT>
2164  const Eigen::Vector6d& /*bodyForce*/, double /*timeStep*/)
2165 {
2166  // Do nothing
2167 }
2168 
2169 //==============================================================================
2170 template <class ConfigSpaceT>
2172  const Eigen::Vector6d& bodyImpulse)
2173 {
2174  switch (Joint::mAspectProperties.mActuatorType)
2175  {
2176  case Joint::FORCE:
2177  case Joint::PASSIVE:
2178  case Joint::SERVO:
2179  case Joint::MIMIC:
2180  updateTotalImpulseDynamic(bodyImpulse);
2181  break;
2182  case Joint::ACCELERATION:
2183  case Joint::VELOCITY:
2184  case Joint::LOCKED:
2185  updateTotalImpulseKinematic(bodyImpulse);
2186  break;
2187  default:
2188  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateTotalImpulse);
2189  break;
2190  }
2191 }
2192 
2193 //==============================================================================
2194 template <class ConfigSpaceT>
2196  const Eigen::Vector6d& bodyImpulse)
2197 {
2198  //
2199  mTotalImpulse = mConstraintImpulses
2200  - getRelativeJacobianStatic().transpose() * bodyImpulse;
2201 }
2202 
2203 //==============================================================================
2204 template <class ConfigSpaceT>
2206  const Eigen::Vector6d& /*bodyImpulse*/)
2207 {
2208  // Do nothing
2209 }
2210 
2211 //==============================================================================
2212 template <class ConfigSpaceT>
2214 {
2215  mTotalImpulse.setZero();
2216 }
2217 
2218 //==============================================================================
2219 template <class ConfigSpaceT>
2221  const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& spatialAcc)
2222 {
2223  switch (Joint::mAspectProperties.mActuatorType)
2224  {
2225  case Joint::FORCE:
2226  case Joint::PASSIVE:
2227  case Joint::SERVO:
2228  case Joint::MIMIC:
2229  updateAccelerationDynamic(artInertia, spatialAcc);
2230  break;
2231  case Joint::ACCELERATION:
2232  case Joint::VELOCITY:
2233  case Joint::LOCKED:
2234  updateAccelerationKinematic(artInertia, spatialAcc);
2235  break;
2236  default:
2237  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateAcceleration);
2238  break;
2239  }
2240 }
2241 
2242 //==============================================================================
2243 template <class ConfigSpaceT>
2245  const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& spatialAcc)
2246 {
2247  //
2248  setAccelerationsStatic(
2249  getInvProjArtInertiaImplicit()
2250  * (mTotalForce
2251  - getRelativeJacobianStatic().transpose() * artInertia
2252  * math::AdInvT(this->getRelativeTransform(), spatialAcc)));
2253 
2254  // Verification
2255  assert(!math::isNan(getAccelerationsStatic()));
2256 }
2257 
2258 //==============================================================================
2259 template <class ConfigSpaceT>
2261  const Eigen::Matrix6d& /*artInertia*/,
2262  const Eigen::Vector6d& /*spatialAcc*/)
2263 {
2264  // Do nothing
2265 }
2266 
2267 //==============================================================================
2268 template <class ConfigSpaceT>
2270  const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& velocityChange)
2271 {
2272  switch (Joint::mAspectProperties.mActuatorType)
2273  {
2274  case Joint::FORCE:
2275  case Joint::PASSIVE:
2276  case Joint::SERVO:
2277  case Joint::MIMIC:
2278  updateVelocityChangeDynamic(artInertia, velocityChange);
2279  break;
2280  case Joint::ACCELERATION:
2281  case Joint::VELOCITY:
2282  case Joint::LOCKED:
2283  updateVelocityChangeKinematic(artInertia, velocityChange);
2284  break;
2285  default:
2286  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateVelocityChange);
2287  break;
2288  }
2289 }
2290 
2291 //==============================================================================
2292 template <class ConfigSpaceT>
2294  const Eigen::Matrix6d& artInertia, const Eigen::Vector6d& velocityChange)
2295 {
2296  //
2297  mVelocityChanges
2298  = getInvProjArtInertia()
2299  * (mTotalImpulse
2300  - getRelativeJacobianStatic().transpose() * artInertia
2301  * math::AdInvT(this->getRelativeTransform(), velocityChange));
2302 
2303  // Verification
2304  assert(!math::isNan(mVelocityChanges));
2305 }
2306 
2307 //==============================================================================
2308 template <class ConfigSpaceT>
2310  const Eigen::Matrix6d& /*artInertia*/,
2311  const Eigen::Vector6d& /*velocityChange*/)
2312 {
2313  // Do nothing
2314 }
2315 
2316 //==============================================================================
2317 template <class ConfigSpaceT>
2319  const Eigen::Vector6d& bodyForce,
2320  double timeStep,
2321  bool withDampingForces,
2322  bool withSpringForces)
2323 {
2324  this->mAspectState.mForces
2325  = getRelativeJacobianStatic().transpose() * bodyForce;
2326 
2327  // Implicit damping force:
2328  // tau_d = -Kd * dq - Kd * h * ddq
2329  if (withDampingForces)
2330  {
2331  const typename ConfigSpaceT::Vector dampingForces
2332  = -Base::mAspectProperties.mDampingCoefficients.cwiseProduct(
2333  getVelocitiesStatic() + getAccelerationsStatic() * timeStep);
2334  this->mAspectState.mForces -= dampingForces;
2335  }
2336 
2337  // Implicit spring force:
2338  // tau_s = -Kp * (q - q0) - Kp * h * dq - Kp * h^2 * ddq
2339  if (withSpringForces)
2340  {
2341  const typename ConfigSpaceT::Vector springForces
2342  = -Base::mAspectProperties.mSpringStiffnesses.cwiseProduct(
2343  getPositionsStatic() - Base::mAspectProperties.mRestPositions
2344  + getVelocitiesStatic() * timeStep
2345  + getAccelerationsStatic() * timeStep * timeStep);
2346  this->mAspectState.mForces -= springForces;
2347  }
2348 }
2349 
2350 //==============================================================================
2351 template <class ConfigSpaceT>
2353  const Eigen::Vector6d& bodyForce,
2354  double timeStep,
2355  bool withDampingForces,
2356  bool withSpringForces)
2357 {
2358  switch (Joint::mAspectProperties.mActuatorType)
2359  {
2360  case Joint::FORCE:
2361  case Joint::PASSIVE:
2362  case Joint::SERVO:
2363  case Joint::MIMIC:
2364  break;
2365  case Joint::ACCELERATION:
2366  case Joint::VELOCITY:
2367  case Joint::LOCKED:
2368  updateForceID(bodyForce, timeStep, withDampingForces, withSpringForces);
2369  break;
2370  default:
2372  break;
2373  }
2374 }
2375 
2376 //==============================================================================
2377 template <class ConfigSpaceT>
2379  const Eigen::Vector6d& bodyImpulse)
2380 {
2381  mImpulses = getRelativeJacobianStatic().transpose() * bodyImpulse;
2382 }
2383 
2384 //==============================================================================
2385 template <class ConfigSpaceT>
2387  const Eigen::Vector6d& bodyImpulse)
2388 {
2389  switch (Joint::mAspectProperties.mActuatorType)
2390  {
2391  case Joint::FORCE:
2392  case Joint::PASSIVE:
2393  case Joint::SERVO:
2394  case Joint::MIMIC:
2395  break;
2396  case Joint::ACCELERATION:
2397  case Joint::VELOCITY:
2398  case Joint::LOCKED:
2399  updateImpulseID(bodyImpulse);
2400  break;
2401  default:
2403  break;
2404  }
2405 }
2406 
2407 //==============================================================================
2408 template <class ConfigSpaceT>
2410 {
2411  switch (Joint::mAspectProperties.mActuatorType)
2412  {
2413  case Joint::FORCE:
2414  case Joint::PASSIVE:
2415  case Joint::SERVO:
2416  case Joint::MIMIC:
2417  updateConstrainedTermsDynamic(timeStep);
2418  break;
2419  case Joint::ACCELERATION:
2420  case Joint::VELOCITY:
2421  case Joint::LOCKED:
2422  updateConstrainedTermsKinematic(timeStep);
2423  break;
2424  default:
2425  GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(updateConstrainedTerms);
2426  break;
2427  }
2428 }
2429 
2430 //==============================================================================
2431 template <class ConfigSpaceT>
2433 {
2434  const double invTimeStep = 1.0 / timeStep;
2435 
2436  setVelocitiesStatic(getVelocitiesStatic() + mVelocityChanges);
2437  setAccelerationsStatic(
2438  getAccelerationsStatic() + mVelocityChanges * invTimeStep);
2439  this->mAspectState.mForces.noalias() += mImpulses * invTimeStep;
2440  // Note: As long as this is only called from BodyNode::updateConstrainedTerms
2441 }
2442 
2443 //==============================================================================
2444 template <class ConfigSpaceT>
2446  double timeStep)
2447 {
2448  this->mAspectState.mForces.noalias() += mImpulses / timeStep;
2449 }
2450 
2451 //==============================================================================
2452 template <class ConfigSpaceT>
2454  Eigen::Vector6d& parentBiasForce,
2455  const Eigen::Matrix6d& childArtInertia,
2456  const Eigen::Vector6d& childBiasForce)
2457 {
2458  // Compute beta
2459  Eigen::Vector6d beta = childBiasForce;
2460  beta.noalias() += childArtInertia * getRelativeJacobianStatic()
2461  * getInvProjArtInertia() * mInvM_a;
2462 
2463  // Verification
2464  assert(!math::isNan(beta));
2465 
2466  // Add child body's bias force to parent body's bias force. Note that mT
2467  // should be updated.
2468  parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta);
2469 }
2470 
2471 //==============================================================================
2472 template <class ConfigSpaceT>
2474  Eigen::Vector6d& parentBiasForce,
2475  const Eigen::Matrix6d& childArtInertia,
2476  const Eigen::Vector6d& childBiasForce)
2477 {
2478  // Compute beta
2479  Eigen::Vector6d beta = childBiasForce;
2480  beta.noalias() += childArtInertia * getRelativeJacobianStatic()
2481  * getInvProjArtInertiaImplicit() * mInvM_a;
2482 
2483  // Verification
2484  assert(!math::isNan(beta));
2485 
2486  // Add child body's bias force to parent body's bias force. Note that mT
2487  // should be updated.
2488  parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta);
2489 }
2490 
2491 //==============================================================================
2492 template <class ConfigSpaceT>
2494  const Eigen::Vector6d& bodyForce)
2495 {
2496  // Compute alpha
2497  mInvM_a = this->mAspectState.mForces
2498  - getRelativeJacobianStatic().transpose() * bodyForce;
2499 }
2500 
2501 //==============================================================================
2502 template <class ConfigSpaceT>
2504  Eigen::MatrixXd& _invMassMat,
2505  const size_t _col,
2506  const Eigen::Matrix6d& artInertia,
2507  const Eigen::Vector6d& spatialAcc)
2508 {
2509  //
2510  mInvMassMatrixSegment
2511  = getInvProjArtInertia()
2512  * (mInvM_a
2513  - getRelativeJacobianStatic().transpose() * artInertia
2514  * math::AdInvT(this->getRelativeTransform(), spatialAcc));
2515 
2516  // Verification
2517  assert(!math::isNan(mInvMassMatrixSegment));
2518 
2519  // Index
2520  size_t iStart = mDofs[0]->mIndexInTree;
2521 
2522  // Assign
2523  _invMassMat.block<NumDofs, 1>(iStart, _col) = mInvMassMatrixSegment;
2524 }
2525 
2526 //==============================================================================
2527 template <class ConfigSpaceT>
2529  Eigen::MatrixXd& invMassMat,
2530  const size_t col,
2531  const Eigen::Matrix6d& artInertia,
2532  const Eigen::Vector6d& spatialAcc)
2533 {
2534  //
2535  mInvMassMatrixSegment
2536  = getInvProjArtInertiaImplicit()
2537  * (mInvM_a
2538  - getRelativeJacobianStatic().transpose() * artInertia
2539  * math::AdInvT(this->getRelativeTransform(), spatialAcc));
2540 
2541  // Verification
2542  assert(!math::isNan(mInvMassMatrixSegment));
2543 
2544  // Index
2545  size_t iStart = mDofs[0]->mIndexInTree;
2546 
2547  // Assign
2548  invMassMat.block<NumDofs, 1>(iStart, col) = mInvMassMatrixSegment;
2549 }
2550 
2551 //==============================================================================
2552 template <class ConfigSpaceT>
2554 {
2555  //
2556  acc += getRelativeJacobianStatic() * mInvMassMatrixSegment;
2557 }
2558 
2559 //==============================================================================
2560 template <class ConfigSpaceT>
2562  const Eigen::Vector6d& spatial)
2563 {
2564  return getRelativeJacobianStatic().transpose() * spatial;
2565 }
2566 
2567 } // namespace dynamics
2568 } // namespace dart
2569 
2570 #endif // DART_DYNAMICS_DETAIL_GenericJoint_HPP_
#define dterr
Output an error message.
Definition: Console.hpp:49
#define dtwarn
Output a warning message.
Definition: Console.hpp:46
BodyPropPtr properties
Definition: SdfParser.cpp:80
std::string * name
Definition: SkelParser.cpp:1697
bool * preserveName
Definition: SkelParser.cpp:1696
Eigen::VectorXd velocity
Definition: SkelParser.cpp:109
Eigen::VectorXd force
Definition: SkelParser.cpp:111
std::size_t index
Definition: SkelParser.cpp:1672
Eigen::VectorXd acceleration
Definition: SkelParser.cpp:110
double * friction
Definition: SkelParser.cpp:1694
Eigen::VectorXd position
Definition: SkelParser.cpp:108
AspectProperties mAspectProperties
Aspect::Properties data, directly accessible to your derived class.
Definition: EmbeddedAspect.hpp:228
typename Impl::AspectState AspectState
Definition: EmbeddedAspect.hpp:440
AspectState mAspectState
Aspect::State data, directly accessible to your derived class.
Definition: EmbeddedAspect.hpp:415
typename Impl::AspectProperties AspectProperties
Definition: EmbeddedAspect.hpp:442
DegreeOfFreedom class is a proxy class for accessing single degrees of freedom (aka generalized coord...
Definition: DegreeOfFreedom.hpp:55
Definition: GenericJoint.hpp:48
void setAspectState(const AspectState &state)
Set the AspectState of this GenericJoint.
Definition: GenericJoint.hpp:114
void setForceLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:1240
double getCommand(std::size_t index) const override
Definition: GenericJoint.hpp:390
typename ConfigSpaceT::Matrix Matrix
Definition: GenericJoint.hpp:59
void addChildArtInertiaImplicitToKinematic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1842
Eigen::VectorXd getCommands() const override
Definition: GenericJoint.hpp:476
void resetVelocities() override
Definition: GenericJoint.hpp:946
const Vector & getPositionsStatic() const
Fixed-size version of getPositions()
Definition: GenericJoint.hpp:734
void setAccelerationsStatic(const Vector &accels)
Fixed-size version of setAccelerations()
Definition: GenericJoint.hpp:760
void copy(const ThisClass &otherJoint)
Copy the Properties of another GenericJoint.
Definition: GenericJoint.hpp:159
void setForceUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:1287
void updateTotalImpulseKinematic(const Eigen::Vector6d &bodyImpulse)
Definition: GenericJoint.hpp:2205
void setCoulombFriction(std::size_t index, double friction) override
Definition: GenericJoint.hpp:1516
const JacobianMatrix & getRelativeJacobianTimeDerivStatic() const
Fixed-size version of getRelativeJacobianTimeDeriv()
Definition: GenericJoint.hpp:1600
void setRestPosition(std::size_t index, double q0) override
Definition: GenericJoint.hpp:1462
void setAccelerations(const Eigen::VectorXd &accelerations) override
Definition: GenericJoint.hpp:1036
void updateTotalImpulse(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2171
void updateForceFD(const Eigen::Vector6d &bodyForce, double timeStep, bool withDampingForcese, bool withSpringForces) override
Definition: GenericJoint.hpp:2352
const Matrix & getInvProjArtInertiaImplicit() const
Get the inverse of projected articulated inertia for implicit joint damping and spring forces.
Definition: GenericJoint.hpp:1740
Eigen::VectorXd getPositionUpperLimits() const override
Definition: GenericJoint.hpp:633
void setVelocityUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:912
void updateForceID(const Eigen::Vector6d &bodyForce, double timeStep, bool withDampingForces, bool withSpringForces) override
Definition: GenericJoint.hpp:2318
Eigen::VectorXd getAccelerationUpperLimits() const override
Definition: GenericJoint.hpp:1148
void updateAccelerationKinematic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc)
Definition: GenericJoint.hpp:2260
void updateVelocityChangeDynamic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange)
Definition: GenericJoint.hpp:2293
Eigen::VectorXd getVelocityLowerLimits() const override
Definition: GenericJoint.hpp:878
void updateInvProjArtInertiaImplicit(const Eigen::Matrix6d &artInertia, double timeStep) override
Definition: GenericJoint.hpp:1901
void setVelocityLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:864
void updateConstrainedTermsDynamic(double timeStep)
Definition: GenericJoint.hpp:2432
void setPositionLowerLimit(std::size_t index, double position) override
Definition: GenericJoint.hpp:544
const GenericJoint< ConfigSpaceT >::JacobianMatrix & getRelativeJacobianStatic() const
Fixed-size version of getRelativeJacobian()
Definition: GenericJoint.hpp:1570
void addChildArtInertiaToDynamic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1773
const math::Jacobian getRelativeJacobianTimeDeriv() const override
Definition: GenericJoint.hpp:1591
void setVelocities(const Eigen::VectorXd &velocities) override
Definition: GenericJoint.hpp:813
void addChildBiasImpulseToKinematic(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse)
Definition: GenericJoint.hpp:2090
void addChildArtInertiaImplicitTo(Eigen::Matrix6d &parentArtInertiaImplicit, const Eigen::Matrix6d &childArtInertiaImplicit) override
Definition: GenericJoint.hpp:1801
void setPosition(std::size_t index, double position) override
Definition: GenericJoint.hpp:490
void updateVelocityChange(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange) override
Definition: GenericJoint.hpp:2269
void setCommand(std::size_t index, double command) override
Definition: GenericJoint.hpp:320
std::array< DegreeOfFreedom *, NumDofs > mDofs
Array of DegreeOfFreedom objects.
Definition: GenericJoint.hpp:678
void setInitialPosition(std::size_t index, double initial) override
Definition: GenericJoint.hpp:674
Eigen::VectorXd getForces() const override
Definition: GenericJoint.hpp:1207
void resetCommands() override
Definition: GenericJoint.hpp:483
void setVelocityChange(std::size_t index, double velocityChange) override
Definition: GenericJoint.hpp:1318
void setAccelerationUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:1134
void setPartialAccelerationTo(Eigen::Vector6d &partialAcceleration, const Eigen::Vector6d &childVelocity) override
Definition: GenericJoint.hpp:1692
double getVelocity(std::size_t index) const override
Definition: GenericJoint.hpp:800
Eigen::Vector6d getBodyConstraintWrench() const override
Definition: GenericJoint.hpp:1647
void addAccelerationTo(Eigen::Vector6d &acc) override
Definition: GenericJoint.hpp:1706
void updateTotalImpulseDynamic(const Eigen::Vector6d &bodyImpulse)
Definition: GenericJoint.hpp:2195
void setForce(std::size_t index, double force) override
Definition: GenericJoint.hpp:1162
void setInitialVelocity(std::size_t index, double initial) override
Definition: GenericJoint.hpp:953
double getForce(std::size_t index) const override
Definition: GenericJoint.hpp:1178
void setInitialVelocities(const Eigen::VectorXd &initial) override
Definition: GenericJoint.hpp:980
void updateInvProjArtInertiaDynamic(const Eigen::Matrix6d &artInertia)
Definition: GenericJoint.hpp:1877
DegreeOfFreedom * getDof(std::size_t index) override
void setPositionUpperLimit(std::size_t index, double position) override
Definition: GenericJoint.hpp:592
void updateInvProjArtInertiaImplicitKinematic(const Eigen::Matrix6d &artInertia, double timeStep)
Definition: GenericJoint.hpp:1946
const math::Jacobian getRelativeJacobian() const override
Definition: GenericJoint.hpp:1562
std::size_t getNumDofs() const override
Definition: GenericJoint.hpp:212
void resetForces() override
Definition: GenericJoint.hpp:1308
void addChildArtInertiaTo(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia) override
Definition: GenericJoint.hpp:1749
Eigen::VectorXd getPositionLowerLimits() const override
Definition: GenericJoint.hpp:585
bool isDofNamePreserved(size_t index) const override
Definition: GenericJoint.hpp:264
void addVelocityTo(Eigen::Vector6d &vel) override
Definition: GenericJoint.hpp:1681
void preserveDofName(size_t index, bool preserve) override
Definition: GenericJoint.hpp:251
double getVelocityUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:899
double getVelocityChange(std::size_t index) const override
Definition: GenericJoint.hpp:1332
void updateTotalForce(const Eigen::Vector6d &bodyForce, double timeStep) override
Definition: GenericJoint.hpp:2103
void resetConstraintImpulses() override
Definition: GenericJoint.hpp:1379
virtual Vector getPositionDifferencesStatic(const Vector &q2, const Vector &q1) const
Fixed-size version of getPositionDifferences()
Definition: GenericJoint.hpp:1425
void setVelocity(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:779
double getRestPosition(std::size_t index) const override
Definition: GenericJoint.hpp:1475
void updateConstrainedTerms(double timeStep) override
Definition: GenericJoint.hpp:2409
void integrateVelocities(double dt) override
Definition: GenericJoint.hpp:1398
Eigen::VectorXd getPositions() const override
Definition: GenericJoint.hpp:537
void addChildBiasImpulseToDynamic(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse)
Definition: GenericJoint.hpp:2070
double getAcceleration(std::size_t index) const override
Definition: GenericJoint.hpp:1023
Eigen::VectorXd getVelocities() const override
Definition: GenericJoint.hpp:830
Eigen::VectorXd getInitialPositions() const override
Definition: GenericJoint.hpp:715
Eigen::VectorXd getPositionDifferences(const Eigen::VectorXd &q2, const Eigen::VectorXd &q1) const override
Definition: GenericJoint.hpp:1406
void setPositionLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:571
double getAccelerationUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1121
void setForceLowerLimit(size_t index, double force) override
Definition: GenericJoint.hpp:1214
void setSpringStiffness(std::size_t index, double k) override
Definition: GenericJoint.hpp:1434
void updateRelativePrimaryAcceleration() const override
Definition: GenericJoint.hpp:1673
void setProperties(const Properties &properties)
Set the Properties of this GenericJoint.
Definition: GenericJoint.hpp:98
double getForceLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1227
static constexpr std::size_t NumDofs
Definition: GenericJoint.hpp:50
const Vector & getVelocitiesStatic() const
Fixed-size version of getVelocities()
Definition: GenericJoint.hpp:753
double getPositionUpperLimit(std::size_t index) const override
Definition: GenericJoint.hpp:606
const std::string & getDofName(size_t index) const override
Definition: GenericJoint.hpp:277
ThisClass & operator=(const ThisClass &other)
Same as copy(const GenericJoint&)
Definition: GenericJoint.hpp:179
double computePotentialEnergy() const override
Definition: GenericJoint.hpp:1545
double getSpringStiffness(std::size_t index) const override
Definition: GenericJoint.hpp:1449
void setDampingCoefficient(std::size_t index, double coeff) override
Definition: GenericJoint.hpp:1488
void updateInvProjArtInertia(const Eigen::Matrix6d &artInertia) override
Definition: GenericJoint.hpp:1853
void resetVelocity(std::size_t index) override
Definition: GenericJoint.hpp:933
void setPositionsStatic(const Vector &positions)
Fixed-size version of setPositions()
Definition: GenericJoint.hpp:722
void addChildBiasForceForInvAugMassMatrix(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce) override
Definition: GenericJoint.hpp:2473
void updateConstrainedTermsKinematic(double timeStep)
Definition: GenericJoint.hpp:2445
void setAccelerationLowerLimits(const Eigen::VectorXd &lowerLimits) override
Definition: GenericJoint.hpp:1087
void setAspectProperties(const AspectProperties &properties)
Set the AspectProperties of this GenericJoint.
Definition: GenericJoint.hpp:125
Eigen::VectorXd getForceUpperLimits() const override
Definition: GenericJoint.hpp:1301
detail::GenericJointProperties< ConfigSpaceT > Properties
Definition: GenericJoint.hpp:62
void updateImpulseFD(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2386
GenericJoint(const ThisClass &)=delete
void updateInvProjArtInertiaImplicitDynamic(const Eigen::Matrix6d &artInertia, double timeStep)
Definition: GenericJoint.hpp:1925
void updateTotalForceKinematic(const Eigen::Vector6d &bodyForce, double timeStep)
Definition: GenericJoint.hpp:2163
void resetTotalImpulses() override
Definition: GenericJoint.hpp:2213
void integratePositions(double dt) override
Definition: GenericJoint.hpp:1386
void setPositions(const Eigen::VectorXd &positions) override
Definition: GenericJoint.hpp:524
void addChildBiasForceToDynamic(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc)
Definition: GenericJoint.hpp:1983
Eigen::VectorXd getForceLowerLimits() const override
Definition: GenericJoint.hpp:1254
double getInitialVelocity(std::size_t index) const override
Definition: GenericJoint.hpp:967
Eigen::VectorXd getAccelerations() const override
Definition: GenericJoint.hpp:1053
typename ConfigSpaceT::Vector Vector
Definition: GenericJoint.hpp:57
void updateVelocityChangeKinematic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &velocityChange)
Definition: GenericJoint.hpp:2309
const Vector & getAccelerationsStatic() const
Fixed-size version of getAccelerations()
Definition: GenericJoint.hpp:772
Eigen::VectorXd getInitialVelocities() const override
Definition: GenericJoint.hpp:994
void setCommands(const Eigen::VectorXd &commands) override
Definition: GenericJoint.hpp:403
Eigen::VectorXd getSpatialToGeneralized(const Eigen::Vector6d &spatial) override
Definition: GenericJoint.hpp:2561
typename ConfigSpaceT::JacobianMatrix JacobianMatrix
Definition: GenericJoint.hpp:58
void getInvAugMassMatrixSegment(Eigen::MatrixXd &invMassMat, const size_t col, const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2528
double getVelocityLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:851
void updateImpulseID(const Eigen::Vector6d &bodyImpulse) override
Definition: GenericJoint.hpp:2378
void setVelocityLowerLimit(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:837
double getInitialPosition(std::size_t index) const override
Definition: GenericJoint.hpp:688
void updateAcceleration(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2220
void addChildBiasForceToKinematic(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc)
Definition: GenericJoint.hpp:2013
void setVelocityUpperLimit(std::size_t index, double velocity) override
Definition: GenericJoint.hpp:885
typename ConfigSpaceT::Point Point
Definition: GenericJoint.hpp:55
void updateInvProjArtInertiaKinematic(const Eigen::Matrix6d &artInertia)
Definition: GenericJoint.hpp:1893
bool hasPositionLimit(std::size_t index) const override
Definition: GenericJoint.hpp:640
void setInitialPositions(const Eigen::VectorXd &initial) override
Definition: GenericJoint.hpp:701
virtual ~GenericJoint()
Destructor.
Definition: GenericJoint.hpp:90
void addChildBiasForceTo(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce, const Eigen::Vector6d &childPartialAcc) override
Definition: GenericJoint.hpp:1954
double getCoulombFriction(std::size_t index) const override
Definition: GenericJoint.hpp:1532
void addInvMassMatrixSegmentTo(Eigen::Vector6d &acc) override
Definition: GenericJoint.hpp:2553
void setConstraintImpulse(std::size_t index, double impulse) override
Definition: GenericJoint.hpp:1352
void resetPosition(std::size_t index) override
Definition: GenericJoint.hpp:654
double getForceUpperLimit(size_t index) const override
Definition: GenericJoint.hpp:1274
void registerDofs() override
Definition: GenericJoint.hpp:1634
void addVelocityChangeTo(Eigen::Vector6d &velocityChange) override
Definition: GenericJoint.hpp:1717
const std::string & setDofName(std::size_t index, const std::string &name, bool preserveName=true) override
Definition: GenericJoint.hpp:219
void setAccelerationUpperLimit(std::size_t index, double acceleration) override
Definition: GenericJoint.hpp:1108
double getDampingCoefficient(std::size_t index) const override
Definition: GenericJoint.hpp:1503
void setPositionUpperLimits(const Eigen::VectorXd &upperLimits) override
Definition: GenericJoint.hpp:619
Eigen::VectorXd getAccelerationLowerLimits() const override
Definition: GenericJoint.hpp:1101
void updateTotalForceDynamic(const Eigen::Vector6d &bodyForce, double timeStep)
Definition: GenericJoint.hpp:2142
void setVelocitiesStatic(const Vector &velocities)
Fixed-size version of setVelocities()
Definition: GenericJoint.hpp:741
void updateRelativeSpatialAcceleration() const override
Definition: GenericJoint.hpp:1664
void updateRelativeSpatialVelocity() const override
Definition: GenericJoint.hpp:1656
void setForceUpperLimit(size_t index, double force) override
Definition: GenericJoint.hpp:1261
size_t getIndexInTree(size_t index) const override
Definition: GenericJoint.hpp:307
Eigen::VectorXd getVelocityUpperLimits() const override
Definition: GenericJoint.hpp:926
void setAccelerationLowerLimit(size_t index, double acceleration) override
Definition: GenericJoint.hpp:1060
double getPosition(std::size_t index) const override
Definition: GenericJoint.hpp:511
void addChildArtInertiaImplicitToDynamic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1825
double getAccelerationLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:1074
void addChildBiasImpulseTo(Eigen::Vector6d &parentBiasImpulse, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasImpulse) override
Definition: GenericJoint.hpp:2042
double getPositionLowerLimit(std::size_t index) const override
Definition: GenericJoint.hpp:558
void updateAccelerationDynamic(const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc)
Definition: GenericJoint.hpp:2244
void resetPositions() override
Definition: GenericJoint.hpp:667
void addChildBiasForceForInvMassMatrix(Eigen::Vector6d &parentBiasForce, const Eigen::Matrix6d &childArtInertia, const Eigen::Vector6d &childBiasForce) override
Definition: GenericJoint.hpp:2453
void getInvMassMatrixSegment(Eigen::MatrixXd &invMassMat, const size_t col, const Eigen::Matrix6d &artInertia, const Eigen::Vector6d &spatialAcc) override
Definition: GenericJoint.hpp:2503
Properties getGenericJointProperties() const
Get the Properties of this GenericJoint.
Definition: GenericJoint.hpp:151
const Matrix & getInvProjArtInertia() const
Get the inverse of the projected articulated inertia.
Definition: GenericJoint.hpp:1730
void setForces(const Eigen::VectorXd &forces) override
Definition: GenericJoint.hpp:1191
void updateTotalForceForInvMassMatrix(const Eigen::Vector6d &bodyForce) override
Definition: GenericJoint.hpp:2493
void resetAccelerations() override
Definition: GenericJoint.hpp:1155
void setAcceleration(std::size_t index, double acceleration) override
Definition: GenericJoint.hpp:1001
void addChildArtInertiaToKinematic(Eigen::Matrix6d &parentArtInertia, const Eigen::Matrix6d &childArtInertia)
Definition: GenericJoint.hpp:1790
double getConstraintImpulse(std::size_t index) const override
Definition: GenericJoint.hpp:1366
size_t getIndexInSkeleton(size_t index) const override
Definition: GenericJoint.hpp:294
void resetVelocityChanges() override
Definition: GenericJoint.hpp:1345
static constexpr ActuatorType PASSIVE
Definition: Joint.hpp:67
static constexpr ActuatorType SERVO
Definition: Joint.hpp:68
static constexpr ActuatorType ACCELERATION
Definition: Joint.hpp:70
void updateArticulatedInertia() const
Tells the Skeleton to update the articulated inertia if it needs updating.
Definition: Joint.cpp:578
static constexpr ActuatorType LOCKED
Definition: Joint.hpp:72
static constexpr ActuatorType FORCE
Definition: Joint.hpp:66
static constexpr ActuatorType MIMIC
Definition: Joint.hpp:69
void setProperties(const Properties &properties)
Set the Properties of this Joint.
Definition: Joint.cpp:109
static constexpr ActuatorType VELOCITY
Definition: Joint.hpp:71
#define GenericJoint_REPORT_UNSUPPORTED_ACTUATOR(func)
Definition: GenericJoint.hpp:61
#define GenericJoint_REPORT_DIM_MISMATCH(func, arg)
Definition: GenericJoint.hpp:44
#define GenericJoint_REPORT_OUT_OF_RANGE(func, index)
Definition: GenericJoint.hpp:53
#define GenericJoint_SET_IF_DIFFERENT(mField, value)
Definition: GenericJoint.hpp:69
Matrix< double, 6, 1 > Vector6d
Definition: MathTypes.hpp:49
Matrix< double, 6, 6 > Matrix6d
Definition: MathTypes.hpp:50
std::shared_ptr< Skeleton > SkeletonPtr
Definition: SmartPointer.hpp:60
SpaceT::Matrix inverse(const typename SpaceT::Matrix &mat)
Definition: ConfigurationSpace.hpp:187
T clip(const T &val, const T &lower, const T &upper)
Definition: Helpers.hpp:159
Eigen::Matrix< double, 6, Eigen::Dynamic > Jacobian
Definition: MathTypes.hpp:114
Eigen::Vector6d AdInvT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_V)
fast version of Ad(Inv(T), V)
Definition: Geometry.cpp:776
Eigen::Vector6d dAdInvT(const Eigen::Isometry3d &_T, const Eigen::Vector6d &_F)
fast version of dAd(Inv(T), F)
Definition: Geometry.cpp:850
Eigen::Vector6d ad(const Eigen::Vector6d &_X, const Eigen::Vector6d &_Y)
adjoint mapping
Definition: Geometry.cpp:808
Inertia transformInertia(const Eigen::Isometry3d &_T, const Inertia &_I)
Definition: Geometry.cpp:1414
bool isNan(double _v)
Returns whether _v is a NaN (Not-A-Number) value.
Definition: Helpers.hpp:187
Definition: BulletCollisionDetector.cpp:65
Definition: GenericJointAspect.hpp:191
Definition: GenericJointAspect.hpp:88
EuclideanPoint mInitialPositions
Initial positions.
Definition: GenericJointAspect.hpp:102
Vector mInitialVelocities
Initial velocities.
Definition: GenericJointAspect.hpp:111
Definition: JointAspect.hpp:112