diff --git a/.travis.yml b/.travis.yml index b35a863cdc781..fad37a42ce49f 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,6 +1,6 @@ language: cpp compiler: - - gcc + #- gcc # Disabled gcc version is 4.6.3 while DART requires 4.7.0 or greater - clang before_install: - 'ci/before_install.sh' diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index debf4ba91775a..3119e12d4a716 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -7,6 +7,7 @@ foreach(APPDIR atlasSimbicon bipedStand hardcodedDesign + hybridDynamics jointConstraints mixedChain operationalSpaceControl diff --git a/apps/hybridDynamics/CMakeLists.txt b/apps/hybridDynamics/CMakeLists.txt new file mode 100644 index 0000000000000..ab4a26afaac8e --- /dev/null +++ b/apps/hybridDynamics/CMakeLists.txt @@ -0,0 +1,7 @@ +############################################### +# apps/hybridDynamics +file(GLOB hybridDynamics_srcs "*.cpp") +file(GLOB hybridDynamics_hdrs "*.h") +add_executable(hybridDynamics ${hybridDynamics_srcs} ${hybridDynamics_hdrs}) +target_link_libraries(hybridDynamics dart) +set_target_properties(hybridDynamics PROPERTIES RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin") diff --git a/apps/hybridDynamics/Main.cpp b/apps/hybridDynamics/Main.cpp new file mode 100644 index 0000000000000..34bd53571eeb8 --- /dev/null +++ b/apps/hybridDynamics/Main.cpp @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include "dart/dart.h" +#include "MyWindow.h" + +int main(int argc, char* argv[]) +{ + // create and initialize the world + dart::simulation::World *myWorld + = dart::utils::SkelParser::readWorld( + DART_DATA_PATH"/skel/fullbody1.skel"); + assert(myWorld != NULL); + Eigen::Vector3d gravity(0.0, -9.81, 0.0); + myWorld->setGravity(gravity); + + dart::dynamics::Skeleton* skel = myWorld->getSkeleton(1); + + std::vector genCoordIds; + genCoordIds.push_back(1); + genCoordIds.push_back(6); // left hip + genCoordIds.push_back(14); // left knee + genCoordIds.push_back(17); // left ankle + genCoordIds.push_back(9); // right hip + genCoordIds.push_back(15); // right knee + genCoordIds.push_back(19); // right ankle + genCoordIds.push_back(13); // lower back + Eigen::VectorXd initConfig(8); + initConfig << -0.2, 0.15, -0.4, 0.25, 0.15, -0.4, 0.25, 0.0; + skel->setPositionSegment(genCoordIds, initConfig); + skel->computeForwardKinematics(true, true, false); + + dart::dynamics::Joint* joint0 = skel->getJoint(0); + joint0->setActuatorType(dart::dynamics::Joint::PASSIVE); + for (size_t i = 1; i < skel->getNumBodyNodes(); ++i) + { + dart::dynamics::Joint* joint = skel->getJoint(i); + joint->setActuatorType(dart::dynamics::Joint::VELOCITY); + } + + // create a window and link it to the world + MyWindow window; + window.setWorld(myWorld); + + std::cout << "space bar: simulation on/off" << std::endl; + std::cout << "'p': playback/stop" << std::endl; + std::cout << "'[' and ']': play one frame backward and forward" << std::endl; + std::cout << "'v': visualization on/off" << std::endl; + std::cout << "'1'--'4': programmed interaction" << std::endl; + std::cout << "'h': harness on/off" << std::endl; + + glutInit(&argc, argv); + window.initWindow(640, 480, "Hybrid Dynamics"); + glutMainLoop(); + + return 0; +} diff --git a/apps/hybridDynamics/MyWindow.cpp b/apps/hybridDynamics/MyWindow.cpp new file mode 100644 index 0000000000000..79a283233dcc9 --- /dev/null +++ b/apps/hybridDynamics/MyWindow.cpp @@ -0,0 +1,147 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "MyWindow.h" + +//============================================================================== +MyWindow::MyWindow() + : SimWindow(), + mHarnessOn(false) +{ +} + +//============================================================================== +MyWindow::~MyWindow() +{ +} + +//============================================================================== +void MyWindow::timeStepping() +{ + dart::dynamics::Skeleton* skel = mWorld->getSkeleton(1); + + size_t index0 = skel->getJoint("j_scapula_left")->getIndexInSkeleton(0); + size_t index1 = skel->getJoint("j_scapula_right")->getIndexInSkeleton(0); + size_t index2 = skel->getJoint("j_forearm_left")->getIndexInSkeleton(0); + size_t index3 = skel->getJoint("j_forearm_right")->getIndexInSkeleton(0); + + size_t index6 = skel->getJoint("j_shin_left")->getIndexInSkeleton(0); + size_t index7 = skel->getJoint("j_shin_right")->getIndexInSkeleton(0); + + skel->setCommand(index0, 1.0 * std::sin(mWorld->getTime() * 4.0)); + skel->setCommand(index1, -1.0 * std::sin(mWorld->getTime() * 4.0)); + skel->setCommand(index2, 0.8 * std::sin(mWorld->getTime() * 4.0)); + skel->setCommand(index3, 0.8 * std::sin(mWorld->getTime() * 4.0)); + + skel->setCommand(index6, 0.1 * std::sin(mWorld->getTime() * 2.0)); + skel->setCommand(index7, 0.1 * std::sin(mWorld->getTime() * 2.0)); + + mWorld->step(); +} + +//============================================================================== +void MyWindow::drawSkels() +{ + glEnable(GL_LIGHTING); + glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); + + SimWindow::drawSkels(); +} + +//============================================================================== +void MyWindow::keyboard(unsigned char _key, int _x, int _y) +{ + switch (_key) + { + case ' ': // use space key to play or stop the motion + mSimulating = !mSimulating; + if (mSimulating) + { + mPlay = false; + glutTimerFunc(mDisplayTimeout, refreshTimer, 0); + } + break; + case 'p': // playBack + mPlay = !mPlay; + if (mPlay) + { + mSimulating = false; + glutTimerFunc(mDisplayTimeout, refreshTimer, 0); + } + break; + case '[': // step backward + if (!mSimulating) + { + mPlayFrame--; + if (mPlayFrame < 0) + mPlayFrame = 0; + glutPostRedisplay(); + } + break; + case ']': // step forwardward + if (!mSimulating) + { + mPlayFrame++; + if (mPlayFrame >= mWorld->getRecording()->getNumFrames()) + mPlayFrame = 0; + glutPostRedisplay(); + } + break; + case 'v': // show or hide markers + mShowMarkers = !mShowMarkers; + break; + case 'h': + mHarnessOn = !mHarnessOn; + if (mHarnessOn) + { + dart::dynamics::Joint* joint + = mWorld->getSkeleton(1)->getBodyNode("h_pelvis")->getParentJoint(); + joint->setActuatorType(dart::dynamics::Joint::LOCKED); + std::cout << "The pelvis is locked." << std::endl; + } + else + { + dart::dynamics::Joint* joint + = mWorld->getSkeleton(1)->getBodyNode("h_pelvis")->getParentJoint(); + joint->setActuatorType(dart::dynamics::Joint::PASSIVE); + std::cout << "The pelvis is unlocked." << std::endl; + } + break; + default: + Win3D::keyboard(_key, _x, _y); + } + glutPostRedisplay(); +} diff --git a/apps/hybridDynamics/MyWindow.h b/apps/hybridDynamics/MyWindow.h new file mode 100644 index 0000000000000..ab87909be37e7 --- /dev/null +++ b/apps/hybridDynamics/MyWindow.h @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2014, Georgia Tech Research Corporation + * All rights reserved. + * + * Author(s): Jeongseok Lee + * + * Georgia Tech Graphics Lab and Humanoid Robotics Lab + * + * Directed by Prof. C. Karen Liu and Prof. Mike Stilman + * + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef APPS_HYBRIDDYNAMICS_MYWINDOW_H_ +#define APPS_HYBRIDDYNAMICS_MYWINDOW_H_ + +#include "dart/dart.h" + +/// \brief +class MyWindow : public dart::gui::SimWindow +{ +public: + /// \brief + MyWindow(); + + /// \brief + virtual ~MyWindow(); + + /// \brief + virtual void timeStepping(); + + /// \brief + virtual void drawSkels(); + + /// \brief + virtual void keyboard(unsigned char _key, int _x, int _y); + +private: + bool mHarnessOn; +}; + +#endif // APPS_HYBRIDDYNAMICS_MYWINDOW_H_ diff --git a/apps/jointConstraints/MyWindow.cpp b/apps/jointConstraints/MyWindow.cpp index 8b155d99e306b..fff10381130a7 100644 --- a/apps/jointConstraints/MyWindow.cpp +++ b/apps/jointConstraints/MyWindow.cpp @@ -138,17 +138,18 @@ void MyWindow::keyboard(unsigned char key, int x, int y) break; case 'h': + mHarnessOn = !mHarnessOn; if (mHarnessOn) { - mWorld->getConstraintSolver()->removeConstraint(mWeldJoint); - mHarnessOn = false; + BodyNode* bd = mWorld->getSkeleton(1)->getBodyNode("h_pelvis"); + mWeldJoint = new WeldJointConstraint(bd); + mWorld->getConstraintSolver()->addConstraint(mWeldJoint); } else { - addWeldConstraint(); + mWorld->getConstraintSolver()->removeConstraint(mWeldJoint); } break; - default: Win3D::keyboard(key,x,y); @@ -156,11 +157,3 @@ void MyWindow::keyboard(unsigned char key, int x, int y) glutPostRedisplay(); } -void MyWindow::addWeldConstraint() -{ - BodyNode* bd = mWorld->getSkeleton(1)->getBodyNode("h_pelvis"); - mWeldJoint = new WeldJointConstraint(bd); - mWorld->getConstraintSolver()->addConstraint(mWeldJoint); - - mHarnessOn = true; -} diff --git a/apps/jointConstraints/MyWindow.h b/apps/jointConstraints/MyWindow.h index 0fbcc14574c49..5e10412df699d 100644 --- a/apps/jointConstraints/MyWindow.h +++ b/apps/jointConstraints/MyWindow.h @@ -73,7 +73,6 @@ class MyWindow : public dart::gui::SimWindow Controller* mController; dart::constraint::WeldJointConstraint* mWeldJoint; int mImpulseDuration; - void addWeldConstraint(); bool mHarnessOn; }; diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index ab8a6f28d320c..1ffcaf3c71553 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -304,14 +304,9 @@ bool ConstraintSolver::checkAndAddSkeleton(Skeleton* _skeleton) //============================================================================== bool ConstraintSolver::containConstraint(const Constraint* _constraint) const { - if (std::find(mManualConstraints.begin(), mManualConstraints.end(), - _constraint) - != mManualConstraints.end()) - { - return true; - } - - return false; + return std::find(mManualConstraints.begin(), + mManualConstraints.end(), + _constraint) != mManualConstraints.end(); } //============================================================================== @@ -338,13 +333,12 @@ void ConstraintSolver::updateConstraints() //---------------------------------------------------------------------------- // Update manual constraints //---------------------------------------------------------------------------- - for (std::vector::iterator it = mManualConstraints.begin(); - it != mManualConstraints.end(); ++it) + for (auto& manualConstraint : mManualConstraints) { - (*it)->update(); + manualConstraint->update(); - if ((*it)->isActive()) - mActiveConstraints.push_back(*it); + if (manualConstraint->isActive()) + mActiveConstraints.push_back(manualConstraint); } //---------------------------------------------------------------------------- @@ -354,21 +348,13 @@ void ConstraintSolver::updateConstraints() mCollisionDetector->detectCollision(true, true); // Destroy previous contact constraints - for (std::vector::const_iterator it - = mContactConstraints.begin(); - it != mContactConstraints.end(); ++it) - { - delete *it; - } + for (const auto& contactConstraint : mContactConstraints) + delete contactConstraint; mContactConstraints.clear(); // Destroy previous soft contact constraints - for (std::vector::const_iterator it - = mSoftContactConstraints.begin(); - it != mSoftContactConstraints.end(); ++it) - { - delete *it; - } + for (const auto& softContactConstraint : mSoftContactConstraints) + delete softContactConstraint; mSoftContactConstraints.clear(); // Create new contact constraints @@ -388,60 +374,51 @@ void ConstraintSolver::updateConstraints() } // Add the new contact constraints to dynamic constraint list - for (std::vector::const_iterator it - = mContactConstraints.begin(); - it != mContactConstraints.end(); ++it) + for (const auto& contactConstraint : mContactConstraints) { - (*it)->update(); + contactConstraint->update(); - if ((*it)->isActive()) - mActiveConstraints.push_back(*it); + if (contactConstraint->isActive()) + mActiveConstraints.push_back(contactConstraint); } // Add the new soft contact constraints to dynamic constraint list - for (std::vector::const_iterator it - = mSoftContactConstraints.begin(); - it != mSoftContactConstraints.end(); ++it) + for (const auto& softContactConstraint : mSoftContactConstraints) { - (*it)->update(); + softContactConstraint->update(); - if ((*it)->isActive()) - mActiveConstraints.push_back(*it); + if (softContactConstraint->isActive()) + mActiveConstraints.push_back(softContactConstraint); } //---------------------------------------------------------------------------- // Update automatic constraints: joint limit constraints //---------------------------------------------------------------------------- // Destroy previous joint limit constraints - for (std::vector::const_iterator it - = mJointLimitConstraints.begin(); - it != mJointLimitConstraints.end(); ++it) - { - delete *it; - } + for (const auto& jointLimitConstraint : mJointLimitConstraints) + delete jointLimitConstraint; mJointLimitConstraints.clear(); // Create new joint limit constraints - for (std::vector::iterator it = mSkeletons.begin(); - it != mSkeletons.end(); ++it) + for (const auto& skel : mSkeletons) { - for (size_t i = 0; i < (*it)->getNumBodyNodes(); i++) + const size_t numBodyNodes = skel->getNumBodyNodes(); + for (size_t i = 0; i < numBodyNodes; i++) { - dynamics::Joint* joint = (*it)->getBodyNode(i)->getParentJoint(); - if (joint->isPositionLimited()) + dynamics::Joint* joint = skel->getBodyNode(i)->getParentJoint(); + + if (joint->isDynamic() && joint->isPositionLimited()) mJointLimitConstraints.push_back(new JointLimitConstraint(joint)); } } // Add active joint limit - for (std::vector::const_iterator it - = mJointLimitConstraints.begin(); - it != mJointLimitConstraints.end(); ++it) + for (auto& jointLimitConstraint : mJointLimitConstraints) { - (*it)->update(); + jointLimitConstraint->update(); - if ((*it)->isActive()) - mActiveConstraints.push_back(*it); + if (jointLimitConstraint->isActive()) + mActiveConstraints.push_back(jointLimitConstraint); } } diff --git a/dart/constraint/ContactConstraint.cpp b/dart/constraint/ContactConstraint.cpp index c66f219c1ecb1..4d0f97829baac 100644 --- a/dart/constraint/ContactConstraint.cpp +++ b/dart/constraint/ContactConstraint.cpp @@ -524,9 +524,7 @@ void ContactConstraint::getInformation(ConstraintInfo* _info) bouncingVelocity = restitutionVel; if (bouncingVelocity > DART_MAX_BOUNCING_VELOCITY) - { bouncingVelocity = DART_MAX_BOUNCING_VELOCITY; - } } } } @@ -553,25 +551,26 @@ void ContactConstraint::applyUnitImpulse(size_t _idx) assert(isActive()); assert(mBodyNode1->isReactive() || mBodyNode2->isReactive()); + dynamics::Skeleton* skel1 = mBodyNode1->getSkeleton(); + dynamics::Skeleton* skel2 = mBodyNode2->getSkeleton(); + // Self collision case - if (mBodyNode1->getSkeleton() == mBodyNode2->getSkeleton()) + if (skel1 == skel2) { - mBodyNode1->getSkeleton()->clearConstraintImpulses(); + skel1->clearConstraintImpulses(); if (mBodyNode1->isReactive()) { // Both bodies are reactive if (mBodyNode2->isReactive()) { - mBodyNode1->getSkeleton()->updateBiasImpulse( - mBodyNode1, mJacobians1[_idx], - mBodyNode2, mJacobians2[_idx]); + skel1->updateBiasImpulse(mBodyNode1, mJacobians1[_idx], + mBodyNode2, mJacobians2[_idx]); } // Only body1 is reactive else { - mBodyNode1->getSkeleton()->updateBiasImpulse(mBodyNode1, - mJacobians1[_idx]); + skel1->updateBiasImpulse(mBodyNode1, mJacobians1[_idx]); } } else @@ -579,8 +578,7 @@ void ContactConstraint::applyUnitImpulse(size_t _idx) // Only body2 is reactive if (mBodyNode2->isReactive()) { - mBodyNode2->getSkeleton()->updateBiasImpulse(mBodyNode2, - mJacobians2[_idx]); + skel2->updateBiasImpulse(mBodyNode2, mJacobians2[_idx]); } // Both bodies are not reactive else @@ -590,25 +588,23 @@ void ContactConstraint::applyUnitImpulse(size_t _idx) } } - mBodyNode1->getSkeleton()->updateVelocityChange(); + skel1->updateVelocityChange(); } // Colliding two distinct skeletons else { if (mBodyNode1->isReactive()) { - mBodyNode1->getSkeleton()->clearConstraintImpulses(); - mBodyNode1->getSkeleton()->updateBiasImpulse(mBodyNode1, - mJacobians1[_idx]); - mBodyNode1->getSkeleton()->updateVelocityChange(); + skel1->clearConstraintImpulses(); + skel1->updateBiasImpulse(mBodyNode1, mJacobians1[_idx]); + skel1->updateVelocityChange(); } if (mBodyNode2->isReactive()) { - mBodyNode2->getSkeleton()->clearConstraintImpulses(); - mBodyNode2->getSkeleton()->updateBiasImpulse(mBodyNode2, - mJacobians2[_idx]); - mBodyNode2->getSkeleton()->updateVelocityChange(); + skel2->clearConstraintImpulses(); + skel2->updateBiasImpulse(mBodyNode2, mJacobians2[_idx]); + skel2->updateVelocityChange(); } } @@ -637,8 +633,8 @@ void ContactConstraint::getVelocityChange(double* _vel, bool _withCfm) } } - // Add small values to diagnal to keep it away from singular, similar to cfm - // varaible in ODE + // Add small values to the diagnal to keep it away from singular, similar to + // cfm variable in ODE if (_withCfm) { _vel[mAppliedImpulseIndex] += _vel[mAppliedImpulseIndex] diff --git a/dart/constraint/ContactConstraint.h b/dart/constraint/ContactConstraint.h index 0922df0d9d895..d4462c28a7fa4 100644 --- a/dart/constraint/ContactConstraint.h +++ b/dart/constraint/ContactConstraint.h @@ -60,8 +60,8 @@ class ContactConstraint : public Constraint /// /// Do not use me anymore. This constructor is not possible to store contact /// force in _contact - DEPRECATED(4.2) explicit ContactConstraint( - const collision::Contact& _contact); + DEPRECATED(4.2) + explicit ContactConstraint(const collision::Contact& _contact); /// Constructor ContactConstraint(collision::Contact& _contact, double _timeStep); diff --git a/dart/constraint/JointLimitConstraint.h b/dart/constraint/JointLimitConstraint.h index 837aae17cb443..1f2847d20725d 100644 --- a/dart/constraint/JointLimitConstraint.h +++ b/dart/constraint/JointLimitConstraint.h @@ -49,6 +49,7 @@ class Joint; namespace constraint { /// JointLimitConstraint handles joint position or velocity limits +// TOOD: better naming class JointLimitConstraint : public Constraint { public: diff --git a/dart/constraint/SoftContactConstraint.cpp b/dart/constraint/SoftContactConstraint.cpp index ae8479ef5d2c1..007c6af623b26 100644 --- a/dart/constraint/SoftContactConstraint.cpp +++ b/dart/constraint/SoftContactConstraint.cpp @@ -605,8 +605,8 @@ void SoftContactConstraint::applyUnitImpulse(size_t _idx) if (mPointMass1) { - mBodyNode1->getSkeleton()->updateBiasImpulse(mSoftBodyNode1, mPointMass1, - mJacobians1[_idx].tail<3>()); + mBodyNode1->getSkeleton()->updateBiasImpulse( + mSoftBodyNode1, mPointMass1, mJacobians1[_idx].tail<3>()); } else { @@ -619,15 +619,15 @@ void SoftContactConstraint::applyUnitImpulse(size_t _idx) if (mPointMass2) { - mBodyNode2->getSkeleton()->updateBiasImpulse(mSoftBodyNode2, mPointMass2, - mJacobians2[_idx].tail<3>()); + mBodyNode2->getSkeleton()->updateBiasImpulse( + mSoftBodyNode2, mPointMass2, mJacobians2[_idx].tail<3>()); } else { if (mBodyNode2->isReactive()) { mBodyNode2->getSkeleton()->updateBiasImpulse(mBodyNode2, - mJacobians2[_idx]); + mJacobians2[_idx]); } } @@ -639,8 +639,8 @@ void SoftContactConstraint::applyUnitImpulse(size_t _idx) if (mPointMass1) { mBodyNode1->getSkeleton()->clearConstraintImpulses(); - mBodyNode1->getSkeleton()->updateBiasImpulse(mSoftBodyNode1, mPointMass1, - mJacobians1[_idx].tail<3>()); + mBodyNode1->getSkeleton()->updateBiasImpulse( + mSoftBodyNode1, mPointMass1, mJacobians1[_idx].tail<3>()); mBodyNode1->getSkeleton()->updateVelocityChange(); } else @@ -657,8 +657,8 @@ void SoftContactConstraint::applyUnitImpulse(size_t _idx) if (mPointMass2) { mBodyNode2->getSkeleton()->clearConstraintImpulses(); - mBodyNode2->getSkeleton()->updateBiasImpulse(mSoftBodyNode2, mPointMass2, - mJacobians2[_idx].tail<3>()); + mBodyNode2->getSkeleton()->updateBiasImpulse( + mSoftBodyNode2, mPointMass2, mJacobians2[_idx].tail<3>()); mBodyNode2->getSkeleton()->updateVelocityChange(); } else diff --git a/dart/constraint/SoftContactConstraint.h b/dart/constraint/SoftContactConstraint.h index b19b2efbe3418..261bf3c4a4166 100644 --- a/dart/constraint/SoftContactConstraint.h +++ b/dart/constraint/SoftContactConstraint.h @@ -63,8 +63,8 @@ class SoftContactConstraint : public Constraint { public: /// Constructor - DEPRECATED(4.2) explicit SoftContactConstraint( - const collision::Contact& _contact); + DEPRECATED(4.2) + explicit SoftContactConstraint(const collision::Contact& _contact); /// Constructor SoftContactConstraint(collision::Contact& _contact, double _timeStep); diff --git a/dart/dynamics/BallJoint.h b/dart/dynamics/BallJoint.h index 8ba7e6813407c..831fbae38e01c 100644 --- a/dart/dynamics/BallJoint.h +++ b/dart/dynamics/BallJoint.h @@ -54,12 +54,6 @@ class BallJoint : public MultiDofJoint<3> /// Destructor virtual ~BallJoint(); - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForces; - } - protected: // Documentation inherited virtual void integratePositions(double _dt); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index aa7025d7d2c76..c0987313430b3 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -987,6 +987,12 @@ void BodyNode::updatePartialAcceleration() //============================================================================== void BodyNode::updateAcceleration() +{ + updateAccelerationID(); +} + +//============================================================================== +void BodyNode::updateAccelerationID() { // Transmit acceleration of parent body to this body if (mParentBodyNode) @@ -1009,6 +1015,13 @@ void BodyNode::updateAcceleration() //============================================================================== void BodyNode::updateBodyWrench(const Eigen::Vector3d& _gravity, bool _withExternalForces) +{ + updateTransmittedForceID(_gravity, _withExternalForces); +} + +//============================================================================== +void BodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, + bool _withExternalForces) { // Gravity force if (mGravityMode == true) @@ -1033,19 +1046,15 @@ void BodyNode::updateBodyWrench(const Eigen::Vector3d& _gravity, mF -= math::dad(mV, mI * mV); // - for (std::vector::iterator iChildBody = mChildBodyNodes.begin(); - iChildBody != mChildBodyNodes.end(); ++iChildBody) + for (const auto& childBodyNode : mChildBodyNodes) { - Joint* childJoint = (*iChildBody)->getParentJoint(); + Joint* childJoint = childBodyNode->getParentJoint(); assert(childJoint != NULL); mF += math::dAdInvT(childJoint->getLocalTransform(), - (*iChildBody)->getBodyForce()); + childBodyNode->getBodyForce()); } - // TODO(JS): mWrench and mF are duplicated. Remove one of them. - mParentJoint->mWrench = mF; - // Verification assert(!math::isNan(mF)); } @@ -1053,21 +1062,7 @@ void BodyNode::updateBodyWrench(const Eigen::Vector3d& _gravity, //============================================================================== void BodyNode::updateGeneralizedForce(bool _withDampingForces) { - assert(mParentJoint != NULL); - - size_t dof = mParentJoint->getNumDofs(); - - if (dof > 0) - { - const math::Jacobian& J = mParentJoint->getLocalJacobian(); - - // if (_withDampingForces) - // mF -= mFDamp; - - assert(!math::isNan(J.transpose()*mF)); - - mParentJoint->setForces(J.transpose() * mF); - } + updateJointForceID(0.001, _withDampingForces, false); } //============================================================================== @@ -1078,17 +1073,17 @@ void BodyNode::updateArtInertia(double _timeStep) mArtInertiaImplicit = mI; // and add child articulated body inertia - for (std::vector::const_iterator it = mChildBodyNodes.begin(); - it != mChildBodyNodes.end(); ++it) + for (const auto& child : mChildBodyNodes) { - (*it)->getParentJoint()->addChildArtInertiaTo( - mArtInertia, (*it)->mArtInertia); - (*it)->getParentJoint()->addChildArtInertiaImplicitTo( - mArtInertiaImplicit, (*it)->mArtInertiaImplicit); + Joint* childJoint = child->getParentJoint(); + + childJoint->addChildArtInertiaTo(mArtInertia, child->mArtInertia); + childJoint->addChildArtInertiaImplicitTo(mArtInertiaImplicit, + child->mArtInertiaImplicit); } // Verification - assert(!math::isNan(mArtInertia)); +// assert(!math::isNan(mArtInertia)); assert(!math::isNan(mArtInertiaImplicit)); // Update parent joint's inverse of projected articulated body inertia @@ -1096,7 +1091,7 @@ void BodyNode::updateArtInertia(double _timeStep) mParentJoint->updateInvProjArtInertiaImplicit(mArtInertiaImplicit, _timeStep); // Verification - assert(!math::isNan(mArtInertia)); +// assert(!math::isNan(mArtInertia)); assert(!math::isNan(mArtInertiaImplicit)); } @@ -1117,13 +1112,14 @@ void BodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, assert(!math::isNan(mBiasForce)); // And add child bias force - for (std::vector::const_iterator it = mChildBodyNodes.begin(); - it != mChildBodyNodes.end(); ++it) + for (const auto& childBodyNode : mChildBodyNodes) { - (*it)->getParentJoint()->addChildBiasForceTo(mBiasForce, - (*it)->mArtInertiaImplicit, - (*it)->mBiasForce, - (*it)->mPartialAcceleration); + Joint* childJoint = childBodyNode->getParentJoint(); + + childJoint->addChildBiasForceTo(mBiasForce, + childBodyNode->mArtInertiaImplicit, + childBodyNode->mBiasForce, + childBodyNode->mPartialAcceleration); } // Verification @@ -1135,12 +1131,77 @@ void BodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, mArtInertiaImplicit * mPartialAcceleration + mBiasForce, _timeStep); } +//============================================================================== +void BodyNode::updateBiasImpulse() +{ + // Update impulsive bias force + mBiasImpulse = -mConstraintImpulse; + + // And add child bias impulse + for (auto& childBodyNode : mChildBodyNodes) + { + Joint* childJoint = childBodyNode->getParentJoint(); + + childJoint->addChildBiasImpulseTo(mBiasImpulse, + childBodyNode->mArtInertia, + childBodyNode->mBiasImpulse); + } + + // Verification + assert(!math::isNan(mBiasImpulse)); + + // Update parent joint's total force + mParentJoint->updateTotalImpulse(mBiasImpulse); +} + //============================================================================== void BodyNode::updateJointAndBodyAcceleration() +{ + updateAccelerationFD(); +} + +//============================================================================== +void BodyNode::updateJointVelocityChange() +{ + updateVelocityChangeFD(); +} + +//============================================================================== +void BodyNode::updateTransmittedWrench() +{ + updateTransmittedForceFD(); +} + +//============================================================================== +void BodyNode::updateTransmittedForceFD() +{ + mF = mBiasForce; + mF.noalias() += mArtInertiaImplicit * mA; + + assert(!math::isNan(mF)); +} + +//============================================================================== +void BodyNode::updateBodyImpForceFwdDyn() +{ + updateTransmittedImpulse(); +} + +//============================================================================== +void BodyNode::updateTransmittedImpulse() +{ + mImpF = mBiasImpulse; + mImpF.noalias() += mArtInertia * mDelV; + + assert(!math::isNan(mImpF)); +} + +//============================================================================== +void BodyNode::updateAccelerationFD() { if (mParentBodyNode) { - // + // Update joint acceleration mParentJoint->updateAcceleration(mArtInertiaImplicit, mParentBodyNode->mA); // Transmit spatial acceleration of parent body to this body @@ -1149,7 +1210,7 @@ void BodyNode::updateJointAndBodyAcceleration() } else { - // + // Update joint acceleration mParentJoint->updateAcceleration(mArtInertiaImplicit, Eigen::Vector6d::Zero()); @@ -1165,15 +1226,72 @@ void BodyNode::updateJointAndBodyAcceleration() } //============================================================================== -void BodyNode::updateTransmittedWrench() +void BodyNode::updateVelocityChangeFD() { - mF = mBiasForce; - mF.noalias() += mArtInertiaImplicit * mA; + if (mParentBodyNode) + { + // Update joint velocity change + mParentJoint->updateVelocityChange(mArtInertia, mParentBodyNode->mDelV); - // TODO(JS): mWrench and mF are duplicated. Remove one of them. - mParentJoint->mWrench = mF; + // Transmit spatial acceleration of parent body to this body + mDelV = math::AdInvT(mParentJoint->mT, mParentBodyNode->mDelV); + } + else + { + // Update joint velocity change + mParentJoint->updateVelocityChange(mArtInertia, Eigen::Vector6d::Zero()); - assert(!math::isNan(mF)); + // Transmit spatial acceleration of parent body to this body + mDelV.setZero(); + } + + // Add parent joint's acceleration to this body + mParentJoint->addVelocityChangeTo(mDelV); + + // Verify the spatial velocity change of this body + assert(!math::isNan(mDelV)); +} + +//============================================================================== +void BodyNode::updateJointForceID(double _timeStep, + double _withDampingForces, + double _withSpringForces) +{ + assert(mParentJoint != NULL); + mParentJoint->updateForceID(mF, _timeStep, + _withDampingForces, _withSpringForces); +} + +//============================================================================== +void BodyNode::updateJointForceFD(double _timeStep, + double _withDampingForces, + double _withSpringForces) +{ + assert(mParentJoint != NULL); + mParentJoint->updateForceFD(mF, _timeStep, + _withDampingForces, _withSpringForces); +} + +//============================================================================== +void BodyNode::updateJointImpulseFD() +{ + assert(mParentJoint != NULL); + mParentJoint->updateImpulseFD(mF); +} + +//============================================================================== +void BodyNode::updateConstrainedTerms(double _timeStep) +{ + // 1. dq = dq + del_dq + // 2. ddq = ddq + del_dq / dt + // 3. tau = tau + imp / dt + mParentJoint->updateConstrainedTerms(_timeStep); + + // + mA += mDelV / _timeStep; + + // + mF += mImpF / _timeStep; } //============================================================================== @@ -1286,100 +1404,40 @@ bool BodyNode::isImpulseReponsible() const //============================================================================== bool BodyNode::isReactive() const { - // TODO(JS): Once hybrid dynamics is implemented, we should consider joint - // type of parent joint. if (mSkeleton->isMobile() && getNumDependentGenCoords() > 0) - return true; - else - return false; -} - -//============================================================================== -void BodyNode::updateBiasImpulse() -{ - // Update impulsive bias force - mBiasImpulse = -mConstraintImpulse; -// assert(mImpFext == Eigen::Vector6d::Zero()); - - // And add child bias impulse - for (std::vector::const_iterator it = mChildBodyNodes.begin(); - it != mChildBodyNodes.end(); ++it) { - (*it)->getParentJoint()->addChildBiasImpulseTo(mBiasImpulse, - (*it)->mArtInertia, - (*it)->mBiasImpulse); - } - - // Verification - assert(!math::isNan(mBiasImpulse)); - - // Update parent joint's total force - mParentJoint->updateTotalImpulse(mBiasImpulse); -} + // Check if all the ancestor joints are motion prescribed. + const BodyNode* body = this; + while (body != NULL) + { + if (body->mParentJoint->isDynamic()) + return true; -//============================================================================== -void BodyNode::updateJointVelocityChange() -{ - if (mParentBodyNode) - { - // - mParentJoint->updateVelocityChange(mArtInertia, mParentBodyNode->mDelV); + body = body->mParentBodyNode; + } + // TODO: Checking if all the ancestor joints are motion prescribed is + // expensive. It would be good to evaluate this in advance and update only + // when necessary. - // Transmit spatial acceleration of parent body to this body - mDelV = math::AdInvT(mParentJoint->mT, mParentBodyNode->mDelV); + return false; } else { - // - mParentJoint->updateVelocityChange(mArtInertia, Eigen::Vector6d::Zero()); - - // Transmit spatial acceleration of parent body to this body - mDelV.setZero(); + return false; } - - // Add parent joint's acceleration to this body - mParentJoint->addVelocityChangeTo(mDelV); - - // Verify the spatial velocity change of this body - assert(!math::isNan(mDelV)); -} - -//============================================================================== -//void BodyNode::updateBodyVelocityChange() -//{ -// if (mParentJoint->getNumDofs() > 0) -// mDelV = mParentJoint->getLocalJacobian() * mParentJoint->getVelsChange(); -// else -// mDelV.setZero(); - -// if (mParentBodyNode) -// { -// mDelV += math::AdInvT(mParentJoint->getLocalTransform(), -// mParentBodyNode->mDelV); -// } - -// assert(!math::isNan(mDelV)); -//} - -//============================================================================== -void BodyNode::updateBodyImpForceFwdDyn() -{ - mImpF = mBiasImpulse; - mImpF.noalias() += mArtInertia * mDelV; - assert(!math::isNan(mImpF)); } //============================================================================== -void BodyNode::updateConstrainedJointAndBodyAcceleration(double _timeStep) +void BodyNode::updateConstrainedJointAndBodyAcceleration(double /*_timeStep*/) { // 1. dq = dq + del_dq - mParentJoint->updateVelocityWithVelocityChange(); + // mParentJoint->updateVelocityWithVelocityChange(); // 2. ddq = ddq + del_dq / dt - mParentJoint->updateAccelerationWithVelocityChange(_timeStep); + // mParentJoint->updateAccelerationWithVelocityChange(_timeStep); // 3. tau = tau + imp / dt - mParentJoint->updateForceWithImpulse(_timeStep); + // mParentJoint->updateForceWithImpulse(_timeStep); } //============================================================================== @@ -1717,9 +1775,9 @@ void BodyNode::_updateBodyJacobian() // Parent Jacobian if (mParentBodyNode) { - assert(mParentBodyNode->getBodyJacobian().cols() - + math::castUIntToInt(mParentJoint->getNumDofs()) - == mBodyJacobian.cols()); + assert(static_cast(mParentBodyNode->getBodyJacobian().cols()) + + mParentJoint->getNumDofs() + == static_cast(mBodyJacobian.cols())); assert(mParentJoint); mBodyJacobian.leftCols(ascendantDof) = @@ -1755,9 +1813,9 @@ void BodyNode::_updateBodyJacobianDeriv() // Parent Jacobian if (mParentBodyNode) { - assert(mParentBodyNode->mBodyJacobianDeriv.cols() - + math::castUIntToInt(mParentJoint->getNumDofs()) - == mBodyJacobianDeriv.cols()); + assert(static_cast(mParentBodyNode->mBodyJacobianDeriv.cols()) + + mParentJoint->getNumDofs() + == static_cast(mBodyJacobianDeriv.cols())); assert(mParentJoint); mBodyJacobianDeriv.leftCols(numParentDOFs) diff --git a/dart/dynamics/BodyNode.h b/dart/dynamics/BodyNode.h index a7ddb34a0fa01..d617695c989fa 100644 --- a/dart/dynamics/BodyNode.h +++ b/dart/dynamics/BodyNode.h @@ -412,7 +412,11 @@ class BodyNode /// Eigen::Vector6d getExternalForceGlobal() const; + /// Get spatial body force transmitted from the parent joint. /// + /// The spatial body force is transmitted to this BodyNode from the parent + /// body through the connecting joint. It is expressed in this BodyNode's + /// frame. const Eigen::Vector6d& getBodyForce() const; //---------------------------------------------------------------------------- @@ -421,13 +425,14 @@ class BodyNode //---------------------------------------------------------------------------- /// Deprecated in 4.2. Please use isReactive(). - DEPRECATED(4.2) bool isImpulseReponsible() const; + DEPRECATED(4.2) + bool isImpulseReponsible() const; - /// Return true if the body can react on force or constraint impulse. + /// Return true if the body can react to force or constraint impulse. /// - /// A body node can be called reactive if the parent skeleton is mobile and - /// the number of dependent generalized coordinates is non zero. The body - /// should be initialized first by calling BodyNode::init(). + /// A body node is reactive if the skeleton is mobile and the number of + /// dependent generalized coordinates is non zero. BodyNode::init() should be + /// called first to update the number of dependent generalized coordinates. bool isReactive() const; /// Set constraint impulse @@ -494,70 +499,147 @@ class BodyNode /// Initialize the vector members with proper sizes. virtual void init(Skeleton* _skeleton); - //-------------------------------------------------------------------------- - // Recursive algorithms - //-------------------------------------------------------------------------- + //---------------------------------------------------------------------------- + /// \{ \name Recursive dynamics routines + //---------------------------------------------------------------------------- /// Update transformation virtual void updateTransform(); - /// Update spatial velocity + /// Update spatial body velocity. virtual void updateVelocity(); - /// Update partial spatial acceleration due to parent joint's velocity + /// Update partial spatial body acceleration due to parent joint's velocity. virtual void updatePartialAcceleration(); - /// Update spatial acceleration with the partial spatial acceleration - virtual void updateAcceleration(); - - // TODO(JS): Need revise - /// - virtual void updateBodyWrench(const Eigen::Vector3d& _gravity, - bool _withExternalForces = false); - - // TODO(JS): Need revise - /// - virtual void updateGeneralizedForce(bool _withDampingForces = false); - - /// Update articulated body inertia with implicit joint damping and spring - /// forces + /// Update articulated body inertia for forward dynamics. + /// \param[in] _timeStep Rquired for implicit joint stiffness and damping. virtual void updateArtInertia(double _timeStep); - /// Update bias force with implicit joint damping and spring forces + /// Update bias force associated with the articulated body inertia for forward + /// dynamics. + /// \param[in] _gravity Vector of gravitational acceleration + /// \param[in] _timeStep Rquired for implicit joint stiffness and damping. virtual void updateBiasForce(const Eigen::Vector3d& _gravity, double _timeStep); - /// Update joint acceleration and body acceleration for forward dynamics - virtual void updateJointAndBodyAcceleration(); + /// Update bias impulse associated with the articulated body inertia for + /// impulse-based forward dynamics. + virtual void updateBiasImpulse(); - /// Update transmitted force from parent joint - virtual void updateTransmittedWrench(); + /// Update spatial body acceleration with the partial spatial body + /// acceleration for inverse dynamics. + virtual void updateAccelerationID(); - //---------------------------------------------------------------------------- - // Impulse based dynamics - //---------------------------------------------------------------------------- + /// Update spatial body acceleration for forward dynamics. + virtual void updateAccelerationFD(); - /// Update impulsive bias force for impulse-based forward dynamics - /// algorithm - virtual void updateBiasImpulse(); + /// Update spatical body velocity change for impluse-based forward dynamics. + virtual void updateVelocityChangeFD(); + + /// Update spatial body force for inverse dynamics. + /// + /// The spatial body force is transmitted to this BodyNode from the parent + /// body through the connecting joint. It is expressed in this BodyNode's + /// frame. + virtual void updateTransmittedForceID(const Eigen::Vector3d& _gravity, + bool _withExternalForces = false); + + /// Update spatial body force for forward dynamics. + /// + /// The spatial body force is transmitted to this BodyNode from the parent + /// body through the connecting joint. It is expressed in this BodyNode's + /// frame. + virtual void updateTransmittedForceFD(); + + /// Update spatial body force for impulse-based forward dynamics. + /// + /// The spatial body impulse is transmitted to this BodyNode from the parent + /// body through the connecting joint. It is expressed in this BodyNode's + /// frame. + virtual void updateTransmittedImpulse(); + // TODO: Rename to updateTransmittedImpulseFD if impulse-based inverse + // dynamics is implemented. + + /// Update the joint force for inverse dynamics. + virtual void updateJointForceID(double _timeStep, + double _withDampingForces, + double _withSpringForces); + + /// Update the joint force for forward dynamics. + virtual void updateJointForceFD(double _timeStep, + double _withDampingForces, + double _withSpringForces); + + /// Update the joint impulse for forward dynamics. + virtual void updateJointImpulseFD(); + + /// Update constrained terms due to the constraint impulses for foward + /// dynamics. + virtual void updateConstrainedTerms(double _timeStep); + + //- DEPRECATED --------------------------------------------------------------- + + /// Update spatial body acceleration with the partial spatial body + /// acceleration. + /// \warning Please use updateAccelerationID(). + DEPRECATED(4.3) + virtual void updateAcceleration(); + + /// Update the joint acceleration and body acceleration + /// \warning Please use updateAccelerationFD(). + DEPRECATED(4.3) + virtual void updateJointAndBodyAcceleration(); - /// Update joint velocity change for impulse-based forward dynamics - /// algorithm + /// Update joint velocity change for impulse-based forward dynamics algorithm + /// \warning Please use updateVelocityChangeFD(). + DEPRECATED(4.3) virtual void updateJointVelocityChange(); - /// Update body velocity change for impulse-based forward dynamics - /// algorithm -// virtual void updateBodyVelocityChange(); + /// Update the spatial body force transmitted to this BodyNode from the + /// parent body through the connecting joint. The spatial body force is + /// expressed in this BodyNode's frame. + /// \warning Please use updateTransmittedForceFD(). + DEPRECATED(4.3) + virtual void updateTransmittedWrench(); + /// Update spatial body force. Inverse dynamics routine. + /// + /// The spatial body force is transmitted to this BodyNode from the parent + /// body through the connecting joint. It is expressed in this BodyNode's + /// frame. /// + /// \warning Please use updateTransmittedForceID(). + DEPRECATED(4.3) + virtual void updateBodyWrench(const Eigen::Vector3d& _gravity, + bool _withExternalForces = false); + + /// updateBodyImpForceFwdDyn + /// \warning Please use updateTransmittedImpulse(). + DEPRECATED(4.3) virtual void updateBodyImpForceFwdDyn(); - /// + /// Update the joint force + /// \warning Please use updateJointForceID(). + DEPRECATED(4.3) + virtual void updateGeneralizedForce(bool _withDampingForces = false); + + /// updateConstrainedJointAndBodyAcceleration + /// \warning Deprecated. Please do not use. + DEPRECATED(4.3) virtual void updateConstrainedJointAndBodyAcceleration(double _timeStep); - /// + /// updateConstrainedTransmittedForce + /// \warning Deprecated. Please do not use. + DEPRECATED(4.3) virtual void updateConstrainedTransmittedForce(double _timeStep); + /// \} + + //---------------------------------------------------------------------------- + /// \{ \name Equations of motion related routines + //---------------------------------------------------------------------------- + /// virtual void updateMassMatrix(); virtual void aggregateMassMatrix(Eigen::MatrixXd* _MCol, int _col); @@ -591,6 +673,8 @@ class BodyNode virtual void aggregateSpatialToGeneralized(Eigen::VectorXd* _generalized, const Eigen::Vector6d& _spatial); + /// \} + //-------------------------------------------------------------------------- // General properties //-------------------------------------------------------------------------- @@ -601,11 +685,11 @@ class BodyNode /// Counts the number of nodes globally. static int msBodyNodeCount; - /// + /// Name std::string mName; - /// If the gravity mode is false, this body node does not - /// being affected by gravity. + /// If the gravity mode is false, this body node does not being affected by + /// gravity. bool mGravityMode; /// Generalized inertia. @@ -627,10 +711,10 @@ class BodyNode /// Coefficient of friction double mRestitutionCoeff; - /// + /// Array of visualization shpaes std::vector mVizShapes; - /// + /// Array of collision shpaes std::vector mColShapes; /// Indicating whether this node is collidable. @@ -646,13 +730,13 @@ class BodyNode /// Pointer to the model this body node belongs to. Skeleton* mSkeleton; - /// + /// Parent joint Joint* mParentJoint; - /// + /// Parent body node BodyNode* mParentBodyNode; - /// + /// Array of child body nodes std::vector mChildBodyNodes; /// List of markers associated @@ -685,6 +769,7 @@ class BodyNode /// Partial spatial body acceleration due to parent joint's velocity Eigen::Vector6d mPartialAcceleration; + // TODO(JS): Rename with more informative name /// Spatial body acceleration w.r.t. body frame Eigen::Vector6d mA; @@ -699,7 +784,7 @@ class BodyNode /// Spatial gravity force Eigen::Vector6d mFgravity; - /// Articulated body inertia + /// Articulated body inertia math::Inertia mArtInertia; /// Articulated body inertia for implicit joing damping and spring forces @@ -741,6 +826,7 @@ class BodyNode /// Constraint impulse: contact impulse, dynamic joint impulse Eigen::Vector6d mConstraintImpulse; + // TODO(JS): rename with more informative one /// Generalized impulsive body force w.r.t. body frame. Eigen::Vector6d mImpF; diff --git a/dart/dynamics/BoxShape.h b/dart/dynamics/BoxShape.h index 698ecea0cdf38..e5e67a30bb23b 100644 --- a/dart/dynamics/BoxShape.h +++ b/dart/dynamics/BoxShape.h @@ -54,7 +54,8 @@ class BoxShape : public Shape { /// \brief Set size of this box. /// \warning Don't use me any more - DEPRECATED(4.0) void setDim(const Eigen::Vector3d& _size); + DEPRECATED(4.0) + void setDim(const Eigen::Vector3d& _size); /// \brief Set size of this box. void setSize(const Eigen::Vector3d& _size); diff --git a/dart/dynamics/EllipsoidShape.h b/dart/dynamics/EllipsoidShape.h index 5833221e01f1f..4d155c3016b11 100644 --- a/dart/dynamics/EllipsoidShape.h +++ b/dart/dynamics/EllipsoidShape.h @@ -54,7 +54,8 @@ class EllipsoidShape : public Shape { /// \brief Set size of this box. /// \warning Don't use me any more - DEPRECATED(4.0) void setDim(const Eigen::Vector3d& _size); + DEPRECATED(4.0) + void setDim(const Eigen::Vector3d& _size); /// \brief Set size of this box. void setSize(const Eigen::Vector3d& _size); diff --git a/dart/dynamics/EulerJoint.h b/dart/dynamics/EulerJoint.h index 93d06fd9b4d87..e7976989f2a74 100644 --- a/dart/dynamics/EulerJoint.h +++ b/dart/dynamics/EulerJoint.h @@ -69,12 +69,6 @@ class EulerJoint : public MultiDofJoint<3> /// AxisOrder getAxisOrder() const; - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForces; - } - protected: // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/FreeJoint.h b/dart/dynamics/FreeJoint.h index 7eaa1f402af84..2fbb30d238028 100644 --- a/dart/dynamics/FreeJoint.h +++ b/dart/dynamics/FreeJoint.h @@ -56,12 +56,6 @@ class FreeJoint : public MultiDofJoint<6> /// Destructor virtual ~FreeJoint(); - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForces; - } - protected: // Documentation inherited virtual void integratePositions(double _dt); diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index d02e4d2fe0dfa..67714ecd0de25 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -48,24 +48,31 @@ namespace dart { namespace dynamics { +//============================================================================== +const Joint::ActuatorType Joint::DefaultActuatorType = Joint::FORCE; + //============================================================================== Joint::Joint(const std::string& _name) : mName(_name), + mActuatorType(FORCE), mChildBodyNode(NULL), mSkeleton(NULL), mT_ParentBodyToJoint(Eigen::Isometry3d::Identity()), mT_ChildBodyToJoint(Eigen::Isometry3d::Identity()), mT(Eigen::Isometry3d::Identity()), mSpatialVelocity(Eigen::Vector6d::Zero()), - mWrench(Eigen::Vector6d::Zero()), mIsPositionLimited(true) { } -Joint::~Joint() { +//============================================================================== +Joint::~Joint() +{ } -const std::string& Joint::setName(const std::string& _name) { +//============================================================================== +const std::string& Joint::setName(const std::string& _name) +{ if(mName == _name) return mName; @@ -84,20 +91,64 @@ const std::string& Joint::setName(const std::string& _name) { return mName; } -const std::string& Joint::getName() const { +//============================================================================== +const std::string& Joint::getName() const +{ return mName; } +//============================================================================== +void Joint::setActuatorType(Joint::ActuatorType _actuatorType) +{ + mActuatorType = _actuatorType; +} + +//============================================================================== +Joint::ActuatorType Joint::getActuatorType() const +{ + return mActuatorType; +} + +//============================================================================== +bool Joint::isKinematic() const +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + return false; + case ACCELERATION: + case VELOCITY: + case LOCKED: + return true; + default: + { + dterr << "Unsupported actuator type." << std::endl; + return false; + } + } +} + +//============================================================================== +bool Joint::isDynamic() const +{ + return !isKinematic(); +} + +//============================================================================== BodyNode* Joint::getChildBodyNode() { return mChildBodyNode; } +//============================================================================== const BodyNode* Joint::getChildBodyNode() const { return mChildBodyNode; } +//============================================================================== BodyNode* Joint::getParentBodyNode() { if(mChildBodyNode) @@ -106,22 +157,27 @@ BodyNode* Joint::getParentBodyNode() return NULL; } +//============================================================================== const BodyNode* Joint::getParentBodyNode() const { return const_cast(this)->getParentBodyNode(); } +//============================================================================== Skeleton* Joint::getSkeleton() { return mSkeleton; } +//============================================================================== const Skeleton* Joint::getSkeleton() const { return mSkeleton; } -const Eigen::Isometry3d& Joint::getLocalTransform() const { +//============================================================================== +const Eigen::Isometry3d& Joint::getLocalTransform() const +{ return mT; } @@ -140,36 +196,51 @@ const Eigen::Isometry3d& Joint::getLocalTransform() const { // return -1; //} -void Joint::setPositionLimited(bool _isPositionLimited) { +//============================================================================== +void Joint::setPositionLimited(bool _isPositionLimited) +{ mIsPositionLimited = _isPositionLimited; } -bool Joint::isPositionLimited() const { +//============================================================================== +bool Joint::isPositionLimited() const +{ return mIsPositionLimited; } -void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) { +//============================================================================== +void Joint::setTransformFromParentBodyNode(const Eigen::Isometry3d& _T) +{ assert(math::verifyTransform(_T)); mT_ParentBodyToJoint = _T; } -void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) { +//============================================================================== +void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) +{ assert(math::verifyTransform(_T)); mT_ChildBodyToJoint = _T; } -const Eigen::Isometry3d&Joint::getTransformFromParentBodyNode() const { +//============================================================================== +const Eigen::Isometry3d&Joint::getTransformFromParentBodyNode() const +{ return mT_ParentBodyToJoint; } -const Eigen::Isometry3d&Joint::getTransformFromChildBodyNode() const { +//============================================================================== +const Eigen::Isometry3d&Joint::getTransformFromChildBodyNode() const +{ return mT_ChildBodyToJoint; } -void Joint::applyGLTransform(renderer::RenderInterface* _ri) { +//============================================================================== +void Joint::applyGLTransform(renderer::RenderInterface* _ri) +{ _ri->transform(mT); } +//============================================================================== void Joint::init(Skeleton* _skel) { mSkeleton = _skel; diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index aa9a272719c3e..f3996beb5cdd1 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -60,6 +60,68 @@ class Skeleton; class Joint { public: + /// Actuator type + /// + /// The command is taken by setCommand() or setCommands(), and the meaning of + /// command is different depending on the actuator type. The default actuator + /// type is FORCE. (TODO: FreeJoint should be PASSIVE?) + /// + /// FORCE/PASSIVE/SERVO joints are dynamic joints while + /// ACCELERATION/VELOCITY/LOCKED joints are kinematic joints. + /// + /// Note the presence of joint damping force and joint spring force for all + /// the actuator types if the coefficients are non-zero. The default + /// coefficients are zero. + /// + /// \sa setActuatorType(), getActuatorType(), + /// setSpringStiffness(), setDampingCoefficient(), + enum ActuatorType + { + /// Command input is joint force, and the output is joint acceleration. + /// + /// If the command is zero, then it's identical to passive joint. The valid + /// joint constraints are position limit, velocity limit, and Coulomb + /// friction, and the invalid joint constraint is force limit. + FORCE, + + /// Passive joint doesn't take any command input, and the output is joint + /// acceleration. + /// + /// The valid joint constraints are position limit, velocity limit, and + /// Coulomb friction, and the invalid joint constraint is force limit. + PASSIVE, + + /// Command input is desired velocity, and the output is joint acceleration. + /// + /// The constraint solver will try to track the desired velocity within the + /// joint force limit. All the joint constarints are valid. + SERVO, + // TODO: Not implemented yet. + + /// Command input is joint acceleration, and the output is joint force. + /// + /// The joint acceleration is always satisfied but it doesn't take the joint + /// force limit into account. All the joint constraints are invalid. + ACCELERATION, + + /// Command input is joint velocity, and the output is joint force. + /// + /// The joint velocity is always satisfied but it doesn't take the joint + /// force limit into account. If you want to consider the joint force limit, + /// should use SERVO instead. All the joint constraints are invalid. + VELOCITY, + + /// Locked joint always set the velocity and acceleration to zero so that + /// the joint dosen't move at all (locked), and the output is joint force. + /// force. + /// + /// All the joint constraints are invalid. + LOCKED + }; + + /// Default actuator type + static const ActuatorType DefaultActuatorType; + /// Constructor explicit Joint(const std::string& _name = "Joint"); @@ -72,6 +134,23 @@ class Joint /// Get joint name const std::string& getName() const; + /// Set actuator type + void setActuatorType(ActuatorType _actuatorType); + + /// Get actuator type + ActuatorType getActuatorType() const; + + /// Return true if this joint is kinematic joint. + /// + /// Kinematic joint means the motion is prescribed by position or velocity or + /// acceleration, which is determined by the actuator type. + /// ACCELERATION/VELOCITY/LOCKED are kinematic joints while + /// FORCE/PASSIVE/SERVO are dynamic joints. + bool isKinematic() const; + + /// Return true if this joint is dynamic joint. + bool isDynamic() const; + /// Get the child BodyNode of this Joint BodyNode* getChildBodyNode(); @@ -102,12 +181,20 @@ class Joint /// Get transformation from child body node to this joint const Eigen::Isometry3d& getTransformFromChildBodyNode() const; - // TODO(JS): /// Set to enforce joint position limit + /// + /// The joint position limit is valid when the actutor type is one of + /// PASSIVE/FORCE. + /// + /// \sa ActuatorType void setPositionLimited(bool _isPositionLimited); - // TODO(JS): /// Get whether enforcing joint position limit + /// + /// The joint position limit is valid when the actutor type is one of + /// PASSIVE/FORCE. + /// + /// \sa ActuatorType bool isPositionLimited() const; /// Set an unique index in skeleton of a generalized coordinate in this joint @@ -117,13 +204,35 @@ class Joint virtual size_t getIndexInSkeleton(size_t _index) const = 0; /// Get number of generalized coordinates - DEPRECATED(4.1) virtual size_t getDof() const = 0; + DEPRECATED(4.1) + virtual size_t getDof() const = 0; /// Get number of generalized coordinates virtual size_t getNumDofs() const = 0; //---------------------------------------------------------------------------- - // Position + /// \{ \name Command + //---------------------------------------------------------------------------- + + /// Set a single command + virtual void setCommand(size_t _index, double _command) = 0; + + /// Set a sinlge command + virtual double getCommand(size_t _index) const = 0; + + /// Set commands + virtual void setCommands(const Eigen::VectorXd& _commands) = 0; + + /// Get commands + virtual Eigen::VectorXd getCommands() const = 0; + + /// Set zero all the positions + virtual void resetCommands() = 0; + + /// \} + + //---------------------------------------------------------------------------- + /// \{ \name Position //---------------------------------------------------------------------------- /// Set a single position @@ -153,8 +262,10 @@ class Joint /// Get upper limit for position virtual double getPositionUpperLimit(size_t _index) const = 0; + /// \} + //---------------------------------------------------------------------------- - // Velocity + /// \{ \name Velocity //---------------------------------------------------------------------------- /// Set a single velocity @@ -184,8 +295,10 @@ class Joint /// Get upper limit of velocity virtual double getVelocityUpperLimit(size_t _index) const = 0; + /// \} + //---------------------------------------------------------------------------- - // Acceleration + /// \{ \name Acceleration //---------------------------------------------------------------------------- /// Set a single acceleration @@ -215,8 +328,10 @@ class Joint /// Get upper limit of acceleration virtual double getAccelerationUpperLimit(size_t _index) const = 0; + /// \} + //---------------------------------------------------------------------------- - // Force + /// \{ \name Force //---------------------------------------------------------------------------- /// Set a single force @@ -246,8 +361,10 @@ class Joint /// Get upper limit of position virtual double getForceUpperLimit(size_t _index) const = 0; + /// \} + //---------------------------------------------------------------------------- - // Velocity change + /// \{ \name Velocity change //---------------------------------------------------------------------------- /// Set a single velocity change @@ -259,8 +376,10 @@ class Joint /// Set zero all the velocity change virtual void resetVelocityChanges() = 0; + /// \} + //---------------------------------------------------------------------------- - // Constraint impulse + /// \{ \name Constraint impulse //---------------------------------------------------------------------------- /// Set a single constraint impulse @@ -272,8 +391,10 @@ class Joint /// Set zero all the constraint impulses virtual void resetConstraintImpulses() = 0; + /// \} + //---------------------------------------------------------------------------- - // Integration + /// \{ \name Integration //---------------------------------------------------------------------------- /// Integrate positions using Euler method @@ -282,8 +403,10 @@ class Joint /// Integrate velocities using Euler method virtual void integrateVelocities(double _dt) = 0; + /// \} + //---------------------------------------------------------------------------- - // Spring and damper + /// \{ \name Spring and damper //---------------------------------------------------------------------------- /// Set spring stiffness for spring force @@ -314,6 +437,8 @@ class Joint /// \param[in] _index Index of joint axis virtual double getDampingCoefficient(size_t _index) const = 0; + /// \} + //---------------------------------------------------------------------------- /// Get potential energy @@ -343,6 +468,7 @@ class Joint /// Get constraint wrench expressed in body node frame virtual Eigen::Vector6d getBodyConstraintWrench() const = 0; + // TODO: Need more informative name. /// Get spring force /// @@ -391,93 +517,136 @@ class Joint virtual void init(Skeleton* _skel); //---------------------------------------------------------------------------- - // Recursive algorithms + /// \{ \name Recursive dynamics routines //---------------------------------------------------------------------------- /// Update transformation from parent body node to child body node virtual void updateLocalTransform() = 0; - /// Update generalized Jacobian from parent body node to child body node - /// w.r.t. local generalized coordinate + /// Update generalized Jacobian from parent body node to child body + /// node w.r.t. local generalized coordinate virtual void updateLocalJacobian() = 0; - /// Update time derivative of generalized Jacobian from parent body node to - /// child body node w.r.t. local generalized coordinate + /// Update time derivative of generalized Jacobian from parent body + /// node to child body node w.r.t. local generalized coordinate virtual void updateLocalJacobianTimeDeriv() = 0; - /// + /// Add joint velocity to _vel virtual void addVelocityTo(Eigen::Vector6d& _vel) = 0; - /// Set partial + /// Set joint partial acceleration to _partialAcceleration virtual void setPartialAccelerationTo( Eigen::Vector6d& _partialAcceleration, const Eigen::Vector6d& _childVelocity) = 0; + // TODO(JS): Rename with more informative name - /// + /// Add joint acceleration to _acc virtual void addAccelerationTo(Eigen::Vector6d& _acc) = 0; - /// + /// Add joint velocity change to _velocityChange virtual void addVelocityChangeTo(Eigen::Vector6d& _velocityChange) = 0; /// Add child's articulated inertia to parent's one - virtual void addChildArtInertiaTo(Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia) = 0; + virtual void addChildArtInertiaTo( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) = 0; - /// Add child's articulated inertia to parent's one + /// Add child's articulated inertia to parent's one. Forward dynamics routine. virtual void addChildArtInertiaImplicitTo( Eigen::Matrix6d& _parentArtInertiaImplicit, const Eigen::Matrix6d& _childArtInertiaImplicit) = 0; + // TODO(JS): rename to updateAInertiaChildAInertia() /// Update inverse of projected articulated body inertia virtual void updateInvProjArtInertia(const Eigen::Matrix6d& _artInertia) = 0; - /// Update inverse of projected articulated body inertia with implicit damping - /// and spring forces + /// Forward dynamics routine. virtual void updateInvProjArtInertiaImplicit( const Eigen::Matrix6d& _artInertia, double _timeStep) = 0; + // TODO(JS): rename to updateAInertiaPsi() /// Add child's bias force to parent's one - virtual void addChildBiasForceTo(Eigen::Vector6d& _parentBiasForce, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce, - const Eigen::Vector6d& _childPartialAcc) = 0; + virtual void addChildBiasForceTo( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) = 0; /// Add child's bias impulse to parent's one - virtual void addChildBiasImpulseTo(Eigen::Vector6d& _parentBiasImpulse, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasImpulse) - = 0; + virtual void addChildBiasImpulseTo( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse) = 0; - /// + /// Update joint total force virtual void updateTotalForce(const Eigen::Vector6d& _bodyForce, double _timeStep) = 0; + // TODO: rename - /// + /// Update joint total impulse virtual void updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) = 0; + // TODO: rename - /// + /// Set total impulses to zero virtual void resetTotalImpulses() = 0; - /// + /// Update joint acceleration virtual void updateAcceleration(const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _spatialAcc) = 0; - /// - virtual void updateVelocityChange(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _velocityChange) = 0; + /// Update joint velocity change + /// \param _artInertia + /// \param _velocityChange + virtual void updateVelocityChange( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange) = 0; - /// - virtual void updateVelocityWithVelocityChange() = 0; + /// Update joint force for inverse dynamics. + /// \param[in] _bodyForce Transmitting spatial body force from the parent + /// BodyNode to the child BodyNode. The spatial force is expressed in the + /// child BodyNode's frame. + virtual void updateForceID(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) = 0; - /// - virtual void updateAccelerationWithVelocityChange(double _timeStep) = 0; + /// Update joint force for forward dynamics. + /// \param[in] _bodyForce Transmitting spatial body force from the parent + /// BodyNode to the child BodyNode. The spatial force is expressed in the + /// child BodyNode's frame. + virtual void updateForceFD(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForcese, + bool _withSpringForces) = 0; - /// - virtual void updateForceWithImpulse(double _timeStep) = 0; + /// Update joint impulses for inverse dynamics + virtual void updateImpulseID(const Eigen::Vector6d& _bodyImpulse) = 0; + + /// Update joint impulses for forward dynamics + virtual void updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) = 0; + + /// Update constrained terms for forward dynamics + virtual void updateConstrainedTerms(double _timeStep) = 0; + + //- DEPRECATED --------------------------------------------------------------- + + /// updateVelocityWithVelocityChange + DEPRECATED(4.3) + virtual void updateVelocityWithVelocityChange() {} + + /// updateAccelerationWithVelocityChange + DEPRECATED(4.3) + virtual void updateAccelerationWithVelocityChange(double _timeStep) {} + + /// updateForceWithImpulse + DEPRECATED(4.3) + virtual void updateForceWithImpulse(double _timeStep) {} + + /// \} //---------------------------------------------------------------------------- - // Recursive algorithms for equations of motion + /// \{ \name Recursive algorithm routines for equations of motion //---------------------------------------------------------------------------- /// Add child's bias force to parent's one @@ -515,10 +684,15 @@ class Joint virtual Eigen::VectorXd getSpatialToGeneralized( const Eigen::Vector6d& _spatial) = 0; + /// \} + protected: /// Joint name std::string mName; + /// Actuator type + ActuatorType mActuatorType; + /// Child BodyNode pointer that this Joint belongs to BodyNode* mChildBodyNode; @@ -538,9 +712,8 @@ class Joint /// velocity is expressed in child body frame Eigen::Vector6d mSpatialVelocity; - // TODO(JS): Temporary code -public: /// Transmitting wrench from parent body to child body expressed in child body + DEPRECATED(4.3) Eigen::Vector6d mWrench; protected: diff --git a/dart/dynamics/MultiDofJoint.h b/dart/dynamics/MultiDofJoint.h index a6664188f3b40..c736080956bc8 100644 --- a/dart/dynamics/MultiDofJoint.h +++ b/dart/dynamics/MultiDofJoint.h @@ -40,8 +40,10 @@ #include #include +#include "dart/config.h" #include "dart/common/Console.h" #include "dart/math/Helpers.h" +#include "dart/dynamics/BodyNode.h" #include "dart/dynamics/Joint.h" #include "dart/dynamics/Skeleton.h" @@ -67,7 +69,8 @@ class MultiDofJoint : public Joint //---------------------------------------------------------------------------- // Documentation inherited - DEPRECATED(4.1) virtual size_t getDof() const; + DEPRECATED(4.1) + virtual size_t getDof() const; // Documentation inherited virtual size_t getNumDofs() const; @@ -78,6 +81,25 @@ class MultiDofJoint : public Joint // Documentation inherited virtual size_t getIndexInSkeleton(size_t _index) const; + //---------------------------------------------------------------------------- + // Command + //---------------------------------------------------------------------------- + + // Documentation inherited + virtual void setCommand(size_t _index, double _command); + + // Documentation inherited + virtual double getCommand(size_t _index) const; + + // Documentation inherited + virtual void setCommands(const Eigen::VectorXd& _commands); + + // Documentation inherited + virtual Eigen::VectorXd getCommands() const; + + // Documentation inherited + virtual void resetCommands(); + //---------------------------------------------------------------------------- // Position //---------------------------------------------------------------------------- @@ -265,86 +287,111 @@ class MultiDofJoint : public Joint // Documentation inherited virtual double getPotentialEnergy() const; + // Documentation inherited + virtual Eigen::Vector6d getBodyConstraintWrench() const override; + protected: //---------------------------------------------------------------------------- - // Recursive dynamics algorithms + /// \{ \name Recursive dynamics routines //---------------------------------------------------------------------------- // Documentation inherited - virtual const math::Jacobian getLocalJacobian() const; + virtual const math::Jacobian getLocalJacobian() const override; // Documentation inherited - virtual const math::Jacobian getLocalJacobianTimeDeriv() const; + virtual const math::Jacobian getLocalJacobianTimeDeriv() const override; // Documentation inherited - virtual void addVelocityTo(Eigen::Vector6d& _vel); + virtual void addVelocityTo(Eigen::Vector6d& _vel) override; // Documentation inherited - virtual void setPartialAccelerationTo(Eigen::Vector6d& _partialAcceleration, - const Eigen::Vector6d& _childVelocity); + virtual void setPartialAccelerationTo( + Eigen::Vector6d& _partialAcceleration, + const Eigen::Vector6d& _childVelocity) override; // Documentation inherited - virtual void addAccelerationTo(Eigen::Vector6d& _acc); + virtual void addAccelerationTo(Eigen::Vector6d& _acc) override; // Documentation inherited - virtual void addVelocityChangeTo(Eigen::Vector6d& _velocityChange); + virtual void addVelocityChangeTo(Eigen::Vector6d& _velocityChange) override; // Documentation inherited - virtual void addChildArtInertiaTo(Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia); + virtual void addChildArtInertiaTo( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) override; // Documentation inherited virtual void addChildArtInertiaImplicitTo( Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia); + const Eigen::Matrix6d& _childArtInertia) override; // Documentation inherited - virtual void updateInvProjArtInertia(const Eigen::Matrix6d& _artInertia); + virtual void updateInvProjArtInertia( + const Eigen::Matrix6d& _artInertia) override; // Documentation inherited virtual void updateInvProjArtInertiaImplicit( - const Eigen::Matrix6d& _artInertia, double _timeStep); + const Eigen::Matrix6d& _artInertia, double _timeStep) override; // Documentation inherited - virtual void addChildBiasForceTo(Eigen::Vector6d& _parentBiasForce, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce, - const Eigen::Vector6d& _childPartialAcc); + virtual void addChildBiasForceTo( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) override; // Documentation inherited - virtual void addChildBiasImpulseTo(Eigen::Vector6d& _parentBiasImpulse, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasImpulse); + virtual void addChildBiasImpulseTo( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse) override; // Documentation inherited virtual void updateTotalForce(const Eigen::Vector6d& _bodyForce, - double _timeStep); + double _timeStep) override; // Documentation inherited - virtual void updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse); + virtual void updateTotalImpulse( + const Eigen::Vector6d& _bodyImpulse) override; // Documentation inherited - virtual void resetTotalImpulses(); + virtual void resetTotalImpulses() override; // Documentation inherited - virtual void updateAcceleration(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _spatialAcc); + virtual void updateAcceleration( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc) override; // Documentation inherited - virtual void updateVelocityChange(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _velocityChange); + virtual void updateVelocityChange( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange) override; // Documentation inherited - virtual void updateVelocityWithVelocityChange(); + virtual void updateForceID(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) override; // Documentation inherited - virtual void updateAccelerationWithVelocityChange(double _timeStep); + virtual void updateForceFD(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) override; // Documentation inherited - virtual void updateForceWithImpulse(double _timeStep); + virtual void updateImpulseID(const Eigen::Vector6d& _bodyImpulse) override; + + // Documentation inherited + virtual void updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) override; + + // Documentation inherited + virtual void updateConstrainedTerms(double _timeStep) override; + + /// \} //---------------------------------------------------------------------------- - // Recursive algorithms for equations of motion + /// \{ \name Recursive algorithm routines for equations of motion //---------------------------------------------------------------------------- // Documentation inherited @@ -382,11 +429,16 @@ class MultiDofJoint : public Joint virtual Eigen::VectorXd getSpatialToGeneralized( const Eigen::Vector6d& _spatial); + /// \} + protected: // TODO(JS): Need? /// Eigen::Matrix mIndexInSkeleton; + /// Command + Eigen::Matrix mCommands; + //---------------------------------------------------------------------------- // Configuration //---------------------------------------------------------------------------- @@ -458,8 +510,8 @@ class MultiDofJoint : public Joint /// Change of generalized velocity Eigen::Matrix mVelocityChanges; -// /// Generalized impulse -// Eigen::Matrix mImpulse; + /// Generalized impulse + Eigen::Matrix mImpulses; /// Generalized constraint impulse Eigen::Matrix mConstraintImpulses; @@ -510,6 +562,94 @@ class MultiDofJoint : public Joint /// Eigen::Matrix mInvMassMatrixSegment; +private: + //---------------------------------------------------------------------------- + /// \{ \name Recursive dynamics routines + //---------------------------------------------------------------------------- + + void addChildArtInertiaToDynamic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia); + + void addChildArtInertiaToKinematic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia); + + void addChildArtInertiaImplicitToDynamic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia); + + void addChildArtInertiaImplicitToKinematic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia); + + void updateInvProjArtInertiaDynamic( + const Eigen::Matrix6d& _artInertia); + + void updateInvProjArtInertiaKinematic( + const Eigen::Matrix6d& _artInertia); + + void updateInvProjArtInertiaImplicitDynamic( + const Eigen::Matrix6d& _artInertia, double _timeStep); + + void updateInvProjArtInertiaImplicitKinematic( + const Eigen::Matrix6d& _artInertia, double _timeStep); + + void addChildBiasForceToDynamic( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc); + + void addChildBiasForceToKinematic( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc); + + void addChildBiasImpulseToDynamic( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse); + + void addChildBiasImpulseToKinematic( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse); + + void updateTotalForceDynamic(const Eigen::Vector6d& _bodyForce, + double _timeStep); + + void updateTotalForceKinematic(const Eigen::Vector6d& _bodyForce, + double _timeStep); + + void updateTotalImpulseDynamic( + const Eigen::Vector6d& _bodyImpulse); + + void updateTotalImpulseKinematic( + const Eigen::Vector6d& _bodyImpulse); + + void updateAccelerationDynamic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc); + + void updateAccelerationKinematic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc); + + void updateVelocityChangeDynamic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange); + + void updateVelocityChangeKinematic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange); + + void updateConstrainedTermsDynamic(double _timeStep); + + void updateConstrainedTermsKinematic(double _timeStep); + + /// \} }; //============================================================================== @@ -517,6 +657,7 @@ template MultiDofJoint::MultiDofJoint(const std::string& _name) : Joint(_name), mIndexInSkeleton(Eigen::Matrix::Constant(0u)), + mCommands(Eigen::Matrix::Constant(0.0)), mPositions(Eigen::Matrix::Constant(0.0)), mPositionLowerLimits(Eigen::Matrix::Constant(-DART_DBL_INF)), mPositionUpperLimits(Eigen::Matrix::Constant(DART_DBL_INF)), @@ -534,7 +675,7 @@ MultiDofJoint::MultiDofJoint(const std::string& _name) mForceUpperLimits(Eigen::Matrix::Constant(DART_DBL_INF)), mForcesDeriv(Eigen::Matrix::Constant(0.0)), mVelocityChanges(Eigen::Matrix::Constant(0.0)), - // mImpulse(Eigen::Matrix::Constant(0.0)), + mImpulses(Eigen::Matrix::Constant(0.0)), mConstraintImpulses(Eigen::Matrix::Constant(0.0)), mSpringStiffness(Eigen::Matrix::Constant(0.0)), mRestPosition(Eigen::Matrix::Constant(0.0)), @@ -597,6 +738,63 @@ size_t MultiDofJoint::getIndexInSkeleton(size_t _index) const return mIndexInSkeleton[_index]; } +//============================================================================== +template +void MultiDofJoint::setCommand(size_t _index, double _position) +{ + if (_index >= getNumDofs()) + { + dterr << "[MultiDofJoint::setCommand]: index[" << _index << "] out of range" + << std::endl; + return; + } + + mCommands[_index] = _position; +} + +//============================================================================== +template +double MultiDofJoint::getCommand(size_t _index) const +{ + if (_index >= getNumDofs()) + { + dterr << "[MultiDofJoint::getCommand]: index[" << _index << "] out of range" + << std::endl; + return 0.0; + } + + return mCommands[_index]; +} + +//============================================================================== +template +void MultiDofJoint::setCommands(const Eigen::VectorXd& _commands) +{ + if (static_cast(_commands.size()) != getNumDofs()) + { + dterr << "[MultiDofJoint::setCommands]: commands's size[" + << _commands.size() << "] is different with the dof [" + << getNumDofs() << "]" << std::endl; + return; + } + + mCommands = _commands; +} + +//============================================================================== +template +Eigen::VectorXd MultiDofJoint::getCommands() const +{ + return mCommands; +} + +//============================================================================== +template +void MultiDofJoint::resetCommands() +{ + mCommands.setZero(); +} + //============================================================================== template void MultiDofJoint::setPosition(size_t _index, double _position) @@ -627,7 +825,7 @@ double MultiDofJoint::getPosition(size_t _index) const template void MultiDofJoint::setPositions(const Eigen::VectorXd& _positions) { - if (_positions.size() != getNumDofs()) + if (static_cast(_positions.size()) != getNumDofs()) { dterr << "setPositions positions's size[" << _positions.size() << "] is different with the dof [" << getNumDofs() << "]" << std::endl; @@ -737,7 +935,7 @@ double MultiDofJoint::getVelocity(size_t _index) const template void MultiDofJoint::setVelocities(const Eigen::VectorXd& _velocities) { - if (_velocities.size() != getNumDofs()) + if (static_cast(_velocities.size()) != getNumDofs()) { dterr << "setVelocities velocities's size[" << _velocities.size() << "] is different with the dof [" << getNumDofs() << "]" << std::endl; @@ -829,6 +1027,12 @@ void MultiDofJoint::setAcceleration(size_t _index, double _acceleration) } mAccelerations[_index] = _acceleration; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == ACCELERATION) + mCommands[_index] = mAccelerations[_index]; +#endif + // TODO: Remove at DART 5.0. } //============================================================================== @@ -849,7 +1053,7 @@ double MultiDofJoint::getAcceleration(size_t _index) const template void MultiDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) { - if (_accelerations.size() != getNumDofs()) + if (static_cast(_accelerations.size()) != getNumDofs()) { dterr << "setAccelerations accelerations's size[" << _accelerations.size() << "] is different with the dof [" << getNumDofs() << "]" << std::endl; @@ -857,6 +1061,12 @@ void MultiDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) } mAccelerations = _accelerations; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == ACCELERATION) + mCommands = mAccelerations; +#endif + // TODO: Remove at DART 5.0. } //============================================================================== @@ -942,6 +1152,12 @@ void MultiDofJoint::setForce(size_t _index, double _force) } mForces[_index] = _force; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == FORCE) + mCommands[_index] = mForces[_index]; +#endif + // TODO: Remove at DART 5.0. } //============================================================================== @@ -961,7 +1177,7 @@ double MultiDofJoint::getForce(size_t _index) template void MultiDofJoint::setForces(const Eigen::VectorXd& _forces) { - if (_forces.size() != getNumDofs()) + if (static_cast(_forces.size()) != getNumDofs()) { dterr << "setForces forces's size[" << _forces.size() << "] is different with the dof [" << getNumDofs() << "]" << std::endl; @@ -969,6 +1185,12 @@ void MultiDofJoint::setForces(const Eigen::VectorXd& _forces) } mForces = _forces; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == FORCE) + mCommands = mForces; +#endif + // TODO: Remove at DART 5.0. } //============================================================================== @@ -1230,6 +1452,14 @@ double MultiDofJoint::getPotentialEnergy() const return pe; } +//============================================================================== +template +Eigen::Vector6d MultiDofJoint::getBodyConstraintWrench() const +{ + assert(mChildBodyNode); + return mChildBodyNode->getBodyForce() - mJacobian * mForces; +} + //============================================================================== template const math::Jacobian MultiDofJoint::getLocalJacobian() const @@ -1261,11 +1491,9 @@ void MultiDofJoint::setPartialAccelerationTo( Eigen::Vector6d& _partialAcceleration, const Eigen::Vector6d& _childVelocity) { - // ad(V, S * dq) - _partialAcceleration = math::ad(_childVelocity, mJacobian * mVelocities); - - // Add joint acceleration - _partialAcceleration.noalias() += mJacobianDeriv * mVelocities; + // ad(V, S * dq) + dS * dq + _partialAcceleration = math::ad(_childVelocity, mJacobian * mVelocities) + + mJacobianDeriv * mVelocities; // Verification assert(!math::isNan(_partialAcceleration)); @@ -1298,6 +1526,32 @@ template void MultiDofJoint::addChildArtInertiaTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + addChildArtInertiaToDynamic(_parentArtInertia, + _childArtInertia); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + addChildArtInertiaToKinematic(_parentArtInertia, + _childArtInertia); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::addChildArtInertiaToDynamic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) { // Child body's articulated inertia Eigen::Matrix AIS = _childArtInertia * mJacobian; @@ -1310,11 +1564,48 @@ void MultiDofJoint::addChildArtInertiaTo( _parentArtInertia += math::transformInertia(mT.inverse(), PI); } +//============================================================================== +template +void MultiDofJoint::addChildArtInertiaToKinematic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) +{ + // Add child body's articulated inertia to parent body's articulated inertia. + // Note that mT should be updated. + _parentArtInertia += math::transformInertia(mT.inverse(), _childArtInertia); +} + //============================================================================== template void MultiDofJoint::addChildArtInertiaImplicitTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + addChildArtInertiaImplicitToDynamic(_parentArtInertia, + _childArtInertia); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + addChildArtInertiaImplicitToKinematic(_parentArtInertia, + _childArtInertia); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::addChildArtInertiaImplicitToDynamic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) { // Child body's articulated inertia Eigen::Matrix AIS = _childArtInertia * mJacobian; @@ -1327,13 +1618,47 @@ void MultiDofJoint::addChildArtInertiaImplicitTo( _parentArtInertia += math::transformInertia(mT.inverse(), PI); } +//============================================================================== +template +void MultiDofJoint::addChildArtInertiaImplicitToKinematic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) +{ + // Add child body's articulated inertia to parent body's articulated inertia. + // Note that mT should be updated. + _parentArtInertia += math::transformInertia(mT.inverse(), _childArtInertia); +} + //============================================================================== template void MultiDofJoint::updateInvProjArtInertia( const Eigen::Matrix6d& _artInertia) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateInvProjArtInertiaDynamic(_artInertia); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateInvProjArtInertiaKinematic(_artInertia); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::updateInvProjArtInertiaDynamic( + const Eigen::Matrix6d& _artInertia) { // Projected articulated inertia - Eigen::Matrix projAI + const Eigen::Matrix projAI = mJacobian.transpose() * _artInertia * mJacobian; // Inversion of projected articulated inertia @@ -1345,11 +1670,43 @@ void MultiDofJoint::updateInvProjArtInertia( assert(!math::isNan(mInvProjArtInertia)); } +//============================================================================== +template +void MultiDofJoint::updateInvProjArtInertiaKinematic( + const Eigen::Matrix6d& /*_artInertia*/) +{ + // Do nothing +} + //============================================================================== template void MultiDofJoint::updateInvProjArtInertiaImplicit( const Eigen::Matrix6d& _artInertia, double _timeStep) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateInvProjArtInertiaImplicitDynamic(_artInertia, _timeStep); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateInvProjArtInertiaImplicitKinematic(_artInertia, _timeStep); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::updateInvProjArtInertiaImplicitDynamic( + const Eigen::Matrix6d& _artInertia, + double _timeStep) { // Projected articulated inertia Eigen::Matrix projAI @@ -1371,6 +1728,15 @@ void MultiDofJoint::updateInvProjArtInertiaImplicit( assert(!math::isNan(mInvProjArtInertiaImplicit)); } +//============================================================================== +template +void MultiDofJoint::updateInvProjArtInertiaImplicitKinematic( + const Eigen::Matrix6d& _artInertia, + double _timeStep) +{ + // Do nothing +} + //============================================================================== template void MultiDofJoint::addChildBiasForceTo( @@ -1378,13 +1744,71 @@ void MultiDofJoint::addChildBiasForceTo( const Eigen::Matrix6d& _childArtInertia, const Eigen::Vector6d& _childBiasForce, const Eigen::Vector6d& _childPartialAcc) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + addChildBiasForceToDynamic(_parentBiasForce, + _childArtInertia, + _childBiasForce, + _childPartialAcc); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + addChildBiasForceToKinematic(_parentBiasForce, + _childArtInertia, + _childBiasForce, + _childPartialAcc); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::addChildBiasForceToDynamic( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) { // Compute beta - Eigen::Vector6d beta + const Eigen::Vector6d beta = _childBiasForce - + _childArtInertia - * (_childPartialAcc - + mJacobian * mInvProjArtInertiaImplicit * mTotalForce); + + _childArtInertia + * (_childPartialAcc + + mJacobian*mInvProjArtInertiaImplicit*mTotalForce); + + // Eigen::Vector6d beta + // = _childBiasForce; + // beta.noalias() += _childArtInertia * _childPartialAcc; + // beta.noalias() += _childArtInertia * mJacobian * mInvProjArtInertiaImplicit * mTotalForce; + + // Verification + assert(!math::isNan(beta)); + + // Add child body's bias force to parent body's bias force. Note that mT + // should be updated. + _parentBiasForce += math::dAdInvT(mT, beta); +} + +//============================================================================== +template +void MultiDofJoint::addChildBiasForceToKinematic( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) +{ + // Compute beta + const Eigen::Vector6d beta + = _childBiasForce + + _childArtInertia*(_childPartialAcc + mJacobian*mAccelerations); // Eigen::Vector6d beta // = _childBiasForce; @@ -1405,11 +1829,40 @@ void MultiDofJoint::addChildBiasImpulseTo( Eigen::Vector6d& _parentBiasImpulse, const Eigen::Matrix6d& _childArtInertia, const Eigen::Vector6d& _childBiasImpulse) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + addChildBiasImpulseToDynamic(_parentBiasImpulse, + _childArtInertia, + _childBiasImpulse); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + addChildBiasImpulseToKinematic(_parentBiasImpulse, + _childArtInertia, + _childBiasImpulse); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::addChildBiasImpulseToDynamic( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse) { // Compute beta - Eigen::Vector6d beta + const Eigen::Vector6d beta = _childBiasImpulse - + _childArtInertia * mJacobian * mInvProjArtInertia * mTotalImpulse; + + _childArtInertia*mJacobian*mInvProjArtInertia*mTotalImpulse; // Verification assert(!math::isNan(beta)); @@ -1419,33 +1872,124 @@ void MultiDofJoint::addChildBiasImpulseTo( _parentBiasImpulse += math::dAdInvT(mT, beta); } +//============================================================================== +template +void MultiDofJoint::addChildBiasImpulseToKinematic( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse) +{ + // Add child body's bias force to parent body's bias force. Note that mT + // should be updated. + _parentBiasImpulse += math::dAdInvT(mT, _childBiasImpulse); +} + //============================================================================== template void MultiDofJoint::updateTotalForce( const Eigen::Vector6d& _bodyForce, double _timeStep) +{ + assert(_timeStep > 0.0); + + switch (mActuatorType) + { + case FORCE: + mForces = mCommands; + updateTotalForceDynamic(_bodyForce, _timeStep); + break; + case PASSIVE: + case SERVO: + mForces.setZero(); + updateTotalForceDynamic(_bodyForce, _timeStep); + break; + case ACCELERATION: + mAccelerations = mCommands; + updateTotalForceKinematic(_bodyForce, _timeStep); + break; + case VELOCITY: + mAccelerations = (mCommands - mVelocities) / _timeStep; + updateTotalForceKinematic(_bodyForce, _timeStep); + break; + case LOCKED: + mVelocities.setZero(); + mAccelerations.setZero(); + updateTotalForceKinematic(_bodyForce, _timeStep); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::updateTotalForceDynamic( + const Eigen::Vector6d& _bodyForce, + double _timeStep) { // Spring force - Eigen::Matrix springForce + const Eigen::Matrix springForce = (-mSpringStiffness).asDiagonal() - * (mPositions - mRestPosition + mVelocities * _timeStep); + *(mPositions - mRestPosition + mVelocities*_timeStep); // Damping force - Eigen::Matrix dampingForce - = (-mDampingCoefficient).asDiagonal() * mVelocities; + const Eigen::Matrix dampingForce + = (-mDampingCoefficient).asDiagonal()*mVelocities; // mTotalForce = mForces + springForce + dampingForce; - mTotalForce.noalias() -= mJacobian.transpose() * _bodyForce; + mTotalForce.noalias() -= mJacobian.transpose()*_bodyForce; +} + +//============================================================================== +template +void MultiDofJoint::updateTotalForceKinematic( + const Eigen::Vector6d& _bodyForce, + double _timeStep) +{ + // Do nothing } //============================================================================== template -void MultiDofJoint::updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) +void MultiDofJoint::updateTotalImpulse( + const Eigen::Vector6d& _bodyImpulse) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateTotalImpulseDynamic(_bodyImpulse); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateTotalImpulseKinematic(_bodyImpulse); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::updateTotalImpulseDynamic( + const Eigen::Vector6d& _bodyImpulse) { // mTotalImpulse = mConstraintImpulses; - mTotalImpulse.noalias() -= mJacobian.transpose() * _bodyImpulse; + mTotalImpulse.noalias() -= mJacobian.transpose()*_bodyImpulse; +} + +//============================================================================== +template +void MultiDofJoint::updateTotalImpulseKinematic( + const Eigen::Vector6d& _bodyImpulse) +{ + // Do nothing } //============================================================================== @@ -1460,28 +2004,85 @@ template void MultiDofJoint::updateAcceleration( const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _spatialAcc) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateAccelerationDynamic(_artInertia, _spatialAcc); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateAccelerationKinematic(_artInertia, _spatialAcc); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::updateAccelerationDynamic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc) { // mAccelerations = mInvProjArtInertiaImplicit * (mTotalForce - mJacobian.transpose() - * _artInertia * math::AdInvT(mT, _spatialAcc)); + *_artInertia*math::AdInvT(mT, _spatialAcc)); // Verification assert(!math::isNan(mAccelerations)); } +//============================================================================== +template +void MultiDofJoint::updateAccelerationKinematic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc) +{ + // Do nothing +} + //============================================================================== template void MultiDofJoint::updateVelocityChange( const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _velocityChange) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateVelocityChangeDynamic(_artInertia, _velocityChange); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateVelocityChangeKinematic(_artInertia, _velocityChange); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::updateVelocityChangeDynamic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange) { // mVelocityChanges = mInvProjArtInertia * (mTotalImpulse - mJacobian.transpose() - * _artInertia * math::AdInvT(mT, _velocityChange)); + *_artInertia*math::AdInvT(mT, _velocityChange)); // Verification assert(!math::isNan(mVelocityChanges)); @@ -1489,23 +2090,132 @@ void MultiDofJoint::updateVelocityChange( //============================================================================== template -void MultiDofJoint::updateVelocityWithVelocityChange() +void MultiDofJoint::updateVelocityChangeKinematic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange) { - mVelocities += mVelocityChanges; + // Do nothing } //============================================================================== template -void MultiDofJoint::updateAccelerationWithVelocityChange(double _timeStep) +void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) { - mAccelerations.noalias() += mVelocityChanges / _timeStep; + mForces = mJacobian.transpose()*_bodyForce; + + // Damping force + if (_withDampingForces) + { + const Eigen::Matrix dampingForces + = (-mDampingCoefficient).asDiagonal()*mVelocities; + mForces -= dampingForces; + } + + // Spring force + if (_withSpringForces) + { + const Eigen::Matrix springForces + = (-mSpringStiffness).asDiagonal() + *(mPositions - mRestPosition + mVelocities*_timeStep); + mForces -= springForces; + } +} + +//============================================================================== +template +void MultiDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateForceID(_bodyForce, _timeStep, _withDampingForces, + _withSpringForces); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::updateImpulseID(const Eigen::Vector6d& _bodyImpulse) +{ + mImpulses = mJacobian.transpose()*_bodyImpulse; +} + +//============================================================================== +template +void MultiDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateImpulseID(_bodyImpulse); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } } //============================================================================== template -void MultiDofJoint::updateForceWithImpulse(double _timeStep) +void MultiDofJoint::updateConstrainedTerms(double _timeStep) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateConstrainedTermsDynamic(_timeStep); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateConstrainedTermsKinematic(_timeStep); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +template +void MultiDofJoint::updateConstrainedTermsDynamic(double _timeStep) +{ + const double invTimeStep = 1.0 / _timeStep; + + mVelocities += mVelocityChanges; + mAccelerations.noalias() += mVelocityChanges*invTimeStep; + mForces.noalias() += mImpulses*invTimeStep; +} + +//============================================================================== +template +void MultiDofJoint::updateConstrainedTermsKinematic( + double _timeStep) { - mForces.noalias() += mConstraintImpulses / _timeStep; + mForces.noalias() += mImpulses / _timeStep; } //============================================================================== diff --git a/dart/dynamics/PlanarJoint.h b/dart/dynamics/PlanarJoint.h index 463c0be456d7a..6b2114bf96438 100644 --- a/dart/dynamics/PlanarJoint.h +++ b/dart/dynamics/PlanarJoint.h @@ -93,12 +93,6 @@ class PlanarJoint : public MultiDofJoint<3> /// Return second translational axis const Eigen::Vector3d& getTranslationalAxis2() const; - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForces; - } - protected: // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index e6cfcc16e888d..8168d73f1c017 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -49,6 +49,7 @@ using namespace Eigen; namespace dart { namespace dynamics { +//============================================================================== PointMass::PointMass(SoftBodyNode* _softBodyNode) : // mIndexInSkeleton(Eigen::Matrix::Zero()), mPositions(Eigen::Vector3d::Zero()), @@ -98,22 +99,26 @@ PointMass::PointMass(SoftBodyNode* _softBodyNode) assert(mParentSoftBodyNode != NULL); } +//============================================================================== PointMass::~PointMass() { delete mShape; } +//============================================================================== void PointMass::setMass(double _mass) { assert(0.0 < _mass); mMass = _mass; } +//============================================================================== double PointMass::getMass() const { return mMass; } +//============================================================================== void PointMass::addConnectedPointMass(PointMass* _pointMass) { assert(_pointMass != NULL); @@ -121,11 +126,13 @@ void PointMass::addConnectedPointMass(PointMass* _pointMass) mConnectedPointMasses.push_back(_pointMass); } +//============================================================================== int PointMass::getNumConnectedPointMasses() const { return mConnectedPointMasses.size(); } +//============================================================================== PointMass* PointMass::getConnectedPointMass(size_t _idx) { assert(0 <= _idx && _idx < mConnectedPointMasses.size()); @@ -133,16 +140,19 @@ PointMass* PointMass::getConnectedPointMass(size_t _idx) return mConnectedPointMasses[_idx]; } +//============================================================================== const PointMass* PointMass::getConnectedPointMass(size_t _idx) const { return const_cast(this)->getConnectedPointMass(_idx); } +//============================================================================== void PointMass::setColliding(bool _isColliding) { mIsColliding = _isColliding; } +//============================================================================== bool PointMass::isColliding() { return mIsColliding; @@ -450,26 +460,31 @@ void PointMass::clearConstraintImpulse() mImpF.setZero(); } +//============================================================================== void PointMass::setRestingPosition(const Eigen::Vector3d& _p) { mX0 = _p; } +//============================================================================== const Eigen::Vector3d& PointMass::getRestingPosition() const { return mX0; } +//============================================================================== const Eigen::Vector3d& PointMass::getLocalPosition() const { return mX; } +//============================================================================== const Eigen::Vector3d& PointMass::getWorldPosition() const { return mW; } +//============================================================================== Eigen::Matrix PointMass::getBodyJacobian() { assert(mParentSoftBodyNode != NULL); @@ -491,6 +506,7 @@ Eigen::Matrix PointMass::getBodyJacobian() return J; } +//============================================================================== Eigen::Matrix PointMass::getWorldJacobian() { return mParentSoftBodyNode->getTransform().linear() @@ -503,47 +519,56 @@ const Eigen::Vector3d& PointMass::getBodyVelocityChange() const return mDelV; } +//============================================================================== SoftBodyNode* PointMass::getParentSoftBodyNode() { return mParentSoftBodyNode; } +//============================================================================== const SoftBodyNode* PointMass::getParentSoftBodyNode() const { return mParentSoftBodyNode; } +//============================================================================== //int PointMass::getNumDependentGenCoords() const //{ // return mDependentGenCoordIndices.size(); //} +//============================================================================== //int PointMass::getDependentGenCoord(int _arrayIndex) const //{ // assert(0 <= _arrayIndex && _arrayIndex < mDependentGenCoordIndices.size()); // return mDependentGenCoordIndices[_arrayIndex]; //} +//============================================================================== const Eigen::Vector3d&PointMass::getBodyVelocity() const { return mV; } +//============================================================================== Eigen::Vector3d PointMass::getWorldVelocity() const { return mParentSoftBodyNode->getTransform().linear() * mV; } +//============================================================================== const Eigen::Vector3d& PointMass::getBodyAcceleration() const { return mA; } +//============================================================================== Eigen::Vector3d PointMass::getWorldAcceleration() const { return mParentSoftBodyNode->getTransform().linear() * mA; } +//============================================================================== void PointMass::init() { // Dependen generalized coordinate setting @@ -561,6 +586,7 @@ void PointMass::init() // mDependentGenCoordIndices[parentDof + 2] = mIndexInSkeleton[2]; } +//============================================================================== void PointMass::updateTransform() { // Local transpose @@ -573,6 +599,7 @@ void PointMass::updateTransform() assert(!math::isNan(mW)); } +//============================================================================== void PointMass::updateVelocity() { // v = w(parent) x mX + v(parent) + dq @@ -582,6 +609,7 @@ void PointMass::updateVelocity() assert(!math::isNan(mV)); } +//============================================================================== void PointMass::updatePartialAcceleration() { // eta = w(parent) x dq @@ -590,7 +618,14 @@ void PointMass::updatePartialAcceleration() assert(!math::isNan(mEta)); } +//============================================================================== void PointMass::updateAcceleration() +{ + updateAccelerationID(); +} + +//============================================================================== +void PointMass::updateAccelerationID() { // dv = dw(parent) x mX + dv(parent) + eata + ddq mA = mParentSoftBodyNode->getBodyAcceleration().head<3>().cross(mX) + @@ -599,8 +634,16 @@ void PointMass::updateAcceleration() assert(!math::isNan(mA)); } +//============================================================================== void PointMass::updateBodyForce(const Eigen::Vector3d& _gravity, bool _withExternalForces) +{ + updateTransmittedForceID(_gravity, _withExternalForces); +} + +//============================================================================== +void PointMass::updateTransmittedForceID(const Eigen::Vector3d& _gravity, + bool _withExternalForces) { // f = m*dv + w(parent) x m*v - fext mF.noalias() = mMass * mA; @@ -614,7 +657,14 @@ void PointMass::updateBodyForce(const Eigen::Vector3d& _gravity, assert(!math::isNan(mF)); } +//============================================================================== void PointMass::updateArticulatedInertia(double _dt) +{ + updateArtInertiaFD(_dt); +} + +//============================================================================== +void PointMass::updateArtInertiaFD(double _timeStep) { // Articulated inertia // - Do nothing @@ -623,8 +673,9 @@ void PointMass::updateArticulatedInertia(double _dt) mPsi = 1.0 / mMass; mImplicitPsi = 1.0 / (mMass - + _dt * mParentSoftBodyNode->getDampingCoefficient() - + _dt * _dt * mParentSoftBodyNode->getVertexSpringStiffness()); + + _timeStep * mParentSoftBodyNode->getDampingCoefficient() + + _timeStep * _timeStep + * mParentSoftBodyNode->getVertexSpringStiffness()); assert(!math::isNan(mImplicitPsi)); // Cache data: AI_S_Psi @@ -637,13 +688,24 @@ void PointMass::updateArticulatedInertia(double _dt) assert(!math::isNan(mImplicitPi)); } -void PointMass::updateGeneralizedForce(bool _withDampingForces) +//============================================================================== +void PointMass::updateGeneralizedForce(bool /*_withDampingForces*/) +{ + updateJointForceID(false, false, false); +} + +//============================================================================== +void PointMass::updateJointForceID(double /*_timeStep*/, + double /*_withDampingForces*/, + double /*_withSpringForces*/) { // tau = f mForces = mF; + // TODO: need to add spring and damping forces } -void PointMass::updateBiasForce(double _dt, const Eigen::Vector3d& _gravity) +//============================================================================== +void PointMass::updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity) { // B = w(parent) x m*v - fext - fgravity // - w(parent) x m*v - fext @@ -680,7 +742,14 @@ void PointMass::updateBiasForce(double _dt, const Eigen::Vector3d& _gravity) assert(!math::isNan(mBeta)); } +//============================================================================== void PointMass::updateJointAndBodyAcceleration() +{ + updateAccelerationFD(); +} + +//============================================================================== +void PointMass::updateAccelerationFD() { // ddq = imp_psi*(alpha - m*(dw(parent) x mX + dv(parent)) Eigen::Vector3d ddq = @@ -698,6 +767,7 @@ void PointMass::updateJointAndBodyAcceleration() assert(!math::isNan(mA)); } +//============================================================================== void PointMass::updateTransmittedForce() { // f = m*dv + B @@ -706,6 +776,7 @@ void PointMass::updateTransmittedForce() assert(!math::isNan(mF)); } +//============================================================================== void PointMass::updateMassMatrix() { mM_dV = mAccelerations @@ -716,6 +787,12 @@ void PointMass::updateMassMatrix() //============================================================================== void PointMass::updateBiasImpulse() +{ + updateBiasImpulseFD(); +} + +//============================================================================== +void PointMass::updateBiasImpulseFD() { mImpB = -mConstraintImpulses; assert(!math::isNan(mImpB)); @@ -731,6 +808,12 @@ void PointMass::updateBiasImpulse() //============================================================================== void PointMass::updateJointVelocityChange() +{ + updateVelocityChangeFD(); +} + +//============================================================================== +void PointMass::updateVelocityChangeFD() { // Eigen::Vector3d del_dq // = mPsi @@ -765,6 +848,12 @@ void PointMass::updateBodyVelocityChange() //============================================================================== void PointMass::updateBodyImpForceFwdDyn() +{ + updateTransmittedImpulse(); +} + +//============================================================================== +void PointMass::updateTransmittedImpulse() { mImpF = mImpB; mImpF.noalias() += mMass * mDelV; @@ -774,14 +863,14 @@ void PointMass::updateBodyImpForceFwdDyn() //============================================================================== void PointMass::updateConstrainedJointAndBodyAcceleration(double _timeStep) { - // 1. dq = dq + del_dq - updateVelocityWithVelocityChange(); +// // 1. dq = dq + del_dq +// updateVelocityWithVelocityChange(); - // 2. ddq = ddq + del_dq / dt - updateAccelerationWithVelocityChange(_timeStep); +// // 2. ddq = ddq + del_dq / dt +// updateAccelerationWithVelocityChange(_timeStep); - // 3. tau = tau + imp / dt - updateForceWithImpulse(_timeStep); +// // 3. tau = tau + imp / dt +// updateForceWithImpulse(_timeStep); } //============================================================================== @@ -794,6 +883,26 @@ void PointMass::updateConstrainedTransmittedForce(double _timeStep) mF += _timeStep * mImpF; } +//============================================================================== +void PointMass::updateConstrainedTermsFD(double _timeStep) +{ + // 1. dq = dq + del_dq + mVelocities += mVelocityChanges; + + // 2. ddq = ddq + del_dq / dt + mAccelerations.noalias() += mVelocityChanges / _timeStep; + + // 3. tau = tau + imp / dt + mForces.noalias() += mConstraintImpulses / _timeStep; + + /// + mA += mDelV / _timeStep; + + /// + mF += _timeStep * mImpF; +} + +//============================================================================== void PointMass::aggregateMassMatrix(Eigen::MatrixXd* _MCol, int _col) { // // Assign @@ -803,6 +912,7 @@ void PointMass::aggregateMassMatrix(Eigen::MatrixXd* _MCol, int _col) // _MCol->block<3, 1>(iStart, _col).noalias() = mM_F; } +//============================================================================== void PointMass::aggregateAugMassMatrix(Eigen::MatrixXd* _MCol, int _col, double _timeStep) { @@ -817,16 +927,19 @@ void PointMass::aggregateAugMassMatrix(Eigen::MatrixXd* _MCol, int _col, // = mM_F + (_timeStep * _timeStep * kv + _timeStep * d) * mAccelerations; } +//============================================================================== void PointMass::updateInvMassMatrix() { mBiasForceForInvMeta = mForces; } +//============================================================================== void PointMass::updateInvAugMassMatrix() { // mBiasForceForInvMeta = mMass * mImplicitPsi * mForces; } +//============================================================================== void PointMass::aggregateInvMassMatrix(Eigen::MatrixXd* _MInvCol, int _col) { // // Assign @@ -838,6 +951,7 @@ void PointMass::aggregateInvMassMatrix(Eigen::MatrixXd* _MInvCol, int _col) // - mParentSoftBodyNode->mInvM_U.tail<3>(); } +//============================================================================== void PointMass::aggregateInvAugMassMatrix(Eigen::MatrixXd* _MInvCol, int _col, double _timeStep) { @@ -851,6 +965,7 @@ void PointMass::aggregateInvAugMassMatrix(Eigen::MatrixXd* _MInvCol, int _col, // + mParentSoftBodyNode->mInvM_U.tail<3>())); } +//============================================================================== void PointMass::aggregateGravityForceVector(Eigen::VectorXd* _g, const Eigen::Vector3d& _gravity) { @@ -863,6 +978,7 @@ void PointMass::aggregateGravityForceVector(Eigen::VectorXd* _g, // _g->segment<3>(iStart) = mG_F; } +//============================================================================== void PointMass::updateCombinedVector() { mCg_dV = mEta @@ -870,6 +986,7 @@ void PointMass::updateCombinedVector() + mParentSoftBodyNode->mCg_dV.tail<3>(); } +//============================================================================== void PointMass::aggregateCombinedVector(Eigen::VectorXd* _Cg, const Eigen::Vector3d& _gravity) { @@ -885,12 +1002,14 @@ void PointMass::aggregateCombinedVector(Eigen::VectorXd* _Cg, // _Cg->segment<3>(iStart) = mCg_F; } +//============================================================================== void PointMass::aggregateExternalForces(Eigen::VectorXd* _Fext) { // int iStart = mIndexInSkeleton[0]; // _Fext->segment<3>(iStart) = mFext; } +//============================================================================== void PointMass::draw(renderer::RenderInterface* _ri, const Eigen::Vector4d& _color, bool _useDefaultColor) const diff --git a/dart/dynamics/PointMass.h b/dart/dynamics/PointMass.h index 93058dd69f1f3..dd82a7f53af47 100644 --- a/dart/dynamics/PointMass.h +++ b/dart/dynamics/PointMass.h @@ -39,6 +39,7 @@ #include #include +#include "dart/common/Deprecated.h" namespace dart { namespace renderer { @@ -219,17 +220,6 @@ class PointMass //---------------------------------------------------------------------------- - /// - void updateVelocityWithVelocityChange(); - - /// - void updateAccelerationWithVelocityChange(double _timeStep); - - /// - void updateForceWithImpulse(double _timeStep); - - //---------------------------------------------------------------------------- - /// Add linear Cartesian force to this node. /// \param[in] _force External force. /// \param[in] _isForceLocal True if _force's reference frame is of the parent @@ -313,65 +303,130 @@ class PointMass /// void init(); - /// + //---------------------------------------------------------------------------- + /// \{ \name Recursive dynamics routines + //---------------------------------------------------------------------------- + + /// \brief Update transformation. void updateTransform(); - /// + /// \brief Update body velocity. void updateVelocity(); - /// + /// \brief Update partial body acceleration due to parent joint's velocity. void updatePartialAcceleration(); - /// + /// \brief Update articulated body inertia. Forward dynamics routine. + /// \param[in] _timeStep Rquired for implicit joint stiffness and damping. + void updateArtInertiaFD(double _timeStep); + + /// \brief Update bias force associated with the articulated body inertia. + /// Forward dynamics routine. + /// \param[in] _gravity Vector of gravitational acceleration + /// \param[in] _timeStep Rquired for implicit joint stiffness and damping. + void updateBiasForceFD(double _dt, const Eigen::Vector3d& _gravity); + + /// \brief Update bias impulse associated with the articulated body inertia. + /// Impulse-based forward dynamics routine. + void updateBiasImpulseFD(); + + /// \brief Update body acceleration with the partial body acceleration. + void updateAccelerationID(); + + /// \brief Update body acceleration. Forward dynamics routine. + void updateAccelerationFD(); + + /// \brief Update body velocity change. Impluse-based forward dynamics + /// routine. + void updateVelocityChangeFD(); + + /// \brief Update body force. Inverse dynamics routine. + void updateTransmittedForceID(const Eigen::Vector3d& _gravity, + bool _withExternalForces = false); + + /// \brief Update body force. Forward dynamics routine. + void updateTransmittedForce(); + + /// \brief Update body force. Impulse-based forward dynamics routine. + void updateTransmittedImpulse(); + + /// \brief Update the joint force. Inverse dynamics routine. + void updateJointForceID(double _timeStep, + double _withDampingForces, + double _withSpringForces); + + /// \brief Update constrained terms due to the constraint impulses. Foward + /// dynamics routine. + void updateConstrainedTermsFD(double _timeStep); + + //- DEPRECATED --------------------------------------------------------------- + + /// \warning Please use updateAccelerationID(). + DEPRECATED(4.3) void updateAcceleration(); - /// + /// \warning Please use updateTransmittedForceID(). + DEPRECATED(4.3) void updateBodyForce(const Eigen::Vector3d& _gravity, bool _withExternalForces = false); - /// + /// \warning Please use updateArtInertiaFD(). + DEPRECATED(4.3) void updateArticulatedInertia(double _dt); - /// + /// \warning Please use updateJointForceID(). + DEPRECATED(4.3) void updateGeneralizedForce(bool _withDampingForces = false); - /// - void updateBiasForce(double _dt, const Eigen::Vector3d& _gravity); - - /// + /// \warning Please use updateAccelerationFD(). + DEPRECATED(4.3) void updateJointAndBodyAcceleration(); - /// - void updateTransmittedForce(); - - /// - void updateMassMatrix(); - - //---------------------------------------------------------------------------- - - /// Update impulsive bias force for impulse-based forward dynamics - /// algorithm + /// \brief Update impulsive bias force for impulse-based forward dynamics + /// algorithm. + /// \warning Please use updateBiasImpulseFD(). + DEPRECATED(4.3) void updateBiasImpulse(); - /// Update joint velocity change for impulse-based forward dynamics - /// algorithm + /// \brief Update joint velocity change for impulse-based forward dynamics + /// algorithm. + /// \warning Please use updateVelocityChangeFD(). + DEPRECATED(4.3) void updateJointVelocityChange(); - /// Update body velocity change for impulse-based forward dynamics - /// algorithm + /// \brief Update body velocity change for impulse-based forward dynamics + /// algorithm. + DEPRECATED(4.3) void updateBodyVelocityChange(); - /// + /// \warning Please use updateTransmittedImpulse(). + DEPRECATED(4.3) void updateBodyImpForceFwdDyn(); - /// + DEPRECATED(4.3) void updateConstrainedJointAndBodyAcceleration(double _timeStep); - /// + DEPRECATED(4.3) void updateConstrainedTransmittedForce(double _timeStep); + DEPRECATED(4.3) + void updateVelocityWithVelocityChange(); + + DEPRECATED(4.3) + void updateAccelerationWithVelocityChange(double _timeStep); + + DEPRECATED(4.3) + void updateForceWithImpulse(double _timeStep); + + /// \} + + //---------------------------------------------------------------------------- + /// \{ \name Equations of motion related routines //---------------------------------------------------------------------------- + /// + void updateMassMatrix(); + /// void aggregateMassMatrix(Eigen::MatrixXd* _MCol, int _col); @@ -407,6 +462,8 @@ class PointMass /// coordinates recursively. void aggregateExternalForces(Eigen::VectorXd* _Fext); + /// \} + //-------------------- Cache Data for Mass Matrix ---------------------------- /// Eigen::Vector3d mM_dV; diff --git a/dart/dynamics/PrismaticJoint.h b/dart/dynamics/PrismaticJoint.h index b8810d2e32ad8..b665c928b6140 100644 --- a/dart/dynamics/PrismaticJoint.h +++ b/dart/dynamics/PrismaticJoint.h @@ -63,12 +63,6 @@ class PrismaticJoint : public SingleDofJoint /// const Eigen::Vector3d& getAxis() const; - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForce; - } - protected: // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/RevoluteJoint.h b/dart/dynamics/RevoluteJoint.h index 4f8e8786519bd..40dcf350fdeb7 100644 --- a/dart/dynamics/RevoluteJoint.h +++ b/dart/dynamics/RevoluteJoint.h @@ -63,12 +63,6 @@ class RevoluteJoint : public SingleDofJoint /// const Eigen::Vector3d& getAxis() const; - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForce; - } - protected: // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/ScrewJoint.h b/dart/dynamics/ScrewJoint.h index 7d9e189fe81aa..58e3be1e7a945 100644 --- a/dart/dynamics/ScrewJoint.h +++ b/dart/dynamics/ScrewJoint.h @@ -70,12 +70,6 @@ class ScrewJoint : public SingleDofJoint /// double getPitch() const; - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForce; - } - protected: // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index fa620554315e5..6456d38a3bce3 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -48,6 +48,7 @@ namespace dynamics { SingleDofJoint::SingleDofJoint(const std::string& _name) : Joint(_name), mIndexInSkeleton(0u), + mCommand(0.0), mPosition(0.0), mPositionLowerLimit(-DART_DBL_INF), mPositionUpperLimit(DART_DBL_INF), @@ -65,7 +66,7 @@ SingleDofJoint::SingleDofJoint(const std::string& _name) mForceUpperLimit(DART_DBL_INF), mForceDeriv(0.0), mVelocityChange(0.0), - // mImpulse(0.0), + mImpulse(0.0), mConstraintImpulse(0.0), mSpringStiffness(0.0), mRestPosition(0.0), @@ -122,6 +123,58 @@ size_t SingleDofJoint::getIndexInSkeleton(size_t _index) const return mIndexInSkeleton; } +//============================================================================== +void SingleDofJoint::setCommand(size_t _index, double _command) +{ + if (_index != 0) + { + dterr << "[SingleDofJoint::setCommand]: index[" << _index << "] out of range" + << std::endl; + return; + } + + mCommand = _command; +} + +//============================================================================== +double SingleDofJoint::getCommand(size_t _index) const +{ + if (_index != 0) + { + dterr << "[SingleDofJoint::getCommand]: index[" << _index << "] out of range" + << std::endl; + return 0.0; + } + + return mCommand; +} + +//============================================================================== +void SingleDofJoint::setCommands(const Eigen::VectorXd& _commands) +{ + if (static_cast(_commands.size()) != getNumDofs()) + { + dterr << "[SingleDofJoint::setCommands]: commands's size[" + << _commands.size() << "] is different with the dof [" << getNumDofs() + << "]" << std::endl; + return; + } + + mCommand = _commands[0]; +} + +//============================================================================== +Eigen::VectorXd SingleDofJoint::getCommands() const +{ + return Eigen::Matrix::Constant(mCommand); +} + +//============================================================================== +void SingleDofJoint::resetCommands() +{ + mCommand = 0.0; +} + //============================================================================== void SingleDofJoint::setPosition(size_t _index, double _position) { @@ -149,7 +202,7 @@ double SingleDofJoint::getPosition(size_t _index) const //============================================================================== void SingleDofJoint::setPositions(const Eigen::VectorXd& _positions) { - if (_positions.size() != math::castUIntToInt(getNumDofs())) + if (static_cast(_positions.size()) != getNumDofs()) { dterr << "setPositions positions's size[" << _positions.size() << "] is different with the dof [" << getNumDofs() << "]" << std::endl; @@ -250,7 +303,7 @@ double SingleDofJoint::getVelocity(size_t _index) const //============================================================================== void SingleDofJoint::setVelocities(const Eigen::VectorXd& _velocities) { - if (_velocities.size() != math::castUIntToInt(getNumDofs())) + if (static_cast(_velocities.size()) != getNumDofs()) { dterr << "setVelocities velocities's size[" << _velocities.size() << "] is different with the dof [" << getNumDofs() << "]" << std::endl; @@ -335,6 +388,11 @@ void SingleDofJoint::setAcceleration(size_t _index, double _acceleration) } mAcceleration = _acceleration; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == ACCELERATION) + mCommand = mAcceleration; +#endif } //============================================================================== @@ -353,7 +411,7 @@ double SingleDofJoint::getAcceleration(size_t _index) const //============================================================================== void SingleDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) { - if (_accelerations.size() != math::castUIntToInt(getNumDofs())) + if (static_cast(_accelerations.size()) != getNumDofs()) { dterr << "setAccelerations accelerations's size[" << _accelerations.size() << "] is different with the dof [" << getNumDofs() << "]" << std::endl; @@ -361,6 +419,11 @@ void SingleDofJoint::setAccelerations(const Eigen::VectorXd& _accelerations) } mAcceleration = _accelerations[0]; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == ACCELERATION) + mCommand = mAcceleration; +#endif } //============================================================================== @@ -439,6 +502,12 @@ void SingleDofJoint::setForce(size_t _index, double _force) } mForce = _force; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == FORCE) + mCommand = mForce; +#endif + // TODO: Remove at DART 5.0. } //============================================================================== @@ -456,7 +525,7 @@ double SingleDofJoint::getForce(size_t _index) //============================================================================== void SingleDofJoint::setForces(const Eigen::VectorXd& _forces) { - if (_forces.size() != math::castUIntToInt(getNumDofs())) + if (static_cast(_forces.size()) != getNumDofs()) { dterr << "setForces forces's size[" << _forces.size() << "] is different with the dof [" << getNumDofs() << "]" << std::endl; @@ -464,6 +533,12 @@ void SingleDofJoint::setForces(const Eigen::VectorXd& _forces) } mForce = _forces[0]; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == FORCE) + mCommand = mForce; +#endif + // TODO: Remove at DART 5.0. } //============================================================================== @@ -476,6 +551,11 @@ Eigen::VectorXd SingleDofJoint::getForces() const void SingleDofJoint::resetForces() { mForce = 0.0; + +#if DART_MAJOR_VERSION == 4 + if (mActuatorType == FORCE) + mCommand = mForce; +#endif } //============================================================================== @@ -702,6 +782,13 @@ double SingleDofJoint::getPotentialEnergy() const return pe; } +//============================================================================== +Eigen::Vector6d SingleDofJoint::getBodyConstraintWrench() const +{ + assert(mChildBodyNode); + return mChildBodyNode->getBodyForce() - mJacobian * mForce; +} + //============================================================================== const math::Jacobian SingleDofJoint::getLocalJacobian() const { @@ -729,14 +816,9 @@ void SingleDofJoint::setPartialAccelerationTo( Eigen::Vector6d& _partialAcceleration, const Eigen::Vector6d& _childVelocity) { - // ad(V, S * dq) - _partialAcceleration = math::ad(_childVelocity, mJacobian * mVelocity); - - // Verification - assert(!math::isNan(_partialAcceleration)); - - // Add joint acceleration - _partialAcceleration.noalias() += mJacobianDeriv * mVelocity; + // ad(V, S * dq) + dS * dq + _partialAcceleration = math::ad(_childVelocity, mJacobian * mVelocity) + + mJacobianDeriv * mVelocity; // Verification assert(!math::isNan(_partialAcceleration)); @@ -760,6 +842,29 @@ void SingleDofJoint::addVelocityChangeTo(Eigen::Vector6d& _velocityChange) //============================================================================== void SingleDofJoint::addChildArtInertiaTo( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + addChildArtInertiaToDynamic(_parentArtInertia, + _childArtInertia); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + addChildArtInertiaToKinematic(_parentArtInertia, + _childArtInertia); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::addChildArtInertiaToDynamic(Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { // Child body's articulated inertia Eigen::Vector6d AIS = _childArtInertia * mJacobian; @@ -772,13 +877,45 @@ void SingleDofJoint::addChildArtInertiaTo( _parentArtInertia += math::transformInertia(mT.inverse(), PI); } +//============================================================================== +void SingleDofJoint::addChildArtInertiaToKinematic( + Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) +{ + // Add child body's articulated inertia to parent body's articulated inertia. + // Note that mT should be updated. + _parentArtInertia += math::transformInertia(mT.inverse(), _childArtInertia); +} + //============================================================================== void SingleDofJoint::addChildArtInertiaImplicitTo( - Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia) + Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + addChildArtInertiaImplicitToDynamic(_parentArtInertia, + _childArtInertia); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + addChildArtInertiaImplicitToKinematic(_parentArtInertia, + _childArtInertia); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::addChildArtInertiaImplicitToDynamic( + Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { // Child body's articulated inertia - Eigen::Vector6d AIS = _childArtInertia * mJacobian; + const Eigen::Vector6d AIS = _childArtInertia * mJacobian; Eigen::Matrix6d PI = _childArtInertia; PI.noalias() -= mInvProjArtInertiaImplicit * AIS * AIS.transpose(); assert(!math::isNan(PI)); @@ -789,7 +926,39 @@ void SingleDofJoint::addChildArtInertiaImplicitTo( } //============================================================================== -void SingleDofJoint::updateInvProjArtInertia(const Eigen::Matrix6d& _artInertia) +void SingleDofJoint::addChildArtInertiaImplicitToKinematic( + Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) +{ + // Add child body's articulated inertia to parent body's articulated inertia. + // Note that mT should be updated. + _parentArtInertia += math::transformInertia(mT.inverse(), _childArtInertia); +} + +//============================================================================== +void SingleDofJoint::updateInvProjArtInertia( + const Eigen::Matrix6d& _artInertia) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateInvProjArtInertiaDynamic(_artInertia); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateInvProjArtInertiaKinematic(_artInertia); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateInvProjArtInertiaDynamic( + const Eigen::Matrix6d& _artInertia) { // Projected articulated inertia double projAI = mJacobian.dot(_artInertia * mJacobian); @@ -801,10 +970,39 @@ void SingleDofJoint::updateInvProjArtInertia(const Eigen::Matrix6d& _artInertia) assert(!math::isNan(mInvProjArtInertia)); } +//============================================================================== +void SingleDofJoint::updateInvProjArtInertiaKinematic( + const Eigen::Matrix6d& /*_artInertia*/) +{ + // Do nothing +} + //============================================================================== void SingleDofJoint::updateInvProjArtInertiaImplicit( const Eigen::Matrix6d& _artInertia, double _timeStep) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateInvProjArtInertiaImplicitDynamic(_artInertia, _timeStep); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateInvProjArtInertiaImplicitKinematic(_artInertia, _timeStep); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateInvProjArtInertiaImplicitDynamic( + const Eigen::Matrix6d& _artInertia, double _timeStep) { // Projected articulated inertia double projAI = mJacobian.dot(_artInertia * mJacobian); @@ -821,24 +1019,75 @@ void SingleDofJoint::updateInvProjArtInertiaImplicit( assert(!math::isNan(mInvProjArtInertiaImplicit)); } +//============================================================================== +void SingleDofJoint::updateInvProjArtInertiaImplicitKinematic( + const Eigen::Matrix6d& /*_artInertia*/, double /*_timeStep*/) +{ + // Do nothing +} + //============================================================================== void SingleDofJoint::addChildBiasForceTo( Eigen::Vector6d& _parentBiasForce, const Eigen::Matrix6d& _childArtInertia, const Eigen::Vector6d& _childBiasForce, const Eigen::Vector6d& _childPartialAcc) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + addChildBiasForceToDynamic(_parentBiasForce, + _childArtInertia, + _childBiasForce, + _childPartialAcc); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + addChildBiasForceToKinematic(_parentBiasForce, + _childArtInertia, + _childBiasForce, + _childPartialAcc); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::addChildBiasForceToDynamic( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) { // Compute beta -// Eigen::Vector6d beta -// = _childBiasForce -// + _childArtInertia -// * (_childPartialAcc -// + mJacobian * mInvProjArtInertiaImplicit * mTotalForce); + Eigen::Vector6d beta = _childBiasForce; + const double coeff = mInvProjArtInertiaImplicit * mTotalForce; + beta.noalias() += _childArtInertia*(_childPartialAcc + coeff*mJacobian); - Eigen::Vector6d beta - = _childBiasForce; - beta.noalias() += _childArtInertia * _childPartialAcc; - beta.noalias() += _childArtInertia * mJacobian * mInvProjArtInertiaImplicit * mTotalForce; + // Verification + assert(!math::isNan(beta)); + + // Add child body's bias force to parent body's bias force. Note that mT + // should be updated. + _parentBiasForce += math::dAdInvT(mT, beta); +} + +//============================================================================== +void SingleDofJoint::addChildBiasForceToKinematic( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) +{ + // Compute beta + Eigen::Vector6d beta = _childBiasForce; + beta.noalias() += _childArtInertia*(_childPartialAcc + + mAcceleration*mJacobian); // Verification assert(!math::isNan(beta)); @@ -853,9 +1102,37 @@ void SingleDofJoint::addChildBiasImpulseTo( Eigen::Vector6d& _parentBiasImpulse, const Eigen::Matrix6d& _childArtInertia, const Eigen::Vector6d& _childBiasImpulse) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + addChildBiasImpulseToDynamic(_parentBiasImpulse, + _childArtInertia, + _childBiasImpulse); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + addChildBiasImpulseToKinematic(_parentBiasImpulse, + _childArtInertia, + _childBiasImpulse); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::addChildBiasImpulseToDynamic( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse) { // Compute beta - Eigen::Vector6d beta + const Eigen::Vector6d beta = _childBiasImpulse + _childArtInertia * mJacobian * mInvProjArtInertia * mTotalImpulse; @@ -868,30 +1145,109 @@ void SingleDofJoint::addChildBiasImpulseTo( } //============================================================================== -void SingleDofJoint::updateTotalForce( - const Eigen::Vector6d& _bodyForce, - double _timeStep) +void SingleDofJoint::addChildBiasImpulseToKinematic( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& /*_childArtInertia*/, + const Eigen::Vector6d& _childBiasImpulse) +{ + // Add child body's bias force to parent body's bias force. Note that mT + // should be updated. + _parentBiasImpulse += math::dAdInvT(mT, _childBiasImpulse); +} + +//============================================================================== +void SingleDofJoint::updateTotalForce(const Eigen::Vector6d& _bodyForce, + double _timeStep) +{ + assert(_timeStep > 0.0); + + switch (mActuatorType) + { + case FORCE: + mForce = mCommand; + updateTotalForceDynamic(_bodyForce, _timeStep); + break; + case PASSIVE: + case SERVO: + mForce = 0.0; + updateTotalForceDynamic(_bodyForce, _timeStep); + break; + case ACCELERATION: + mAcceleration = mCommand; + updateTotalForceKinematic(_bodyForce, _timeStep); + break; + case VELOCITY: + mAcceleration = (mCommand - mVelocity) / _timeStep; + updateTotalForceKinematic(_bodyForce, _timeStep); + break; + case LOCKED: + mVelocity = 0.0; + mAcceleration = 0.0; + updateTotalForceKinematic(_bodyForce, _timeStep); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateTotalForceDynamic( + const Eigen::Vector6d& _bodyForce, double _timeStep) { // Spring force - double springForce - = -mSpringStiffness * (mPosition - + mVelocity * _timeStep - - mRestPosition); + const double nextPosition = mPosition + _timeStep*mVelocity; + const double springForce = -mSpringStiffness * (nextPosition - mRestPosition); // Damping force - double dampingForce = -mDampingCoefficient * mVelocity; + const double dampingForce = -mDampingCoefficient * mVelocity; // Compute alpha mTotalForce = mForce + springForce + dampingForce - mJacobian.dot(_bodyForce); } +//============================================================================== +void SingleDofJoint::updateTotalForceKinematic( + const Eigen::Vector6d& /*_bodyForce*/, double /*_timeStep*/) +{ + // Do nothing +} + //============================================================================== void SingleDofJoint::updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) { - // + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateTotalImpulseDynamic(_bodyImpulse); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateTotalImpulseKinematic(_bodyImpulse); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateTotalImpulseDynamic( + const Eigen::Vector6d& _bodyImpulse) +{ mTotalImpulse = mConstraintImpulse - mJacobian.dot(_bodyImpulse); } +//============================================================================== +void SingleDofJoint::updateTotalImpulseKinematic( + const Eigen::Vector6d& /*_bodyImpulse*/) +{ + // Do nothing +} + //============================================================================== void SingleDofJoint::resetTotalImpulses() { @@ -900,21 +1256,72 @@ void SingleDofJoint::resetTotalImpulses() //============================================================================== void SingleDofJoint::updateAcceleration(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _spatialAcc) + const Eigen::Vector6d& _spatialAcc) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateAccelerationDynamic(_artInertia, _spatialAcc); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateAccelerationKinematic(_artInertia, _spatialAcc); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateAccelerationDynamic( + const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _spatialAcc) { // - mAcceleration - = mInvProjArtInertiaImplicit - * (mTotalForce - - mJacobian.dot(_artInertia * math::AdInvT(mT, _spatialAcc))); + const double val = mJacobian.dot(_artInertia * math::AdInvT(mT, _spatialAcc)); + mAcceleration = mInvProjArtInertiaImplicit * (mTotalForce - val); // Verification assert(!math::isNan(mAcceleration)); } //============================================================================== -void SingleDofJoint::updateVelocityChange(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _velocityChange) +void SingleDofJoint::updateAccelerationKinematic( + const Eigen::Matrix6d& /*_artInertia*/, + const Eigen::Vector6d& /*_spatialAcc*/) +{ + // Do nothing +} + +//============================================================================== +void SingleDofJoint::updateVelocityChange( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateVelocityChangeDynamic(_artInertia, _velocityChange); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateVelocityChangeKinematic(_artInertia, _velocityChange); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateVelocityChangeDynamic( + const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _velocityChange) { // mVelocityChange @@ -927,21 +1334,122 @@ void SingleDofJoint::updateVelocityChange(const Eigen::Matrix6d& _artInertia, } //============================================================================== -void SingleDofJoint::updateVelocityWithVelocityChange() +void SingleDofJoint::updateVelocityChangeKinematic( + const Eigen::Matrix6d& /*_artInertia*/, + const Eigen::Vector6d& /*_velocityChange*/) +{ + // Do nothing +} + +//============================================================================== +void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) +{ + mForce = mJacobian.dot(_bodyForce); + + // Damping force + if (_withDampingForces) + { + const double dampingForce = -mDampingCoefficient * mVelocity; + mForce -= dampingForce; + } + + // Spring force + if (_withSpringForces) + { + const double nextPosition = mPosition + _timeStep*mVelocity; + const double springForce = -mSpringStiffness*(nextPosition - mRestPosition); + mForce -= springForce; + } +} + +//============================================================================== +void SingleDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateForceID(_bodyForce, _timeStep, _withDampingForces, + _withSpringForces); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateImpulseID(const Eigen::Vector6d& _bodyImpulse) { - mVelocity += mVelocityChange; + mImpulse = mJacobian.dot(_bodyImpulse); } //============================================================================== -void SingleDofJoint::updateAccelerationWithVelocityChange(double _timeStep) +void SingleDofJoint::updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) { - mAcceleration += mVelocityChange / _timeStep; + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateImpulseID(_bodyImpulse); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateConstrainedTerms(double _timeStep) +{ + switch (mActuatorType) + { + case FORCE: + case PASSIVE: + case SERVO: + updateConstrainedTermsDynamic(_timeStep); + break; + case ACCELERATION: + case VELOCITY: + case LOCKED: + updateConstrainedTermsKinematic(_timeStep); + break; + default: + dterr << "Unsupported actuator type." << std::endl; + break; + } +} + +//============================================================================== +void SingleDofJoint::updateConstrainedTermsDynamic(double _timeStep) +{ + const double invTimeStep = 1.0 / _timeStep; + + mVelocity += mVelocityChange; + mAcceleration += mVelocityChange*invTimeStep; + mForce += mConstraintImpulse*invTimeStep; } //============================================================================== -void SingleDofJoint::updateForceWithImpulse(double _timeStep) +void SingleDofJoint::updateConstrainedTermsKinematic(double _timeStep) { - mForce += mConstraintImpulse / _timeStep; + mForce += mImpulse / _timeStep; } //============================================================================== diff --git a/dart/dynamics/SingleDofJoint.h b/dart/dynamics/SingleDofJoint.h index 59290bfb33fa3..4ca15ac47c399 100644 --- a/dart/dynamics/SingleDofJoint.h +++ b/dart/dynamics/SingleDofJoint.h @@ -58,7 +58,8 @@ class SingleDofJoint : public Joint virtual ~SingleDofJoint(); // Documentation inherited - DEPRECATED(4.1) virtual size_t getDof() const; + DEPRECATED(4.1) + virtual size_t getDof() const; // Documentation inherited virtual size_t getNumDofs() const; @@ -69,6 +70,25 @@ class SingleDofJoint : public Joint // Documentation inherited virtual size_t getIndexInSkeleton(size_t _index) const; + //---------------------------------------------------------------------------- + // Command + //---------------------------------------------------------------------------- + + // Documentation inherited + virtual void setCommand(size_t _index, double _command); + + // Documentation inherited + virtual double getCommand(size_t _index) const; + + // Documentation inherited + virtual void setCommands(const Eigen::VectorXd& _commands); + + // Documentation inherited + virtual Eigen::VectorXd getCommands() const; + + // Documentation inherited + virtual void resetCommands(); + //---------------------------------------------------------------------------- // Position //---------------------------------------------------------------------------- @@ -258,129 +278,161 @@ class SingleDofJoint : public Joint /// Get potential energy virtual double getPotentialEnergy() const; + // Documentation inherited + virtual Eigen::Vector6d getBodyConstraintWrench() const override; + protected: //---------------------------------------------------------------------------- - // Recursive dynamics algorithms + /// \{ \name Recursive dynamics routines //---------------------------------------------------------------------------- // Documentation inherited - virtual const math::Jacobian getLocalJacobian() const; + virtual const math::Jacobian getLocalJacobian() const override; // Documentation inherited - virtual const math::Jacobian getLocalJacobianTimeDeriv() const; + virtual const math::Jacobian getLocalJacobianTimeDeriv() const override; // Documentation inherited - virtual void addVelocityTo(Eigen::Vector6d& _vel); + virtual void addVelocityTo(Eigen::Vector6d& _vel) override; // Documentation inherited - virtual void setPartialAccelerationTo(Eigen::Vector6d& _partialAcceleration, - const Eigen::Vector6d& _childVelocity); + virtual void setPartialAccelerationTo( + Eigen::Vector6d& _partialAcceleration, + const Eigen::Vector6d& _childVelocity) override; // Documentation inherited - virtual void addAccelerationTo(Eigen::Vector6d& _acc); + virtual void addAccelerationTo(Eigen::Vector6d& _acc) override; // Documentation inherited - virtual void addVelocityChangeTo(Eigen::Vector6d& _velocityChange); + virtual void addVelocityChangeTo(Eigen::Vector6d& _velocityChange) override; // Documentation inherited - virtual void addChildArtInertiaTo(Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia); + virtual void addChildArtInertiaTo( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) override; // Documentation inherited virtual void addChildArtInertiaImplicitTo( Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia); + const Eigen::Matrix6d& _childArtInertia) override; // Documentation inherited - virtual void updateInvProjArtInertia(const Eigen::Matrix6d& _artInertia); + virtual void updateInvProjArtInertia( + const Eigen::Matrix6d& _artInertia) override; // Documentation inherited virtual void updateInvProjArtInertiaImplicit( const Eigen::Matrix6d& _artInertia, - double _timeStep); + double _timeStep) override; // Documentation inherited - virtual void addChildBiasForceTo(Eigen::Vector6d& _parentBiasForce, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce, - const Eigen::Vector6d& _childPartialAcc); + virtual void addChildBiasForceTo( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) override; // Documentation inherited - virtual void addChildBiasImpulseTo(Eigen::Vector6d& _parentBiasImpulse, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasImpulse); + virtual void addChildBiasImpulseTo( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse) override; // Documentation inherited virtual void updateTotalForce(const Eigen::Vector6d& _bodyForce, - double _timeStep); + double _timeStep) override; // Documentation inherited - virtual void updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse); + virtual void updateTotalImpulse( + const Eigen::Vector6d& _bodyImpulse) override; // Documentation inherited - virtual void resetTotalImpulses(); + virtual void resetTotalImpulses() override; // Documentation inherited - virtual void updateAcceleration(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _spatialAcc); + virtual void updateAcceleration( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc) override; + + // Documentation inherited + virtual void updateVelocityChange( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange) override; + + // Documentation inherited + virtual void updateForceID(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) override; // Documentation inherited - virtual void updateVelocityChange(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _velocityChange); + virtual void updateForceFD(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) override; // Documentation inherited - virtual void updateVelocityWithVelocityChange(); + virtual void updateImpulseID(const Eigen::Vector6d& _bodyImpulse) override; // Documentation inherited - virtual void updateAccelerationWithVelocityChange(double _timeStep); + virtual void updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) override; // Documentation inherited - virtual void updateForceWithImpulse(double _timeStep); + virtual void updateConstrainedTerms(double _timeStep) override; + + /// \} //---------------------------------------------------------------------------- - // Recursive algorithms for equations of motion + /// \{ \name Recursive algorithm routines for equations of motion //---------------------------------------------------------------------------- - /// Add child's bias force to parent's one + // Documentation inherited virtual void addChildBiasForceForInvMassMatrix( Eigen::Vector6d& _parentBiasForce, const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce); + const Eigen::Vector6d& _childBiasForce) override; - /// Add child's bias force to parent's one + // Documentation inherited virtual void addChildBiasForceForInvAugMassMatrix( Eigen::Vector6d& _parentBiasForce, const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce); + const Eigen::Vector6d& _childBiasForce) override; - /// + // Documentation inherited virtual void updateTotalForceForInvMassMatrix( - const Eigen::Vector6d& _bodyForce); + const Eigen::Vector6d& _bodyForce) override; // Documentation inherited - virtual void getInvMassMatrixSegment(Eigen::MatrixXd& _invMassMat, - const size_t _col, - const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _spatialAcc); + virtual void getInvMassMatrixSegment( + Eigen::MatrixXd& _invMassMat, + const size_t _col, + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc) override; // Documentation inherited - virtual void getInvAugMassMatrixSegment(Eigen::MatrixXd& _invMassMat, - const size_t _col, - const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _spatialAcc); + virtual void getInvAugMassMatrixSegment( + Eigen::MatrixXd& _invMassMat, + const size_t _col, + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc) override; // Documentation inherited - virtual void addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc); + virtual void addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc) override; // Documentation inherited virtual Eigen::VectorXd getSpatialToGeneralized( - const Eigen::Vector6d& _spatial); + const Eigen::Vector6d& _spatial) override; + + /// \} protected: // TODO(JS): Need? /// size_t mIndexInSkeleton; + /// Command + double mCommand; + //---------------------------------------------------------------------------- // Configuration //---------------------------------------------------------------------------- @@ -452,8 +504,8 @@ class SingleDofJoint : public Joint /// Change of generalized velocity double mVelocityChange; -// /// Generalized impulse -// double mImpulse; + /// Generalized impulse + double mImpulse; /// Generalized constraint impulse double mConstraintImpulse; @@ -505,6 +557,95 @@ class SingleDofJoint : public Joint double mInvMassMatrixSegment; private: + //---------------------------------------------------------------------------- + /// \{ \name Recursive dynamics routines + //---------------------------------------------------------------------------- + + void addChildArtInertiaToDynamic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia); + + void addChildArtInertiaToKinematic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia); + + void addChildArtInertiaImplicitToDynamic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia); + + void addChildArtInertiaImplicitToKinematic( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia); + + void updateInvProjArtInertiaDynamic( + const Eigen::Matrix6d& _artInertia); + + void updateInvProjArtInertiaKinematic( + const Eigen::Matrix6d& _artInertia); + + void updateInvProjArtInertiaImplicitDynamic( + const Eigen::Matrix6d& _artInertia, + double _timeStep); + + void updateInvProjArtInertiaImplicitKinematic( + const Eigen::Matrix6d& _artInertia, + double _timeStep); + + void addChildBiasForceToDynamic( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc); + + void addChildBiasForceToKinematic( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc); + + void addChildBiasImpulseToDynamic( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse); + + void addChildBiasImpulseToKinematic( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse); + + void updateTotalForceDynamic(const Eigen::Vector6d& _bodyForce, + double _timeStep); + + void updateTotalForceKinematic(const Eigen::Vector6d& _bodyForce, + double _timeStep); + + void updateTotalImpulseDynamic( + const Eigen::Vector6d& _bodyImpulse); + + void updateTotalImpulseKinematic( + const Eigen::Vector6d& _bodyImpulse); + + void updateAccelerationDynamic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc); + + void updateAccelerationKinematic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc); + + void updateVelocityChangeDynamic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange); + + void updateVelocityChangeKinematic( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange); + + void updateConstrainedTermsDynamic(double _timeStep); + + void updateConstrainedTermsKinematic(double _timeStep); + + /// \} }; } // namespace dynamics diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 380095c1dd9a2..ab3d545f061d0 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -473,6 +473,71 @@ GenCoordInfo Skeleton::getGenCoordInfo(size_t _index) const return mGenCoordInfos[_index]; } +//============================================================================== +void Skeleton::setCommand(size_t _index, double _command) +{ + assert(_index < getNumDofs()); + + mGenCoordInfos[_index].joint->setCommand(mGenCoordInfos[_index].localIndex, + _command); +} + +//============================================================================== +double Skeleton::getCommand(size_t _index) const +{ + assert(_index getCommand( + mGenCoordInfos[_index].localIndex); +} + +//============================================================================== +void Skeleton::setCommands(const Eigen::VectorXd& _commands) +{ + size_t index = 0; + size_t dof = 0; + Joint* joint; + + for (size_t i = 0; i < mBodyNodes.size(); ++i) + { + joint = mBodyNodes[i]->getParentJoint(); + + dof = joint->getNumDofs(); + + if (dof) + { + joint->setCommands(_commands.segment(index, dof)); + + index += dof; + } + } +} + +//============================================================================== +Eigen::VectorXd Skeleton::getCommands() const +{ + size_t index = 0; + size_t dof = getNumDofs(); + Eigen::VectorXd commands(dof); + + for (size_t i = 0; i < dof; ++i) + { + commands[index++] + = mGenCoordInfos[i].joint->getCommand(mGenCoordInfos[i].localIndex); + } + + assert(index == dof); + + return commands; +} + +//============================================================================== +void Skeleton::resetCommands() +{ + for (auto& bodyNode : mBodyNodes) + bodyNode->getParentJoint()->resetCommands(); +} + //============================================================================== void Skeleton::setPosition(size_t _index, double _position) { @@ -1041,7 +1106,7 @@ void Skeleton::computeForwardKinematics(bool _updateTransforms, for (std::vector::iterator it = mBodyNodes.begin(); it != mBodyNodes.end(); ++it) { - (*it)->updateAcceleration(); + (*it)->updateAccelerationID(); } } @@ -1222,8 +1287,8 @@ void Skeleton::updateMassMatrix() if (getNumDofs() == 0) return; - assert(mM.cols() == math::castUIntToInt(getNumDofs()) - && mM.rows() == math::castUIntToInt(getNumDofs())); + assert(static_cast(mM.cols()) == getNumDofs() + && static_cast(mM.rows()) == getNumDofs()); mM.setZero(); @@ -1275,8 +1340,8 @@ void Skeleton::updateAugMassMatrix() if (getNumDofs() == 0) return; - assert(mAugM.cols() == math::castUIntToInt(getNumDofs()) - && mAugM.rows() == math::castUIntToInt(getNumDofs())); + assert(static_cast(mAugM.cols()) == getNumDofs() + && static_cast(mAugM.rows()) == getNumDofs()); mAugM.setZero(); @@ -1329,8 +1394,8 @@ void Skeleton::updateInvMassMatrix() if (getNumDofs() == 0) return; - assert(mInvM.cols() == math::castUIntToInt(getNumDofs()) - && mInvM.rows() == math::castUIntToInt(getNumDofs())); + assert(static_cast(mInvM.cols()) == getNumDofs() + && static_cast(mInvM.rows()) == getNumDofs()); // We don't need to set mInvM as zero matrix as long as the below is correct // mInvM.setZero(); @@ -1395,8 +1460,8 @@ void Skeleton::updateInvAugMassMatrix() if (getNumDofs() == 0) return; - assert(mInvAugM.cols() == math::castUIntToInt(getNumDofs()) - && mInvAugM.rows() == math::castUIntToInt(getNumDofs())); + assert(static_cast(mInvAugM.cols()) == getNumDofs() + && static_cast(mInvAugM.rows()) == getNumDofs()); // We don't need to set mInvM as zero matrix as long as the below is correct // mInvM.setZero(); @@ -1456,7 +1521,7 @@ void Skeleton::updateCoriolisForces() if (getNumDofs() == 0) return; - assert(mCvec.size() == math::castUIntToInt(getNumDofs())); + assert(static_cast(mCvec.size()) == getNumDofs()); mCvec.setZero(); @@ -1487,7 +1552,7 @@ void Skeleton::updateGravityForces() if (getNumDofs() == 0) return; - assert(mG.size() == math::castUIntToInt(getNumDofs())); + assert(static_cast(mG.size()) == getNumDofs()); // Calcualtion mass matrix, M mG.setZero(); @@ -1512,7 +1577,7 @@ void Skeleton::updateCoriolisAndGravityForces() if (getNumDofs() == 0) return; - assert(mCg.size() == math::castUIntToInt(getNumDofs())); + assert(static_cast(mCg.size()) == getNumDofs()); mCg.setZero(); for (std::vector::iterator it = mBodyNodes.begin(); @@ -1542,7 +1607,7 @@ void Skeleton::updateExternalForces() if (getNumDofs() == 0) return; - assert(mFext.size() == math::castUIntToInt(getNumDofs())); + assert(static_cast(mFext.size()) == getNumDofs()); // Clear external force. mFext.setZero(); @@ -1660,32 +1725,29 @@ void Skeleton::computeForwardDynamicsRecursionPartA() void Skeleton::computeForwardDynamicsRecursionPartB() { // Backward recursion -// if (mIsArticulatedInertiaDirty) + if (mIsArticulatedInertiaDirty) { - for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); - it != mBodyNodes.rend(); ++it) + for (auto it = mBodyNodes.rbegin(); it != mBodyNodes.rend(); ++it) { (*it)->updateArtInertia(mTimeStep); (*it)->updateBiasForce(mGravity, mTimeStep); } + // TODO: Replace with std::make_reverse_iterator when we migrate to C++14 mIsArticulatedInertiaDirty = false; } -// else -// { -// for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); -// it != mBodyNodes.rend(); ++it) -// { -// (*it)->updateBiasForce(mGravity, mTimeStep); -// } -// } + else + { + for (auto it = mBodyNodes.rbegin(); it != mBodyNodes.rend(); ++it) + (*it)->updateBiasForce(mGravity, mTimeStep); + } // Forward recursion - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) + for (auto& bodyNode : mBodyNodes) { - (*it)->updateJointAndBodyAcceleration(); - (*it)->updateTransmittedWrench(); + bodyNode->updateAccelerationFD(); + bodyNode->updateTransmittedForceFD(); + bodyNode->updateJointForceFD(mTimeStep, true, true); } } @@ -1709,7 +1771,7 @@ void Skeleton::computeInverseDynamicsRecursionA() (*it)->updateTransform(); (*it)->updateVelocity(); (*it)->updatePartialAcceleration(); - (*it)->updateAcceleration(); + (*it)->updateAccelerationID(); } mIsArticulatedInertiaDirty = true; @@ -1733,57 +1795,35 @@ void Skeleton::computeInverseDynamicsRecursionA() //============================================================================== void Skeleton::computeInverseDynamicsRecursionB(bool _withExternalForces, - bool _withDampingForces) + bool _withDampingForces, + bool _withSpringForces) { // Skip immobile or 0-dof skeleton if (getNumDofs() == 0) return; // Backward recursion - for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); - it != mBodyNodes.rend(); ++it) + for (auto it = mBodyNodes.rbegin(); it != mBodyNodes.rend(); ++it) { - (*it)->updateBodyWrench(mGravity, _withExternalForces); - (*it)->updateGeneralizedForce(_withDampingForces); + (*it)->updateTransmittedForceID(mGravity, _withExternalForces); + (*it)->updateJointForceID(mTimeStep, + _withDampingForces, + _withSpringForces); } } -//============================================================================== -//void Skeleton::computeHybridDynamics() -//{ -// dterr << "Not implemented yet.\n"; -//} - -//============================================================================== -//void Skeleton::computeHybridDynamicsRecursionA() -//{ -// dterr << "Not implemented yet.\n"; -//} - -//============================================================================== -//void Skeleton::computeHybridDynamicsRecursionB() -//{ -// dterr << "Not implemented yet.\n"; -//} - //============================================================================== void Skeleton::clearExternalForces() { - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) - { - (*it)->clearExternalForces(); - } + for (auto& bodyNode : mBodyNodes) + bodyNode->clearExternalForces(); } //============================================================================== void Skeleton::clearConstraintImpulses() { - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) - { - (*it)->clearConstraintImpulse(); - } + for (auto& bodyNode : mBodyNodes) + bodyNode->clearConstraintImpulse(); } //============================================================================== @@ -1845,9 +1885,10 @@ void Skeleton::updateBiasImpulse(BodyNode* _bodyNode, } //============================================================================== -void Skeleton::updateBiasImpulse( - BodyNode* _bodyNode1, const Eigen::Vector6d& _imp1, - BodyNode* _bodyNode2, const Eigen::Vector6d& _imp2) +void Skeleton::updateBiasImpulse(BodyNode* _bodyNode1, + const Eigen::Vector6d& _imp1, + BodyNode* _bodyNode2, + const Eigen::Vector6d& _imp2) { // Assertions assert(_bodyNode1 != NULL); @@ -1876,17 +1917,11 @@ void Skeleton::updateBiasImpulse( std::vector::reverse_iterator it2 = std::find(mBodyNodes.rbegin(), mBodyNodes.rend(), _bodyNode2); - std::vector::reverse_iterator it; - if (it1 < it2) - it = it1; - else - it = it2; + std::vector::reverse_iterator it = std::min(it1, it2); // Prepare cache data for (; it != mBodyNodes.rend(); ++it) - { (*it)->updateBiasImpulse(); - } _bodyNode1->mConstraintImpulse.setZero(); _bodyNode2->mConstraintImpulse.setZero(); @@ -1930,11 +1965,8 @@ void Skeleton::updateBiasImpulse(SoftBodyNode* _softBodyNode, //============================================================================== void Skeleton::updateVelocityChange() { - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) - { - (*it)->updateJointVelocityChange(); - } + for (auto& bodyNode : mBodyNodes) + bodyNode->updateVelocityChangeFD(); } //============================================================================== @@ -1959,8 +1991,7 @@ void Skeleton::computeImpulseForwardDynamics() // Backward recursion if (mIsArticulatedInertiaDirty) { - for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); - it != mBodyNodes.rend(); ++it) + for (auto it = mBodyNodes.rbegin(); it != mBodyNodes.rend(); ++it) { (*it)->updateArtInertia(mTimeStep); (*it)->updateBiasImpulse(); @@ -1970,32 +2001,17 @@ void Skeleton::computeImpulseForwardDynamics() } else { - for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); - it != mBodyNodes.rend(); ++it) - { + for (auto it = mBodyNodes.rbegin(); it != mBodyNodes.rend(); ++it) (*it)->updateBiasImpulse(); - } } // Forward recursion - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) + for (auto& bodyNode : mBodyNodes) { - (*it)->updateJointVelocityChange(); -// (*it)->updateBodyVelocityChange(); - (*it)->updateBodyImpForceFwdDyn(); - } - - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) - { - // 1. dq = dq + del_dq - // 2. ddq = ddq + del_dq / dt - // 3. tau = tau + imp / dt - (*it)->updateConstrainedJointAndBodyAcceleration(mTimeStep); - - // 4. F(+) = F(-) + ImpF / dt - (*it)->updateConstrainedTransmittedForce(mTimeStep); + bodyNode->updateVelocityChangeFD(); + bodyNode->updateTransmittedImpulse(); + bodyNode->updateJointImpulseFD(); + bodyNode->updateConstrainedTerms(mTimeStep); } } diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 4ba13d3441e77..b41ec9312cff4 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -208,7 +208,8 @@ class Skeleton //---------------------------------------------------------------------------- /// Return degrees of freedom of this skeleton - DEPRECATED(4.1) size_t getDof() const; + DEPRECATED(4.1) + size_t getDof() const; /// Return degrees of freedom of this skeleton size_t getNumDofs() const; @@ -216,6 +217,27 @@ class Skeleton /// GenCoordInfo getGenCoordInfo(size_t _index) const; + //---------------------------------------------------------------------------- + /// \{ \name Command + //---------------------------------------------------------------------------- + + /// Set a single command + virtual void setCommand(size_t _index, double _command); + + /// Set a sinlge command + virtual double getCommand(size_t _index) const; + + /// Set commands + virtual void setCommands(const Eigen::VectorXd& _commands); + + /// Get commands + virtual Eigen::VectorXd getCommands() const; + + /// Set zero all the positions + virtual void resetCommands(); + + /// \} + //---------------------------------------------------------------------------- // Position //---------------------------------------------------------------------------- @@ -354,13 +376,15 @@ class Skeleton //---------------------------------------------------------------------------- /// - DEPRECATED(4.2) void setConstraintImpulses(const Eigen::VectorXd& _impulses); + DEPRECATED(4.2) + void setConstraintImpulses(const Eigen::VectorXd& _impulses); /// Set constraint impulses applying to joint void setJointConstraintImpulses(const Eigen::VectorXd& _impulses); /// - DEPRECATED(4.2) Eigen::VectorXd getConstraintImpulses() const; + DEPRECATED(4.2) + Eigen::VectorXd getConstraintImpulses() const; /// Return constraint impulses applied to joint Eigen::VectorXd getJointConstraintImpulses() const; @@ -405,9 +429,6 @@ class Skeleton void computeInverseDynamics(bool _withExternalForces = false, bool _withDampingForces = false); - /// Compute hybrid dynamics -// void computeHybridDynamics(); - //---------------------------------------------------------------------------- // Impulse-based dynamics algorithms //---------------------------------------------------------------------------- @@ -416,19 +437,19 @@ class Skeleton /// (b) generalized constraints on Joint void clearConstraintImpulses(); - // TODO(JS): To be deprecated /// Set constraint force vector. - DEPRECATED(4.2) void setConstraintForceVector(const Eigen::VectorXd& _Fc); + DEPRECATED(4.2) + void setConstraintForceVector(const Eigen::VectorXd& _Fc); /// Update bias impulses void updateBiasImpulse(BodyNode* _bodyNode); - /// Update bias impulses due to impulse[_imp] on body node [_bodyNode] + /// \brief Update bias impulses due to impulse [_imp] on body node [_bodyNode] /// \param _bodyNode Body node contraint impulse, _imp, is applied /// \param _imp Constraint impulse expressed in body frame of _bodyNode void updateBiasImpulse(BodyNode* _bodyNode, const Eigen::Vector6d& _imp); - /// Update bias impulses due to impulse[_imp] on body node [_bodyNode] + /// \brief Update bias impulses due to impulse [_imp] on body node [_bodyNode] /// \param _bodyNode Body node contraint impulse, _imp1, is applied /// \param _imp Constraint impulse expressed in body frame of _bodyNode1 /// \param _bodyNode Body node contraint impulse, _imp2, is applied @@ -436,12 +457,12 @@ class Skeleton void updateBiasImpulse(BodyNode* _bodyNode1, const Eigen::Vector6d& _imp1, BodyNode* _bodyNode2, const Eigen::Vector6d& _imp2); - /// Update bias impulses due to impulse[_imp] on body node [_bodyNode] + /// \brief Update bias impulses due to impulse[_imp] on body node [_bodyNode] void updateBiasImpulse(SoftBodyNode* _softBodyNode, PointMass* _pointMass, const Eigen::Vector3d& _imp); - /// Update velocity changes in body nodes and joints due to applied + /// \brief Update velocity changes in body nodes and joints due to applied /// impulse void updateVelocityChange(); @@ -459,9 +480,6 @@ class Skeleton /// Compute impulse-based inverse dynamics // void computeImpulseInverseDynamics() {} - /// Compute impulse-based hybrid dynamics -// void computeImpulseHybridDynamics() {} - //---------------------------------------------------------------------------- // Equations of Motion //---------------------------------------------------------------------------- @@ -483,28 +501,32 @@ class Skeleton /// Get Coriolis force vector of the skeleton. /// \remarks Please use getCoriolisForces() instead. - DEPRECATED(4.2) const Eigen::VectorXd& getCoriolisForceVector(); + DEPRECATED(4.2) + const Eigen::VectorXd& getCoriolisForceVector(); /// Get Coriolis force vector of the skeleton. const Eigen::VectorXd& getCoriolisForces(); /// Get gravity force vector of the skeleton. /// \remarks Please use getGravityForces() instead. - DEPRECATED(4.2) const Eigen::VectorXd& getGravityForceVector(); + DEPRECATED(4.2) + const Eigen::VectorXd& getGravityForceVector(); /// Get gravity force vector of the skeleton. const Eigen::VectorXd& getGravityForces(); /// Get combined vector of Coriolis force and gravity force of the skeleton. /// \remarks Please use getCoriolisAndGravityForces() instead. - DEPRECATED(4.2) const Eigen::VectorXd& getCombinedVector(); + DEPRECATED(4.2) + const Eigen::VectorXd& getCombinedVector(); /// Get combined vector of Coriolis force and gravity force of the skeleton. const Eigen::VectorXd& getCoriolisAndGravityForces(); /// Get external force vector of the skeleton. /// \remarks Please use getExternalForces() instead. - DEPRECATED(4.2) const Eigen::VectorXd& getExternalForceVector(); + DEPRECATED(4.2) + const Eigen::VectorXd& getExternalForceVector(); /// Get external force vector of the skeleton. const Eigen::VectorXd& getExternalForces(); @@ -514,7 +536,8 @@ class Skeleton /// Get constraint force vector. /// \remarks Please use getConstraintForces() instead. - DEPRECATED(4.2) const Eigen::VectorXd& getConstraintForceVector(); + DEPRECATED(4.2) + const Eigen::VectorXd& getConstraintForceVector(); /// Get constraint force vector. const Eigen::VectorXd& getConstraintForces(); @@ -578,13 +601,8 @@ class Skeleton /// Compute recursion part B of inverse dynamics void computeInverseDynamicsRecursionB(bool _withExternalForces = false, - bool _withDampingForces = false); - - /// Compute recursion part A of hybrid dynamics -// void computeHybridDynamicsRecursionA(); - - /// Compute recursion part B of hybrid dynamics -// void computeHybridDynamicsRecursionB(); + bool _withDampingForces = false, + bool _withSpringForces = false); //---------------------------------------------------------------------------- // Friendship @@ -607,31 +625,35 @@ class Skeleton /// Update Coriolis force vector of the skeleton. /// \remarks Please use updateCoriolisForces() instead. - DEPRECATED(4.2) virtual void updateCoriolisForceVector(); + DEPRECATED(4.2) + virtual void updateCoriolisForceVector(); /// Update Coriolis force vector of the skeleton. void updateCoriolisForces(); /// Update gravity force vector of the skeleton. /// \remarks Please use updateGravityForces() instead. - DEPRECATED(4.2) virtual void updateGravityForceVector(); + DEPRECATED(4.2) + virtual void updateGravityForceVector(); /// Update gravity force vector of the skeleton. void updateGravityForces(); /// Update combined vector of the skeletong. /// \remarks Please use updateCoriolisAndGravityForces() instead. - DEPRECATED(4.2) virtual void updateCombinedVector(); + DEPRECATED(4.2) + virtual void updateCombinedVector(); /// Update combined vector of the skeletong. void updateCoriolisAndGravityForces(); - /// update external force vector to generalized torques. + /// update external force vector to generalized forces. /// \remarks Please use updateExternalForces() instead. - DEPRECATED(4.2) virtual void updateExternalForceVector(); + DEPRECATED(4.2) + virtual void updateExternalForceVector(); // TODO(JS): Not implemented yet - /// update external force vector to generalized torques. + /// update external force vector to generalized forces. void updateExternalForces(); // /// Update damping force vector. diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index a7da7e1282583..8e5532fdcf787 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -219,9 +219,9 @@ void SoftBodyNode::addFace(const Eigen::Vector3i& _face) assert(_face[0] != _face[1]); assert(_face[1] != _face[2]); assert(_face[2] != _face[0]); - assert(0 <= _face[0] && _face[0] < math::castUIntToInt(mPointMasses.size())); - assert(0 <= _face[1] && _face[1] < math::castUIntToInt(mPointMasses.size())); - assert(0 <= _face[2] && _face[2] < math::castUIntToInt(mPointMasses.size())); + assert(0 <= _face[0] && static_cast(_face[0]) < mPointMasses.size()); + assert(0 <= _face[1] && static_cast(_face[1]) < mPointMasses.size()); + assert(0 <= _face[2] && static_cast(_face[2]) < mPointMasses.size()); mFaces.push_back(_face); } @@ -275,20 +275,20 @@ void SoftBodyNode::updatePartialAcceleration() } //============================================================================== -void SoftBodyNode::updateAcceleration() +void SoftBodyNode::updateAccelerationID() { - BodyNode::updateAcceleration(); + BodyNode::updateAccelerationID(); for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateAcceleration(); + mPointMasses.at(i)->updateAccelerationID(); } //============================================================================== -void SoftBodyNode::updateBodyWrench(const Eigen::Vector3d& _gravity, - bool _withExternalForces) +void SoftBodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, + bool _withExternalForces) { - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateBodyForce(_gravity, _withExternalForces); + for (auto& pointMass : mPointMasses) + pointMass->updateTransmittedForceID(_gravity, _withExternalForces); // Gravity force if (mGravityMode == true) @@ -313,42 +313,60 @@ void SoftBodyNode::updateBodyWrench(const Eigen::Vector3d& _gravity, mF -= math::dad(mV, mI * mV); // - for (std::vector::iterator iChildBody = mChildBodyNodes.begin(); - iChildBody != mChildBodyNodes.end(); ++iChildBody) + for (const auto& childBodyNode : mChildBodyNodes) { - Joint* childJoint = (*iChildBody)->getParentJoint(); + Joint* childJoint = childBodyNode->getParentJoint(); assert(childJoint != NULL); mF += math::dAdInvT(childJoint->getLocalTransform(), - (*iChildBody)->getBodyForce()); + childBodyNode->getBodyForce()); + } + for (auto& pointMass : mPointMasses) + { + mF.head<3>() += pointMass->mX.cross(pointMass->mF); + mF.tail<3>() += pointMass->mF; } -// for (size_t i = 0; i < mPointMasses.size(); i++) -// { -// mF.head<3>() += mPointMasses[i]->mX.cross(mPointMasses[i]->mF); -// mF.tail<3>() += mPointMasses[i]->mF; -// } - - // TODO(JS): mWrench and mF are duplicated. Remove one of them. - mParentJoint->mWrench = mF; // Verification assert(!math::isNan(mF)); } //============================================================================== -void SoftBodyNode::updateGeneralizedForce(bool _withDampingForces) +void SoftBodyNode::updateJointForceID(double _timeStep, + double _withDampingForces, + double _withSpringForces) { for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateGeneralizedForce(_withDampingForces); + mPointMasses.at(i)->updateJointForceID(_timeStep, + _withDampingForces, + _withSpringForces); - BodyNode::updateGeneralizedForce(_withDampingForces); + BodyNode::updateJointForceID(_timeStep, + _withDampingForces, + _withSpringForces); +} + +//============================================================================== +void SoftBodyNode::updateJointForceFD(double _timeStep, + double _withDampingForces, + double _withSpringForces) +{ + BodyNode::updateJointForceFD(_timeStep, + _withDampingForces, + _withSpringForces); +} + +//============================================================================== +void SoftBodyNode::updateJointImpulseFD() +{ + BodyNode::updateJointImpulseFD(); } //============================================================================== void SoftBodyNode::updateArtInertia(double _timeStep) { - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateArticulatedInertia(_timeStep); + for (auto& pointMass : mPointMasses) + pointMass->updateArtInertiaFD(_timeStep); assert(mParentJoint != NULL); @@ -357,20 +375,20 @@ void SoftBodyNode::updateArtInertia(double _timeStep) mArtInertiaImplicit = mI; // and add child articulated body inertia - for (std::vector::const_iterator it = mChildBodyNodes.begin(); - it != mChildBodyNodes.end(); ++it) + for (const auto& child : mChildBodyNodes) { - (*it)->getParentJoint()->addChildArtInertiaTo( - mArtInertia, (*it)->mArtInertia); - (*it)->getParentJoint()->addChildArtInertiaImplicitTo( - mArtInertiaImplicit, (*it)->mArtInertiaImplicit); + Joint* childJoint = child->getParentJoint(); + + childJoint->addChildArtInertiaTo(mArtInertia, child->mArtInertia); + childJoint->addChildArtInertiaImplicitTo(mArtInertiaImplicit, + child->mArtInertiaImplicit); } // - for (size_t i = 0; i < mPointMasses.size(); i++) + for (const auto& pointMass : mPointMasses) { - _addPiToArtInertia(mPointMasses[i]->mX, mPointMasses[i]->mPi); - _addPiToArtInertiaImplicit(mPointMasses[i]->mX, mPointMasses[i]->mImplicitPi); + _addPiToArtInertia(pointMass->mX, pointMass->mPi); + _addPiToArtInertiaImplicit(pointMass->mX, pointMass->mImplicitPi); } // Verification @@ -390,8 +408,8 @@ void SoftBodyNode::updateArtInertia(double _timeStep) void SoftBodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, double _timeStep) { - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateBiasForce(_timeStep, _gravity); + for (auto& pointMass : mPointMasses) + pointMass->updateBiasForceFD(_timeStep, _gravity); // Gravity force if (mGravityMode == true) @@ -406,20 +424,21 @@ void SoftBodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, assert(!math::isNan(mBiasForce)); // And add child bias force - for (std::vector::const_iterator it = mChildBodyNodes.begin(); - it != mChildBodyNodes.end(); ++it) + for (const auto& childBodyNode : mChildBodyNodes) { - (*it)->getParentJoint()->addChildBiasForceTo(mBiasForce, - (*it)->mArtInertiaImplicit, - (*it)->mBiasForce, - (*it)->mPartialAcceleration); + Joint* childJoint = childBodyNode->getParentJoint(); + + childJoint->addChildBiasForceTo(mBiasForce, + childBodyNode->mArtInertiaImplicit, + childBodyNode->mBiasForce, + childBodyNode->mPartialAcceleration); } // - for (size_t i = 0; i < mPointMasses.size(); i++) + for (const auto& pointMass : mPointMasses) { - mBiasForce.head<3>() += mPointMasses[i]->mX.cross(mPointMasses[i]->mBeta); - mBiasForce.tail<3>() += mPointMasses[i]->mBeta; + mBiasForce.head<3>() += pointMass->mX.cross(pointMass->mBeta); + mBiasForce.tail<3>() += pointMass->mBeta; } // Verifycation @@ -432,47 +451,46 @@ void SoftBodyNode::updateBiasForce(const Eigen::Vector3d& _gravity, } //============================================================================== -void SoftBodyNode::updateJointAndBodyAcceleration() +void SoftBodyNode::updateAccelerationFD() { - BodyNode::updateJointAndBodyAcceleration(); + BodyNode::updateAccelerationFD(); - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateJointAndBodyAcceleration(); + for (auto& pointMass : mPointMasses) + pointMass->updateAccelerationFD(); } //============================================================================== -void SoftBodyNode::updateTransmittedWrench() +void SoftBodyNode::updateTransmittedForceFD() { - BodyNode::updateTransmittedWrench(); + BodyNode::updateTransmittedForceFD(); - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateTransmittedForce(); + for (auto& pointMass : mPointMasses) + pointMass->updateTransmittedForce(); } //============================================================================== void SoftBodyNode::updateBiasImpulse() { - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateBiasImpulse(); + for (auto& pointMass : mPointMasses) + pointMass->updateBiasImpulseFD(); // Update impulsive bias force mBiasImpulse = -mConstraintImpulse; -// assert(mImpFext == Eigen::Vector6d::Zero()); // And add child bias impulse - for (std::vector::const_iterator it = mChildBodyNodes.begin(); - it != mChildBodyNodes.end(); ++it) + for (auto& childBodyNode : mChildBodyNodes) { - (*it)->getParentJoint()->addChildBiasImpulseTo(mBiasImpulse, - (*it)->mArtInertia, - (*it)->mBiasImpulse); + Joint* childJoint = childBodyNode->getParentJoint(); + + childJoint->addChildBiasImpulseTo(mBiasImpulse, + childBodyNode->mArtInertia, + childBodyNode->mBiasImpulse); } - // TODO(JS): - for (size_t i = 0; i < mPointMasses.size(); i++) + for (auto& pointMass : mPointMasses) { - mBiasImpulse.head<3>() += mPointMasses[i]->mX.cross(mPointMasses[i]->mImpBeta); - mBiasImpulse.tail<3>() += mPointMasses[i]->mImpBeta; + mBiasImpulse.head<3>() += pointMass->mX.cross(pointMass->mImpBeta); + mBiasImpulse.tail<3>() += pointMass->mImpBeta; } // Verification @@ -483,45 +501,30 @@ void SoftBodyNode::updateBiasImpulse() } //============================================================================== -void SoftBodyNode::updateJointVelocityChange() +void SoftBodyNode::updateVelocityChangeFD() { - BodyNode::updateJointVelocityChange(); + BodyNode::updateVelocityChangeFD(); - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateJointVelocityChange(); + for (auto& pointMass : mPointMasses) + pointMass->updateVelocityChangeFD(); } //============================================================================== -//void SoftBodyNode::updateBodyVelocityChange() -//{ -// BodyNode::updateBodyVelocityChange(); - -// for (size_t i = 0; i < mPointMasses.size(); ++i) -// mPointMasses.at(i)->updateBodyVelocityChange(); -//} - -//============================================================================== -void SoftBodyNode::updateBodyImpForceFwdDyn() +void SoftBodyNode::updateTransmittedImpulse() { - BodyNode::updateBodyImpForceFwdDyn(); + BodyNode::updateTransmittedImpulse(); - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateBodyImpForceFwdDyn(); + for (auto& pointMass : mPointMasses) + pointMass->updateTransmittedImpulse(); } //============================================================================== -void SoftBodyNode::updateConstrainedJointAndBodyAcceleration(double _timeStep) +void SoftBodyNode::updateConstrainedTerms(double _timeStep) { - BodyNode::updateConstrainedJointAndBodyAcceleration(_timeStep); + BodyNode::updateConstrainedTerms(_timeStep); - for (size_t i = 0; i < mPointMasses.size(); ++i) - mPointMasses.at(i)->updateConstrainedJointAndBodyAcceleration(_timeStep); -} - -//============================================================================== -void SoftBodyNode::updateConstrainedTransmittedForce(double _timeStep) -{ - BodyNode::updateConstrainedTransmittedForce(_timeStep); + for (auto& pointMass : mPointMasses) + pointMass->updateConstrainedTermsFD(_timeStep); } //============================================================================== diff --git a/dart/dynamics/SoftBodyNode.h b/dart/dynamics/SoftBodyNode.h index a55c388088cf0..2c7d27f45bb0e 100644 --- a/dart/dynamics/SoftBodyNode.h +++ b/dart/dynamics/SoftBodyNode.h @@ -136,60 +136,69 @@ class SoftBodyNode : public BodyNode // Documentation inherited. // virtual void aggregatePointMassGenCoords(std::vector* _genCoords); - // Documentation inherited. - virtual void updateTransform(); + //---------------------------------------------------------------------------- + /// \{ \name Recursive dynamics routines + //---------------------------------------------------------------------------- // Documentation inherited. - virtual void updateVelocity(); + virtual void updateTransform() override; // Documentation inherited. - virtual void updatePartialAcceleration(); + virtual void updateVelocity() override; // Documentation inherited. - virtual void updateAcceleration(); + virtual void updatePartialAcceleration() override; // Documentation inherited. - virtual void updateBodyWrench(const Eigen::Vector3d& _gravity, - bool _withExternalForces = false); + virtual void updateArtInertia(double _timeStep) override; // Documentation inherited. - virtual void updateGeneralizedForce(bool _withDampingForces = false); + virtual void updateBiasForce(const Eigen::Vector3d& _gravity, + double _timeStep) override; // Documentation inherited. - virtual void updateArtInertia(double _timeStep); + virtual void updateBiasImpulse() override; // Documentation inherited. - virtual void updateBiasForce(const Eigen::Vector3d& _gravity, - double _timeStep); + virtual void updateAccelerationID() override; // Documentation inherited. - virtual void updateJointAndBodyAcceleration(); + virtual void updateAccelerationFD() override; // Documentation inherited. - virtual void updateTransmittedWrench(); + virtual void updateVelocityChangeFD() override; - //---------------------------------------------------------------------------- - // Impulse based dynamics - //---------------------------------------------------------------------------- + // Documentation inherited. + virtual void updateTransmittedForceID( + const Eigen::Vector3d& _gravity, + bool _withExternalForces = false) override; // Documentation inherited. - virtual void updateBiasImpulse(); + virtual void updateTransmittedForceFD() override; // Documentation inherited. - virtual void updateJointVelocityChange(); + virtual void updateTransmittedImpulse() override; // Documentation inherited. -// virtual void updateBodyVelocityChange(); + virtual void updateJointForceID(double _timeStep, + double _withDampingForces, + double _withSpringForces) override; // Documentation inherited. - virtual void updateBodyImpForceFwdDyn(); + virtual void updateJointForceFD(double _timeStep, + double _withDampingForces, + double _withSpringForces) override; // Documentation inherited. - virtual void updateConstrainedJointAndBodyAcceleration(double _timeStep); + virtual void updateJointImpulseFD() override; // Documentation inherited. - virtual void updateConstrainedTransmittedForce(double _timeStep); + virtual void updateConstrainedTerms(double _timeStep) override; + + /// \} + //---------------------------------------------------------------------------- + /// \{ \name Equations of motion related routines //---------------------------------------------------------------------------- // Documentation inherited. @@ -233,6 +242,8 @@ class SoftBodyNode : public BodyNode // Documentation inherited. virtual void aggregateExternalForces(Eigen::VectorXd* _Fext); + /// \} + // Documentation inherited. virtual void clearExternalForces(); diff --git a/dart/dynamics/TranslationalJoint.h b/dart/dynamics/TranslationalJoint.h index ec115ef33496b..3b4544293cbd2 100644 --- a/dart/dynamics/TranslationalJoint.h +++ b/dart/dynamics/TranslationalJoint.h @@ -54,12 +54,6 @@ class TranslationalJoint : public MultiDofJoint<3> /// Destructor virtual ~TranslationalJoint(); - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForces; - } - protected: // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/UniversalJoint.h b/dart/dynamics/UniversalJoint.h index 1398eed47b42b..daa8997ad9c0a 100644 --- a/dart/dynamics/UniversalJoint.h +++ b/dart/dynamics/UniversalJoint.h @@ -70,12 +70,6 @@ class UniversalJoint : public MultiDofJoint<2> /// const Eigen::Vector3d& getAxis2() const; - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench - mJacobian * mForces; - } - protected: // Documentation inherited virtual void updateLocalTransform(); diff --git a/dart/dynamics/WeldJoint.h b/dart/dynamics/WeldJoint.h index 2410e9872f19b..cbdfe990441ec 100644 --- a/dart/dynamics/WeldJoint.h +++ b/dart/dynamics/WeldJoint.h @@ -62,12 +62,6 @@ class WeldJoint : public ZeroDofJoint // Documentation inherited virtual void setTransformFromChildBodyNode(const Eigen::Isometry3d& _T); - // Documentation inherited - virtual Eigen::Vector6d getBodyConstraintWrench() const - { - return mWrench; - } - protected: //---------------------------------------------------------------------------- // Recursive algorithms diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index 229626be85146..90d0e7bb48712 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -83,6 +83,39 @@ size_t ZeroDofJoint::getIndexInSkeleton(size_t _index) const return 0; } +//============================================================================== +void ZeroDofJoint::setCommand(size_t /*_index*/, double /*_command*/) +{ + // Do nothing +} + +//============================================================================== +double ZeroDofJoint::getCommand(size_t _index) const +{ + dterr << "[ZeroDofJoint::getCommand]: index[" << _index << "] out of range" + << std::endl; + + return 0.0; +} + +//============================================================================== +void ZeroDofJoint::setCommands(const Eigen::VectorXd& /*_commands*/) +{ + // Do nothing +} + +//============================================================================== +Eigen::VectorXd ZeroDofJoint::getCommands() const +{ + return Eigen::Matrix(); +} + +//============================================================================== +void ZeroDofJoint::resetCommands() +{ + // Do nothing +} + //============================================================================== void ZeroDofJoint::setPosition(size_t _index, double _position) { @@ -395,6 +428,13 @@ double ZeroDofJoint::getPotentialEnergy() const return 0.0; } +//============================================================================== +Eigen::Vector6d ZeroDofJoint::getBodyConstraintWrench() const +{ + assert(mChildBodyNode); + return mChildBodyNode->getBodyForce(); +} + //============================================================================== const math::Jacobian ZeroDofJoint::getLocalJacobian() const { @@ -434,8 +474,9 @@ void ZeroDofJoint::addAccelerationTo(Eigen::Vector6d& /*_acc*/) } //============================================================================== -void ZeroDofJoint::addChildArtInertiaTo(Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia) +void ZeroDofJoint::addChildArtInertiaTo( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) { // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. @@ -444,8 +485,7 @@ void ZeroDofJoint::addChildArtInertiaTo(Eigen::Matrix6d& _parentArtInertia, //============================================================================== void ZeroDofJoint::addChildArtInertiaImplicitTo( - Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia) + Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. @@ -453,7 +493,8 @@ void ZeroDofJoint::addChildArtInertiaImplicitTo( } //============================================================================== -void ZeroDofJoint::updateInvProjArtInertia(const Eigen::Matrix6d& /*_artInertia*/) +void ZeroDofJoint::updateInvProjArtInertia( + const Eigen::Matrix6d& /*_artInertia*/) { // Do nothing } @@ -467,15 +508,16 @@ void ZeroDofJoint::updateInvProjArtInertiaImplicit( } //============================================================================== -void ZeroDofJoint::addChildBiasForceTo(Eigen::Vector6d& _parentBiasForce, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce, - const Eigen::Vector6d& _childPartialAcc) +void ZeroDofJoint::addChildBiasForceTo( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) { // Add child body's bias force to parent body's bias force. Note that mT // should be updated. _parentBiasForce += math::dAdInvT(mT, _childBiasForce - + _childArtInertia * _childPartialAcc); + + _childArtInertia*_childPartialAcc); } //============================================================================== @@ -491,7 +533,7 @@ void ZeroDofJoint::addChildBiasImpulseTo( //============================================================================== void ZeroDofJoint::updateTotalForce(const Eigen::Vector6d& /*_bodyForce*/, - double /*_timeStep*/) + double /*_timeStep*/) { // Do nothing } @@ -510,32 +552,51 @@ void ZeroDofJoint::resetTotalImpulses() //============================================================================== void ZeroDofJoint::updateAcceleration(const Eigen::Matrix6d& /*_artInertia*/, - const Eigen::Vector6d& /*_spatialAcc*/) + const Eigen::Vector6d& /*_spatialAcc*/) +{ + // Do nothing +} + +//============================================================================== +void ZeroDofJoint::updateVelocityChange( + const Eigen::Matrix6d& /*_artInertia*/, + const Eigen::Vector6d& /*_velocityChange*/) +{ + // Do nothing +} + +//============================================================================== +void ZeroDofJoint::updateForceID(const Eigen::Vector6d& /*_bodyForce*/, + double /*_timeStep*/, + bool /*_withDampingForces*/, + bool /*_withSpringForces*/) { // Do nothing } //============================================================================== -void ZeroDofJoint::updateVelocityChange(const Eigen::Matrix6d& /*_artInertia*/, - const Eigen::Vector6d& /*_velocityChange*/) +void ZeroDofJoint::updateForceFD(const Eigen::Vector6d& /*_bodyForce*/, + double /*_timeStep*/, + bool /*_withDampingForces*/, + bool /*_withSpringForces*/) { // Do nothing } //============================================================================== -void ZeroDofJoint::updateVelocityWithVelocityChange() +void ZeroDofJoint::updateImpulseID(const Eigen::Vector6d& /*_bodyImpulse*/) { // Do nothing } //============================================================================== -void ZeroDofJoint::updateAccelerationWithVelocityChange(double _timeStep) +void ZeroDofJoint::updateImpulseFD(const Eigen::Vector6d& /*_bodyImpulse*/) { // Do nothing } //============================================================================== -void ZeroDofJoint::updateForceWithImpulse(double _timeStep) +void ZeroDofJoint::updateConstrainedTerms(double /*_timeStep*/) { // Do nothing } diff --git a/dart/dynamics/ZeroDofJoint.h b/dart/dynamics/ZeroDofJoint.h index 6ce960e56cc31..3cd7bb59e2df1 100644 --- a/dart/dynamics/ZeroDofJoint.h +++ b/dart/dynamics/ZeroDofJoint.h @@ -62,7 +62,8 @@ class ZeroDofJoint : public Joint //---------------------------------------------------------------------------- // Documentation inherited - DEPRECATED(4.1) virtual size_t getDof() const; + DEPRECATED(4.1) + virtual size_t getDof() const; // Documentation inherited virtual size_t getNumDofs() const; @@ -73,6 +74,25 @@ class ZeroDofJoint : public Joint // Documentation inherited virtual size_t getIndexInSkeleton(size_t _index) const; + //---------------------------------------------------------------------------- + // Command + //---------------------------------------------------------------------------- + + // Documentation inherited + virtual void setCommand(size_t _index, double _command); + + // Documentation inherited + virtual double getCommand(size_t _index) const; + + // Documentation inherited + virtual void setCommands(const Eigen::VectorXd& _commands); + + // Documentation inherited + virtual Eigen::VectorXd getCommands() const; + + // Documentation inherited + virtual void resetCommands(); + //---------------------------------------------------------------------------- // Position //---------------------------------------------------------------------------- @@ -260,87 +280,112 @@ class ZeroDofJoint : public Joint /// Get potential energy virtual double getPotentialEnergy() const; + // Documentation inherited + virtual Eigen::Vector6d getBodyConstraintWrench() const override; + protected: //---------------------------------------------------------------------------- - // Recursive dynamics algorithms + /// \{ \name Recursive dynamics routines //---------------------------------------------------------------------------- // Documentation inherited - virtual const math::Jacobian getLocalJacobian() const; + virtual const math::Jacobian getLocalJacobian() const override; // Documentation inherited - virtual const math::Jacobian getLocalJacobianTimeDeriv() const; + virtual const math::Jacobian getLocalJacobianTimeDeriv() const override; // Documentation inherited - virtual void addVelocityTo(Eigen::Vector6d& _vel); + virtual void addVelocityTo(Eigen::Vector6d& _vel) override; // Documentation inherited - virtual void setPartialAccelerationTo(Eigen::Vector6d& _partialAcceleration, - const Eigen::Vector6d& _childVelocity); + virtual void setPartialAccelerationTo( + Eigen::Vector6d& _partialAcceleration, + const Eigen::Vector6d& _childVelocity) override; // Documentation inherited - virtual void addAccelerationTo(Eigen::Vector6d& _acc); + virtual void addAccelerationTo(Eigen::Vector6d& _acc) override; // Documentation inherited - virtual void addVelocityChangeTo(Eigen::Vector6d& _velocityChange); + virtual void addVelocityChangeTo(Eigen::Vector6d& _velocityChange) override; // Documentation inherited - virtual void addChildArtInertiaTo(Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia); + virtual void addChildArtInertiaTo( + Eigen::Matrix6d& _parentArtInertia, + const Eigen::Matrix6d& _childArtInertia) override; // Documentation inherited virtual void addChildArtInertiaImplicitTo( Eigen::Matrix6d& _parentArtInertia, - const Eigen::Matrix6d& _childArtInertia); + const Eigen::Matrix6d& _childArtInertia) override; // Documentation inherited - virtual void updateInvProjArtInertia(const Eigen::Matrix6d& _artInertia); + virtual void updateInvProjArtInertia( + const Eigen::Matrix6d& _artInertia) override; // Documentation inherited virtual void updateInvProjArtInertiaImplicit( const Eigen::Matrix6d& _artInertia, - double _timeStep); + double _timeStep) override; // Documentation inherited - virtual void addChildBiasForceTo(Eigen::Vector6d& _parentBiasForce, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasForce, - const Eigen::Vector6d& _childPartialAcc); + virtual void addChildBiasForceTo( + Eigen::Vector6d& _parentBiasForce, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasForce, + const Eigen::Vector6d& _childPartialAcc) override; // Documentation inherited - virtual void addChildBiasImpulseTo(Eigen::Vector6d& _parentBiasImpulse, - const Eigen::Matrix6d& _childArtInertia, - const Eigen::Vector6d& _childBiasImpulse); + virtual void addChildBiasImpulseTo( + Eigen::Vector6d& _parentBiasImpulse, + const Eigen::Matrix6d& _childArtInertia, + const Eigen::Vector6d& _childBiasImpulse) override; // Documentation inherited virtual void updateTotalForce(const Eigen::Vector6d& _bodyForce, - double _timeStep); + double _timeStep) override; // Documentation inherited - virtual void updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse); + virtual void updateTotalImpulse( + const Eigen::Vector6d& _bodyImpulse) override; // Documentation inherited - virtual void resetTotalImpulses(); + virtual void resetTotalImpulses() override; // Documentation inherited - virtual void updateAcceleration(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _spatialAcc); + virtual void updateAcceleration( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _spatialAcc) override; // Documentation inherited - virtual void updateVelocityChange(const Eigen::Matrix6d& _artInertia, - const Eigen::Vector6d& _velocityChange); + virtual void updateVelocityChange( + const Eigen::Matrix6d& _artInertia, + const Eigen::Vector6d& _velocityChange) override; // Documentation inherited - virtual void updateVelocityWithVelocityChange(); + virtual void updateForceID(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) override; // Documentation inherited - virtual void updateAccelerationWithVelocityChange(double _timeStep); + virtual void updateForceFD(const Eigen::Vector6d& _bodyForce, + double _timeStep, + bool _withDampingForces, + bool _withSpringForces) override; // Documentation inherited - virtual void updateForceWithImpulse(double _timeStep); + virtual void updateImpulseID(const Eigen::Vector6d& _bodyImpulse) override; + + // Documentation inherited + virtual void updateImpulseFD(const Eigen::Vector6d& _bodyImpulse) override; + + // Documentation inherited + virtual void updateConstrainedTerms(double _timeStep) override; + + /// \} //---------------------------------------------------------------------------- - // Recursive algorithms for equations of motion + /// \{ \name Recursive algorithm routines for equations of motion //---------------------------------------------------------------------------- /// Add child's bias force to parent's one @@ -378,6 +423,8 @@ class ZeroDofJoint : public Joint virtual Eigen::VectorXd getSpatialToGeneralized( const Eigen::Vector6d& _spatial); + /// \} + private: }; diff --git a/dart/lcpsolver/Lemke.cpp b/dart/lcpsolver/Lemke.cpp index 013f631d8fbd3..f1e8b4146aebd 100644 --- a/dart/lcpsolver/Lemke.cpp +++ b/dart/lcpsolver/Lemke.cpp @@ -167,16 +167,16 @@ int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q, break; } - int jSize = math::castUIntToInt(j.size()); + size_t jSize = j.size(); Eigen::VectorXd minRatio(jSize); - for (int i = 0; i < jSize; ++i) { + for (size_t i = 0; i < jSize; ++i) { minRatio[i] = (x[j[i]] + zer_tol) / d[j[i]]; } double theta = minRatio.minCoeff(); std::vector tmpJ; std::vector tmpMinRatio; - for (int i = 0; i < jSize; ++i) { + for (size_t i = 0; i < jSize; ++i) { if (x[j[i]] / d[j[i]] <= theta) { tmpJ.push_back(j[i]); tmpMinRatio.push_back(minRatio[i]); @@ -194,14 +194,14 @@ int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q, // } j = tmpJ; - jSize = math::castUIntToInt(j.size()); + jSize = j.size(); if (jSize == 0) { err = 4; break; } lvindex = -1; - for (int i = 0; i < jSize; ++i) { + for (size_t i = 0; i < jSize; ++i) { if (bas[j[i]] == t) lvindex = i; } @@ -211,7 +211,7 @@ int Lemke(const Eigen::MatrixXd& _M, const Eigen::VectorXd& _q, theta = tmpMinRatio[0]; lvindex = 0; - for (int i = 0; i < jSize; ++i) { + for (size_t i = 0; i < jSize; ++i) { if (tmpMinRatio[i]-theta > piv_tol) { theta = tmpMinRatio[i]; lvindex = i; diff --git a/dart/math/Helpers.h b/dart/math/Helpers.h index 99f4591ab90fb..22a7ed2f38e36 100644 --- a/dart/math/Helpers.h +++ b/dart/math/Helpers.h @@ -53,6 +53,7 @@ #include // Local Headers #include "dart/common/Console.h" +#include "dart/common/Deprecated.h" #include "dart/math/MathTypes.h" namespace dart { @@ -222,17 +223,19 @@ inline double random(double _min, double _max) { * (_max - _min)); } +DEPRECATED(4.3) inline int castUIntToInt(size_t _x) { - if (_x <= INT_MAX) - return static_cast(_x); +// if (_x <= INT_MAX) +// return static_cast(_x); - if (_x >= INT_MIN) - return static_cast(_x - INT_MIN) + INT_MIN; +// if (_x >= INT_MIN) +// return static_cast(_x - INT_MIN) + INT_MIN; - dterr << "x is out of range." << std::endl; +// dterr << "x is out of range." << std::endl; - throw _x; // Or whatever else you like +// throw _x; // Or whatever else you like + return static_cast(_x); } } // namespace math diff --git a/dart/optimizer/Problem.cpp b/dart/optimizer/Problem.cpp index 905cf7026ae04..2d3555c26200b 100644 --- a/dart/optimizer/Problem.cpp +++ b/dart/optimizer/Problem.cpp @@ -71,7 +71,7 @@ size_t Problem::getDimension() const //============================================================================== void Problem::setInitialGuess(const Eigen::VectorXd& _initGuess) { - assert(_initGuess.size() == math::castUIntToInt(mDimension) + assert(static_cast(_initGuess.size()) == mDimension && "Invalid size."); mInitialGuess = _initGuess; } @@ -85,7 +85,7 @@ const Eigen::VectorXd& Problem::getInitialGuess() const //============================================================================== void Problem::setLowerBounds(const Eigen::VectorXd& _lb) { - assert(_lb.size() == math::castUIntToInt(mDimension) && "Invalid size."); + assert(static_cast(_lb.size()) == mDimension && "Invalid size."); mLowerBounds = _lb; } @@ -98,7 +98,7 @@ const Eigen::VectorXd& Problem::getLowerBounds() const //============================================================================== void Problem::setUpperBounds(const Eigen::VectorXd& _ub) { - assert(_ub.size() == math::castUIntToInt(mDimension) && "Invalid size."); + assert(static_cast(_ub.size()) == mDimension && "Invalid size."); mUpperBounds = _ub; } @@ -210,7 +210,7 @@ double Problem::getOptimumValue() const //============================================================================== void Problem::setOptimalSolution(const Eigen::VectorXd& _optParam) { - assert(_optParam.size() == math::castUIntToInt(mDimension) + assert(static_cast(_optParam.size()) == mDimension && "Invalid size."); mOptimalSolution = _optParam; } diff --git a/dart/optimizer/ipopt/IpoptSolver.cpp b/dart/optimizer/ipopt/IpoptSolver.cpp index 70be6dcb6ff62..775129943634b 100644 --- a/dart/optimizer/ipopt/IpoptSolver.cpp +++ b/dart/optimizer/ipopt/IpoptSolver.cpp @@ -138,9 +138,9 @@ bool DartTNLP::get_bounds_info(Ipopt::Index n, { // here, the n and m we gave IPOPT in get_nlp_info are passed back to us. // If desired, we could assert to make sure they are what we think they are. - assert(n == math::castUIntToInt(mProblem->getDimension())); - assert(m == math::castUIntToInt(mProblem->getNumEqConstraints() - + mProblem->getNumIneqConstraints())); + assert(static_cast(n) == mProblem->getDimension()); + assert(static_cast(m) == mProblem->getNumEqConstraints() + + mProblem->getNumIneqConstraints()); // lower and upper bounds for (Ipopt::Index i = 0; i < n; i++) @@ -249,8 +249,8 @@ bool DartTNLP::eval_g(Ipopt::Index _n, Ipopt::Index _m, Ipopt::Number* _g) { - assert(_m == math::castUIntToInt(mProblem->getNumEqConstraints() - + mProblem->getNumIneqConstraints())); + assert(static_cast(_m) == mProblem->getNumEqConstraints() + + mProblem->getNumIneqConstraints()); // TODO(JS): if (_new_x) diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 59b0fd81b2dc2..06fce7e0cb8ea 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -102,64 +102,53 @@ double World::getTimeStep() const return mTimeStep; } - //============================================================================== - -void World::reset() { +void World::reset() +{ mTime = 0.0; mFrame = 0; - mRecording->clear(); + mRecording->clear(); } - -void World::step() +//============================================================================== +void World::step(bool _resetCommand) { - // Integrate velocity unconstrained skeletons - for (std::vector::iterator it = mSkeletons.begin(); - it != mSkeletons.end(); ++it) + // Integrate velocity for unconstrained skeletons + for (auto& skel : mSkeletons) { - if (!(*it)->isMobile()) + if (!skel->isMobile()) continue; - (*it)->computeForwardDynamicsRecursionPartB(); - (*it)->integrateVelocities(mTimeStep); - (*it)->computeForwardKinematics(false, true, false); + skel->computeForwardDynamicsRecursionPartB(); + skel->integrateVelocities(mTimeStep); + skel->computeForwardKinematics(false, true, false); } - // Detect active constraints and compute constraint impulses + // Detect activated constraints and compute constraint impulses mConstraintSolver->solve(); // Compute velocity changes given constraint impulses - for (std::vector::iterator it = mSkeletons.begin(); - it != mSkeletons.end(); ++it) + for (auto& skel : mSkeletons) { - if ((*it)->isImpulseApplied() && (*it)->isMobile()) - { - (*it)->computeImpulseForwardDynamics(); - (*it)->setImpulseApplied(false); - } - } - - // - for (std::vector::iterator it = mSkeletons.begin(); - it != mSkeletons.end(); ++it) - { - if (!(*it)->isMobile()) + if (!skel->isMobile()) continue; - (*it)->integratePositions(mTimeStep); - } + if (skel->isImpulseApplied()) + { + skel->computeImpulseForwardDynamics(); + skel->setImpulseApplied(false); + } - for (std::vector::iterator it = mSkeletons.begin(); - it != mSkeletons.end(); ++it) - { - if (!(*it)->isMobile()) - continue; + skel->integratePositions(mTimeStep); + skel->computeForwardDynamicsRecursionPartA(); - (*it)->computeForwardDynamicsRecursionPartA(); - (*it)->resetForces(); - (*it)->clearExternalForces(); -// (*it)->clearConstraintImpulses(); + if (_resetCommand) + { + skel->resetForces(); + skel->clearExternalForces(); +// skel->clearConstraintImpulses(); + skel->resetCommands(); + } } mTime += mTimeStep; diff --git a/dart/simulation/World.h b/dart/simulation/World.h index 4de5aa66175af..83143544f013c 100644 --- a/dart/simulation/World.h +++ b/dart/simulation/World.h @@ -142,7 +142,9 @@ class World void reset(); /// Calculate the dynamics and integrate the world for one step - void step(); + /// \param[in} _resetCommand True if you want to reset to zero the joint + /// command after simulation step. + void step(bool _resetCommand = true); /// Set current time void setTime(double _time); diff --git a/dart/utils/CMakeLists.txt b/dart/utils/CMakeLists.txt index 96bdb0be62d4d..125da0104eff8 100644 --- a/dart/utils/CMakeLists.txt +++ b/dart/utils/CMakeLists.txt @@ -10,9 +10,11 @@ add_subdirectory(urdf) dart_add_library(dart_utils ${srcs} ${hdrs}) target_link_libraries( dart_utils - dart_collision dart_utils_sdf dart_utils_urdf + dart_collision + dart_dynamics + dart_simulation ${DART_DEPENDENCIES} ) diff --git a/dart/utils/Parser.cpp b/dart/utils/Parser.cpp index 7bf95abcb229b..f8b474eb7369e 100644 --- a/dart/utils/Parser.cpp +++ b/dart/utils/Parser.cpp @@ -545,6 +545,12 @@ void openXMLFile(tinyxml2::XMLDocument& doc, }; } +bool hasAttribute(tinyxml2::XMLElement* element, const char* const name) +{ + const char* const result = element->Attribute(name); + return result != 0; +} + std::string getAttribute(tinyxml2::XMLElement * element, const char* const name) { @@ -617,7 +623,7 @@ ElementEnumerator& ElementEnumerator::operator=(const ElementEnumerator& _rhs) this->m_name = _rhs.m_name; this->m_parent = _rhs.m_parent; this->m_current = _rhs.m_current; - return *this; + return *this; } } // namespace utils diff --git a/dart/utils/Parser.h b/dart/utils/Parser.h index 40431f0fe36cd..8ae86dfec82e2 100644 --- a/dart/utils/Parser.h +++ b/dart/utils/Parser.h @@ -89,6 +89,7 @@ Eigen::Isometry3d getValueIsometry3dWithExtrinsicRotation(tinyxml2::XMLElement* void openXMLFile(tinyxml2::XMLDocument& doc, const char* const filename); bool hasElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); tinyxml2::XMLElement* getElement(tinyxml2::XMLElement* _parentElement, const std::string& _name); +bool hasAttribute(tinyxml2::XMLElement* element, const char* const name); std::string getAttribute(tinyxml2::XMLElement* element, const char* const name); void getAttribute(tinyxml2::XMLElement* element, const char* const name, double* d); diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index da8670e5ad86d..a531ab9a582cb 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -884,6 +884,34 @@ dynamics::Joint* SkelParser::readJoint( std::string name = getAttribute(_jointElement, "name"); newJoint->setName(name); + //-------------------------------------------------------------------------- + // Actuator attribute + if (hasAttribute(_jointElement, "actuator")) + { + const std::string actuator = getAttribute(_jointElement, "actuator"); + + if (actuator == "force") + newJoint->setActuatorType(dynamics::Joint::FORCE); + else if (actuator == "passive") + newJoint->setActuatorType(dynamics::Joint::PASSIVE); + else if (actuator == "servo") + newJoint->setActuatorType(dynamics::Joint::SERVO); + else if (actuator == "acceleration") + newJoint->setActuatorType(dynamics::Joint::ACCELERATION); + else if (actuator == "velocity") + newJoint->setActuatorType(dynamics::Joint::VELOCITY); + else if (actuator == "locked") + newJoint->setActuatorType(dynamics::Joint::LOCKED); + else + dterr << "Joint named [" << newJoint->getName() + << "] contains invalid actuator attribute [" + << actuator << "]." << std::endl; + } + else + { + newJoint->setActuatorType(dynamics::Joint::DefaultActuatorType); + } + //-------------------------------------------------------------------------- // parent SkelBodyNode softParentBodyNode; diff --git a/data/skel/test/collision_of_prescribed_joints_test.skel b/data/skel/test/collision_of_prescribed_joints_test.skel new file mode 100644 index 0000000000000..62ee6ed4d7009 --- /dev/null +++ b/data/skel/test/collision_of_prescribed_joints_test.skel @@ -0,0 +1,249 @@ + + + + + 0.001 + 0 -9.81 0 + dart + + + + 1 + 0 -0.5 0 0 0 0 + + 5 + 0 0 0 + + + 0 0 0 0 0 0 + + + 4.0 0.1 0.5 + + + 0.8 0.2 0.2 + + + 0 0 0 0 0 0 + + + 4.0 0.1 0.5 + + + + + + world + link 1 + -0 0 0 0 0 0 + + 0 0 1 + + + + + + 1 + -1.2 -0.3 0 0 0 0 + + 5 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + + world + link 1 + -0.125 0 0 0 0 0 + + 0 0 1 + + + + + + 1 + -0.6 -0.3 0 0 0 0 + + 5 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + + world + link 1 + -0.125 0 0 0 0 0 + + 0 0 1 + + + + + + 1 + 0.0 -0.3 0 0 0 0 + + 5 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + + world + link 1 + -0.125 0 0 0 0 0 + + 0 0 1 + + + + + + 1 + +0.6 -0.3 0 0 0 0 + + 5 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + + world + link 1 + -0.125 0 0 0 0 0 + + 0 0 1 + + + + + + 1 + +1.2 -0.3 0 0 0 0 + + 5 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + + world + link 1 + -0.125 0 0 0 0 0 + + 0 0 1 + + + + + + 1 + +1.8 -0.3 0 0 0 0 + + 5 + 0 0 0 + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + 0 0 0 0 0 0 + + + 0.25 0.05 0.05 + + + + + + world + link 1 + -0.125 0 0 0 0 0 + + 0 0 1 + + + + + diff --git a/data/skel/test/hybrid_dynamics_test.skel b/data/skel/test/hybrid_dynamics_test.skel new file mode 100644 index 0000000000000..f4b3896f21942 --- /dev/null +++ b/data/skel/test/hybrid_dynamics_test.skel @@ -0,0 +1,172 @@ + + + + + 0.001 + 0 -9.81 0 + fcl_mesh + + + + 1 + 0 0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 -1.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 -2.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 -3.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 -4.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + world + link 1 + 0 0.5 0 0 0 0 + + 1 0 0 + + + + link 1 + link 2 + 0 0.5 0 0 0 0 + + 1 0 0 + + + + link 2 + link 3 + 0 0.5 0 0 0 0 + + 1 0 0 + + + + link 3 + link 4 + 0 0.5 0 0 0 0 + + 1 0 0 + + + + link 4 + link 5 + 0 0.5 0 0 0 0 + + 1 0 0 + + + + + \ No newline at end of file diff --git a/data/skel/test/joint_actuator_type_test.skel b/data/skel/test/joint_actuator_type_test.skel new file mode 100644 index 0000000000000..d917d2d687082 --- /dev/null +++ b/data/skel/test/joint_actuator_type_test.skel @@ -0,0 +1,185 @@ + + + + + 0.001 + 0 -9.81 0 + fcl_mesh + + + + 1 + 0 0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 -1.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 -2.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 -3.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + 1 + 0 -4.0 0 0 0 0 + + 5 + 0 0.5 0 + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + 0 0 0 0 0 0 + + + 0.1 1.0 0.1 + + + + + + world + link 1 + 0 0.5 0 0 0 0 + + 1 0 0 + + + + link 1 + link 2 + 0 0.5 0 0 0 0 + + 1 0 0 + + -1.57 + +1.57 + + + + + link 2 + link 3 + 0 0.5 0 0 0 0 + + 1 0 0 + + + + link 3 + link 4 + 0 0.5 0 0 0 0 + + 1 0 0 + + -1 + +1 + + + + 0 1 0 + + -2 + +2 + + + + + link 4 + link 5 + 0 0.5 0 0 0 0 + xyz + + + + \ No newline at end of file diff --git a/doxygen/Doxyfile.in b/doxygen/Doxyfile.in index 2e896c3bd2acf..c638b545fff17 100644 --- a/doxygen/Doxyfile.in +++ b/doxygen/Doxyfile.in @@ -169,7 +169,7 @@ SHORT_NAMES = NO # description.) # The default value is: NO. -JAVADOC_AUTOBRIEF = NO +JAVADOC_AUTOBRIEF = YES # If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first # line (until the first dot) of a Qt-style comment as the brief description. If diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 208bcb9ba0eb5..e563135488a26 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -41,13 +41,20 @@ #include #include -#include "dart/common/Console.h" -#include "dart/math/Helpers.h" +#include "dart/config.h" +#include "dart/common/common.h" +#include "dart/math/math.h" +#include "dart/dynamics/dynamics.h" //#include "dart/collision/unc/UNCCollisionDetector.h" +#include "dart/simulation/simulation.h" +#include "dart/utils/utils.h" using namespace dart; using namespace math; //using namespace collision; +using namespace dynamics; +using namespace simulation; +using namespace utils; class COLLISION : public testing::Test { @@ -497,10 +504,92 @@ TEST_F(COLLISION, FCL_BOX_BOX) //} -/* ********************************************************************************************* */ -int main(int argc, char* argv[]) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); +//============================================================================== +TEST_F(COLLISION, CollisionOfPrescribedJoints) +{ + // There are one red plate (static skeleton) and 5 pendulums with different + // actuator types. This test check if the motion prescribed joints are exactly + // tracking the prescribed motion eventhough there are collision with other + // objects. + + const double tol = 1e-9; + const double timeStep = 1e-3; + const size_t numFrames = 5e+0; // 5 secs + + // Load world and skeleton + World* world = SkelParser::readWorld( + DART_DATA_PATH"/skel/test/collision_of_prescribed_joints_test.skel"); + world->setTimeStep(timeStep); + EXPECT_TRUE(world != NULL); + EXPECT_NEAR(world->getTimeStep(), timeStep, tol); + + Skeleton* skel1 = world->getSkeleton("skeleton 1"); + Skeleton* skel2 = world->getSkeleton("skeleton 2"); + Skeleton* skel3 = world->getSkeleton("skeleton 3"); + Skeleton* skel4 = world->getSkeleton("skeleton 4"); + Skeleton* skel5 = world->getSkeleton("skeleton 5"); + Skeleton* skel6 = world->getSkeleton("skeleton 6"); + EXPECT_TRUE(skel1 != NULL); + EXPECT_TRUE(skel2 != NULL); + EXPECT_TRUE(skel3 != NULL); + EXPECT_TRUE(skel4 != NULL); + EXPECT_TRUE(skel5 != NULL); + EXPECT_TRUE(skel6 != NULL); + + Joint* joint1 = skel1->getJoint(0); + Joint* joint2 = skel2->getJoint(0); + Joint* joint3 = skel3->getJoint(0); + Joint* joint4 = skel4->getJoint(0); + Joint* joint5 = skel5->getJoint(0); + Joint* joint6 = skel6->getJoint(0); + EXPECT_TRUE(joint1 != NULL); + EXPECT_TRUE(joint2 != NULL); + EXPECT_TRUE(joint3 != NULL); + EXPECT_TRUE(joint4 != NULL); + EXPECT_TRUE(joint5 != NULL); + EXPECT_TRUE(joint6 != NULL); + EXPECT_EQ(joint1->getActuatorType(), Joint::FORCE); + EXPECT_EQ(joint2->getActuatorType(), Joint::PASSIVE); + EXPECT_EQ(joint3->getActuatorType(), Joint::SERVO); + EXPECT_EQ(joint4->getActuatorType(), Joint::ACCELERATION); + EXPECT_EQ(joint5->getActuatorType(), Joint::VELOCITY); + EXPECT_EQ(joint6->getActuatorType(), Joint::LOCKED); + + for (size_t i = 0; i < numFrames; ++i) + { + const double time = world->getTime(); + + joint1->setCommand(0, -0.5 * DART_PI * std::cos(time)); + joint2->setCommand(0, -0.5 * DART_PI * std::cos(time)); + joint3->setCommand(0, -0.5 * DART_PI * std::cos(time)); + joint4->setCommand(0, -0.5 * DART_PI * std::cos(time)); + joint5->setCommand(0, -0.5 * DART_PI * std::sin(time)); + joint6->setCommand(0, -0.5 * DART_PI * std::sin(time)); // ignored + + world->step(false); + + EXPECT_TRUE(joint1->isDynamic()); + EXPECT_TRUE(joint2->isDynamic()); + EXPECT_TRUE(joint3->isDynamic()); + + // Check if the motion prescribed joints are following the prescribed motion + // eventhough there is a collision with other objects + EXPECT_TRUE(joint4->isKinematic()); + EXPECT_NEAR(joint4->getAcceleration(0), joint4->getCommand(0), tol); + EXPECT_TRUE(joint5->isKinematic()); + EXPECT_NEAR(joint5->getVelocity(0), joint5->getCommand(0), tol); + + // The velocity and acceleration of locked joint always must be zero. + EXPECT_TRUE(joint6->isKinematic()); + EXPECT_NEAR(joint6->getVelocity(0), 0.0, tol); + EXPECT_NEAR(joint6->getAcceleration(0), 0.0, tol); + } +} + +//============================================================================== +int main(int argc, char* argv[]) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } -/* ********************************************************************************************* */ diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 62cfc2b903a08..18d962b229691 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -1015,7 +1015,7 @@ void DynamicsTest::testConstraintImpulse(const std::string& _fileName) EXPECT_NEAR(constraintVector1(l), constraintVector2(index), 1e-6); index++; } - assert(bodyJacobian.cols() == math::castUIntToInt(index)); + assert(static_cast(bodyJacobian.cols()) == index); } } } @@ -1207,6 +1207,113 @@ TEST_F(DynamicsTest, testImpulseBasedDynamics) } } +//============================================================================== +TEST_F(DynamicsTest, HybridDynamics) +{ + const double tol = 1e-9; + const double timeStep = 1e-3; + const size_t numFrames = 5e+3; // 5 secs + + // Load world and skeleton + World* world = utils::SkelParser::readWorld( + DART_DATA_PATH"/skel/test/hybrid_dynamics_test.skel"); + world->setTimeStep(timeStep); + EXPECT_TRUE(world != NULL); + EXPECT_NEAR(world->getTimeStep(), timeStep, tol); + + Skeleton* skel = world->getSkeleton("skeleton 1"); + EXPECT_TRUE(skel != NULL); + EXPECT_NEAR(skel->getTimeStep(), timeStep, tol); + + const size_t numDofs = skel->getNumDofs(); + + // Zero initial states + Eigen::VectorXd q0 = Eigen::VectorXd::Zero(numDofs); + Eigen::VectorXd dq0 = Eigen::VectorXd::Zero(numDofs); + + // Initialize the skeleton with the zero initial states + skel->setPositions(q0); + skel->setVelocities(dq0); + skel->computeForwardKinematics(true, true, true); + EXPECT_TRUE(equals(skel->getPositions(), q0)); + EXPECT_TRUE(equals(skel->getVelocities(), dq0)); + + // Make sure all the joint actuator types + EXPECT_EQ(skel->getJoint(0)->getActuatorType(), Joint::FORCE); + EXPECT_EQ(skel->getJoint(1)->getActuatorType(), Joint::ACCELERATION); + EXPECT_EQ(skel->getJoint(2)->getActuatorType(), Joint::VELOCITY); + EXPECT_EQ(skel->getJoint(3)->getActuatorType(), Joint::ACCELERATION); + EXPECT_EQ(skel->getJoint(4)->getActuatorType(), Joint::VELOCITY); + + // Prepare command for each joint types per simulation steps + Eigen::MatrixXd command = Eigen::MatrixXd::Zero(numFrames, numDofs); + Eigen::VectorXd amp = Eigen::VectorXd::Zero(numDofs); + for (size_t i = 0; i < numDofs; ++i) + amp[i] = math::random(-2.0, 2.0); + for (size_t i = 0; i < numFrames; ++i) + { + for (size_t j = 0; j < numDofs; ++j) + command(i,j) = amp[j] * std::sin(i * timeStep); + } + + // Record joint forces for joint[1~4] + Eigen::MatrixXd forces = Eigen::MatrixXd::Zero(numFrames, numDofs); + for (size_t i = 0; i < numFrames; ++i) + { + skel->setCommands(command.row(i)); + + world->step(false); + + forces.row(i) = skel->getForces(); + + EXPECT_NEAR(command(i,0), skel->getForce(0), tol); + EXPECT_NEAR(command(i,1), skel->getAcceleration(1), tol); + EXPECT_NEAR(command(i,2), skel->getVelocity(2), tol); + EXPECT_NEAR(command(i,3), skel->getAcceleration(3), tol); + EXPECT_NEAR(command(i,4), skel->getVelocity(4), tol); + } + + // Restore the skeleton to the initial state + skel->setPositions(q0); + skel->setVelocities(dq0); + skel->computeForwardKinematics(true, true, true); + EXPECT_TRUE(equals(skel->getPositions(), q0)); + EXPECT_TRUE(equals(skel->getVelocities(), dq0)); + + // Change all the actuator types to force + skel->getJoint(0)->setActuatorType(Joint::FORCE); + skel->getJoint(1)->setActuatorType(Joint::FORCE); + skel->getJoint(2)->setActuatorType(Joint::FORCE); + skel->getJoint(3)->setActuatorType(Joint::FORCE); + skel->getJoint(4)->setActuatorType(Joint::FORCE); + EXPECT_EQ(skel->getJoint(0)->getActuatorType(), Joint::FORCE); + EXPECT_EQ(skel->getJoint(1)->getActuatorType(), Joint::FORCE); + EXPECT_EQ(skel->getJoint(2)->getActuatorType(), Joint::FORCE); + EXPECT_EQ(skel->getJoint(3)->getActuatorType(), Joint::FORCE); + EXPECT_EQ(skel->getJoint(4)->getActuatorType(), Joint::FORCE); + + // Test if the skeleton moves as the command with the joint forces + Eigen::MatrixXd output = Eigen::MatrixXd::Zero(numFrames, numDofs); + for (size_t i = 0; i < numFrames; ++i) + { + skel->setCommands(forces.row(i)); + + world->step(false); + + output(i,0) = skel->getJoint(0)->getForce(0); + output(i,1) = skel->getJoint(1)->getAcceleration(0); + output(i,2) = skel->getJoint(2)->getVelocity(0); + output(i,3) = skel->getJoint(3)->getAcceleration(0); + output(i,4) = skel->getJoint(4)->getVelocity(0); + + EXPECT_NEAR(command(i,0), output(i,0), tol); + EXPECT_NEAR(command(i,1), output(i,1), tol); + EXPECT_NEAR(command(i,2), output(i,2), tol); + EXPECT_NEAR(command(i,3), output(i,3), tol); + EXPECT_NEAR(command(i,4), output(i,4), tol); + } +} + //============================================================================== int main(int argc, char* argv[]) { diff --git a/unittests/testParser.cpp b/unittests/testParser.cpp index 38d57577c5a2e..f424d6bab0431 100644 --- a/unittests/testParser.cpp +++ b/unittests/testParser.cpp @@ -313,6 +313,40 @@ TEST(SKEL_PARSER, PLANAR_JOINT) delete world; } +//============================================================================== +TEST(SKEL_PARSER, JointActuatorType) +{ + World* world = SkelParser::readWorld( + DART_DATA_PATH"/skel/test/joint_actuator_type_test.skel"); + EXPECT_TRUE(world != NULL); + + Skeleton* skel1 = world->getSkeleton("skeleton 1"); + EXPECT_TRUE(skel1 != NULL); + + // Test for no actuator type attribute being specified + Joint* joint0 = skel1->getJoint("joint0"); + EXPECT_EQ(joint0->getActuatorType(), Joint::DefaultActuatorType); + EXPECT_EQ(joint0->getActuatorType(), Joint::FORCE); + + // Test for when actuator type attribute are specified + Joint* joint1 = skel1->getJoint("joint1"); + EXPECT_EQ(joint1->getActuatorType(), Joint::FORCE); + + // Test for only a dof name being changed + Joint* joint2 = skel1->getJoint("joint2"); + EXPECT_EQ(joint2->getActuatorType(), Joint::PASSIVE); + joint2->setActuatorType(Joint::FORCE); + EXPECT_EQ(joint2->getActuatorType(), Joint::FORCE); + + // Test for when actuator type attribute are specified + Joint* joint3 = skel1->getJoint("joint3"); + EXPECT_EQ(joint3->getActuatorType(), Joint::ACCELERATION); + + // Test for when actuator type attribute are specified + Joint* joint4 = skel1->getJoint("joint4"); + EXPECT_EQ(joint4->getActuatorType(), Joint::VELOCITY); +} + /******************************************************************************/ int main(int argc, char* argv[]) {