From 8243914fb5c1126c83d8cbcb4af3a6dbba5beb5e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Mar 2014 20:35:56 -0400 Subject: [PATCH 01/11] Remove JointType - Type defendent program is not recommended - Only workaround for Issue #122 use dynamic_cast but this should be gone once this issue is resolved --- dart/dynamics/BallJoint.cpp | 5 ++- dart/dynamics/EulerJoint.cpp | 5 ++- dart/dynamics/FreeJoint.cpp | 5 ++- dart/dynamics/Joint.cpp | 16 +++----- dart/dynamics/Joint.h | 58 +++++++--------------------- dart/dynamics/PlanarJoint.cpp | 2 +- dart/dynamics/PrismaticJoint.cpp | 5 ++- dart/dynamics/RevoluteJoint.cpp | 5 ++- dart/dynamics/ScrewJoint.cpp | 5 ++- dart/dynamics/Skeleton.cpp | 25 ++++++++---- dart/dynamics/TranslationalJoint.cpp | 3 +- dart/dynamics/UniversalJoint.cpp | 3 +- dart/dynamics/WeldJoint.cpp | 3 +- 13 files changed, 62 insertions(+), 78 deletions(-) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index c9d6f9b9b55cd..5160e91c055ea 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -45,8 +45,9 @@ namespace dart { namespace dynamics { BallJoint::BallJoint(const std::string& _name) - : Joint(BALL, _name), - mT_Joint(Eigen::Isometry3d::Identity()) { + : Joint(_name), + mT_Joint(Eigen::Isometry3d::Identity()) +{ mGenCoords.push_back(&mCoordinate[0]); mGenCoords.push_back(&mCoordinate[1]); mGenCoords.push_back(&mCoordinate[2]); diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index be55770d941c6..fb404dea83df3 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -46,8 +46,9 @@ namespace dart { namespace dynamics { EulerJoint::EulerJoint(const std::string& _name) - : Joint(EULER, _name), - mAxisOrder(AO_XYZ) { + : Joint(_name), + mAxisOrder(AO_XYZ) +{ mGenCoords.push_back(&mCoordinate[0]); mGenCoords.push_back(&mCoordinate[1]); mGenCoords.push_back(&mCoordinate[2]); diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index fc6567f30a928..082ae579dab6a 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -45,8 +45,9 @@ namespace dart { namespace dynamics { FreeJoint::FreeJoint(const std::string& _name) - : Joint(FREE, _name), - mT_Joint(Eigen::Isometry3d::Identity()) { + : Joint(_name), + mT_Joint(Eigen::Isometry3d::Identity()) +{ mGenCoords.push_back(&mCoordinate[0]); mGenCoords.push_back(&mCoordinate[1]); mGenCoords.push_back(&mCoordinate[2]); diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index a80220a022bdc..8dc405108d689 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -46,14 +46,14 @@ namespace dart { namespace dynamics { -Joint::Joint(JointType _type, const std::string& _name) +Joint::Joint(const std::string& _name) : mName(_name), mSkelIndex(-1), - mJointType(_type), mIsPositionLimited(true), mT_ParentBodyToJoint(Eigen::Isometry3d::Identity()), mT_ChildBodyToJoint(Eigen::Isometry3d::Identity()), - mT(Eigen::Isometry3d::Identity()) { + mT(Eigen::Isometry3d::Identity()) +{ } Joint::~Joint() { @@ -67,19 +67,15 @@ const std::string& Joint::getName() const { return mName; } -Joint::JointType Joint::getJointType() const { - return mJointType; -} - -const Eigen::Isometry3d&Joint::getLocalTransform() const { +const Eigen::Isometry3d& Joint::getLocalTransform() const { return mT; } -const math::Jacobian&Joint::getLocalJacobian() const { +const math::Jacobian& Joint::getLocalJacobian() const { return mS; } -const math::Jacobian&Joint::getLocalJacobianTimeDeriv() const { +const math::Jacobian& Joint::getLocalJacobianTimeDeriv() const { return mdS; } diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index ce6d30e44fe59..4c9b9de3f0081 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -56,51 +56,27 @@ namespace dynamics { class BodyNode; /// \brief -class Joint : public GenCoordSystem { +class Joint : public GenCoordSystem +{ public: + /// \brief Set BodyNode as friend class friend class BodyNode; - //-------------------------------------------------------------------------- - // Types - //-------------------------------------------------------------------------- - /// \brief - enum JointType { - WELD, // 0-dof - REVOLUTE, // 1-dof - PRISMATIC, // 1-dof - SCREW, // 1-dof - UNIVERSAL, // 2-dof - TRANSLATIONAL, // 3-dof - BALL, // 3-dof - PLANAR, // 3-dof - EULER, // 3-dof - FREE // 6-dof - }; - - //-------------------------------------------------------------------------- - // Constructor and Destructor - //-------------------------------------------------------------------------- - /// \brief - Joint(JointType _type, const std::string& _name = "Noname Joint"); + //--------------------- Constructor and Destructor --------------------------- + /// \brief Constructor + Joint(const std::string& _name = "Noname Joint"); - /// \brief + /// \brief Destructor virtual ~Joint(); - //-------------------------------------------------------------------------- - // - //-------------------------------------------------------------------------- - /// \brief + //---------------------------------------------------------------------------- + /// \brief Set joint name void setName(const std::string& _name); - /// \brief + /// \brief Get joint name const std::string& getName() const; - //-------------------------------------------------------------------------- - // Kinematical Properties - //-------------------------------------------------------------------------- - /// \brief - JointType getJointType() const; - + //------------------------ Kinematical Properties ---------------------------- /// \brief const Eigen::Isometry3d& getLocalTransform() const; @@ -119,18 +95,14 @@ class Joint : public GenCoordSystem { /// presented at this joint, return -1. int getGenCoordLocalIndex(int _dofSkelIndex) const; - //-------------------------------------------------------------------------- - // Dynamics Properties - //-------------------------------------------------------------------------- + //------------------------ Dynamics Properties ------------------------------- /// \brief void setPositionLimited(bool _isPositionLimited); /// \brief bool isPositionLimited() const; - //-------------------------------------------------------------------------- - // Structueral Properties - //-------------------------------------------------------------------------- + //----------------------- Structueral Properties ----------------------------- /// \brief int getSkeletonIndex() const; @@ -275,10 +247,6 @@ class Joint : public GenCoordSystem { /// \brief std::vector mRestPosition; - -private: - /// \brief Type of joint e.g. ball, hinge etc. - JointType mJointType; }; } // namespace dynamics diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 650a47fea6822..1059feaccaa23 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -45,7 +45,7 @@ namespace dart { namespace dynamics { PlanarJoint::PlanarJoint(const std::string& _name) - : Joint(PLANAR, _name) + : Joint(_name) { mGenCoords.push_back(&mCoordinate[0]); mGenCoords.push_back(&mCoordinate[1]); diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index 147f4d52bc0dd..aafbdb310c3ee 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -46,8 +46,9 @@ namespace dynamics { PrismaticJoint::PrismaticJoint(const Eigen::Vector3d& axis, const std::string& _name) - : Joint(PRISMATIC, _name), - mAxis(axis.normalized()) { + : Joint(_name), + mAxis(axis.normalized()) +{ mGenCoords.push_back(&mCoordinate); mS = Eigen::Matrix::Zero(); diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 0d22d5aaebf9c..7a6b02b370830 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -47,8 +47,9 @@ namespace dynamics { RevoluteJoint::RevoluteJoint(const Eigen::Vector3d& axis, const std::string& _name) - : Joint(REVOLUTE, _name), - mAxis(axis.normalized()) { + : Joint(_name), + mAxis(axis.normalized()) +{ mGenCoords.push_back(&mCoordinate); mS = Eigen::Matrix::Zero(); diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index 9882bc0f855c3..a04041cc7f1b5 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -47,9 +47,10 @@ namespace dynamics { ScrewJoint::ScrewJoint(const Eigen::Vector3d& axis, double _pitch, const std::string& _name) - : Joint(SCREW, _name), + : Joint(_name), mAxis(axis.normalized()), - mPitch(_pitch) { + mPitch(_pitch) +{ mGenCoords.push_back(&mCoordinate); mS = Eigen::Matrix::Zero(); diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 59f7cda084223..f8bf6736be8eb 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -48,6 +48,8 @@ #include "dart/dynamics/BodyNode.h" #include "dart/dynamics/GenCoord.h" #include "dart/dynamics/Joint.h" +#include "dart/dynamics/BallJoint.h" // Fix for #122 should remove this too +#include "dart/dynamics/FreeJoint.h" // Fix for #122 should remove this too #include "dart/dynamics/Marker.h" namespace dart { @@ -316,19 +318,26 @@ void Skeleton::setConfig(const Eigen::VectorXd& _config) { } } -void Skeleton::setState(const Eigen::VectorXd& _state) { +void Skeleton::setState(const Eigen::VectorXd& _state) +{ set_q(_state.head(_state.size() / 2)); set_dq(_state.tail(_state.size() / 2)); for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) { + it != mBodyNodes.end(); ++it) + { // TODO(JS): This is workaround for Issue #122. - if ((*it)->getParentJoint()->getJointType() == Joint::BALL - || (*it)->getParentJoint()->getJointType() == Joint::FREE) { + // TODO(JS): If possible we recomment not to use dynamic_cast. Fix for this + // issue should not to use dynamic_cast. + if (dynamic_cast((*it)->getParentJoint()) + || dynamic_cast((*it)->getParentJoint())) + { (*it)->updateTransform_Issue122(mTimeStep); (*it)->updateVelocity(); (*it)->updateEta_Issue122(); - } else { + } + else + { (*it)->updateTransform(); (*it)->updateVelocity(); (*it)->updateEta(); @@ -336,7 +345,8 @@ void Skeleton::setState(const Eigen::VectorXd& _state) { } for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); - it != mBodyNodes.rend(); ++it) { + it != mBodyNodes.rend(); ++it) + { (*it)->updateArticulatedInertia(mTimeStep); } @@ -351,7 +361,8 @@ void Skeleton::setState(const Eigen::VectorXd& _state) { mIsDampingForceVectorDirty = true; for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) { + it != mBodyNodes.end(); ++it) + { (*it)->mIsBodyJacobianDirty = true; (*it)->mIsBodyJacobianTimeDerivDirty = true; } diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index 6c9eb8e58c498..064ad8d1f7232 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -44,7 +44,8 @@ namespace dart { namespace dynamics { TranslationalJoint::TranslationalJoint(const std::string& _name) - : Joint(TRANSLATIONAL, _name) { + : Joint(_name) +{ mGenCoords.push_back(&mCoordinate[0]); mGenCoords.push_back(&mCoordinate[1]); mGenCoords.push_back(&mCoordinate[2]); diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 13b228ff8bf52..2e526b4304600 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -47,7 +47,8 @@ namespace dynamics { UniversalJoint::UniversalJoint(const Eigen::Vector3d& _axis0, const Eigen::Vector3d& _axis1, const std::string& _name) - : Joint(UNIVERSAL, _name) { + : Joint(_name) +{ mGenCoords.push_back(&mCoordinate[0]); mGenCoords.push_back(&mCoordinate[1]); diff --git a/dart/dynamics/WeldJoint.cpp b/dart/dynamics/WeldJoint.cpp index 99774d5605bb6..c503f24b6c4e9 100644 --- a/dart/dynamics/WeldJoint.cpp +++ b/dart/dynamics/WeldJoint.cpp @@ -45,7 +45,8 @@ namespace dart { namespace dynamics { WeldJoint::WeldJoint(const std::string& _name) - : Joint(WELD, _name) { + : Joint(_name) +{ mS = math::Jacobian::Zero(6, 0); mdS = math::Jacobian::Zero(6, 0); } From 6f0e9e109a4699be464fe7b8a09042216048e385 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Mar 2014 21:08:44 -0400 Subject: [PATCH 02/11] Add Skeleton pointer to Joint class and clean up Joint class - Skeleton pointer is required to notice to the skeleton that joint values are changed - The skeleton point is set by Joint::init() and the init() function called by BodyNode::init() --- dart/dynamics/BodyNode.cpp | 12 ++- dart/dynamics/Joint.cpp | 11 +++ dart/dynamics/Joint.h | 174 +++++++++++++++++++------------------ 3 files changed, 107 insertions(+), 90 deletions(-) diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 272a799ebe7c9..f1902835acec3 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -367,12 +367,13 @@ bool BodyNode::isColliding() { return mIsColliding; } -void BodyNode::init(Skeleton* _skeleton, int _skeletonIndex) { +void BodyNode::init(Skeleton* _skeleton, int _skeletonIndex) +{ assert(_skeleton); mSkeleton = _skeleton; mSkelIndex = _skeletonIndex; - mParentJoint->mSkelIndex = _skeletonIndex; + mParentJoint->init(_skeleton, _skeletonIndex); //-------------------------------------------------------------------------- // Fill the list of generalized coordinates this node depends on, and sort @@ -380,6 +381,7 @@ void BodyNode::init(Skeleton* _skeleton, int _skeletonIndex) { //-------------------------------------------------------------------------- if (mParentBodyNode) mDependentGenCoordIndices = mParentBodyNode->mDependentGenCoordIndices; + else mDependentGenCoordIndices.clear(); for (int i = 0; i < mParentJoint->getNumGenCoords(); i++) @@ -390,8 +392,10 @@ void BodyNode::init(Skeleton* _skeleton, int _skeletonIndex) { #ifndef NDEBUG // Check whether there is duplicated indices. int nDepGenCoordIndices = mDependentGenCoordIndices.size(); - for (int i = 0; i < nDepGenCoordIndices - 1; i++) { - for (int j = i + 1; j < nDepGenCoordIndices; j++) { + for (int i = 0; i < nDepGenCoordIndices - 1; i++) + { + for (int j = i + 1; j < nDepGenCoordIndices; j++) + { assert(mDependentGenCoordIndices[i] != mDependentGenCoordIndices[j] && "Duplicated index is found in mDependentGenCoordIndices."); diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 8dc405108d689..034f5af900c9a 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -67,6 +67,11 @@ const std::string& Joint::getName() const { return mName; } +Skeleton*Joint::getSkeleton() const +{ + return mSkeleton; +} + const Eigen::Isometry3d& Joint::getLocalTransform() const { return mT; } @@ -125,6 +130,12 @@ void Joint::applyGLTransform(renderer::RenderInterface* _ri) { _ri->transform(mT); } +void Joint::init(Skeleton* _skel, int _skelIdx) +{ + mSkeleton = _skel; + mSkelIndex = _skelIdx; +} + void Joint::setDampingCoefficient(int _idx, double _d) { assert(0 <= _idx && _idx < getNumGenCoords()); assert(_d >= 0.0); diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 4c9b9de3f0081..80f21b2c3eed9 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -54,90 +54,53 @@ namespace dart { namespace dynamics { class BodyNode; +class Skeleton; -/// \brief +/// \class joint class Joint : public GenCoordSystem { public: /// \brief Set BodyNode as friend class friend class BodyNode; - //--------------------- Constructor and Destructor --------------------------- +public: + //---------------------- Constructor and Destructor -------------------------- /// \brief Constructor Joint(const std::string& _name = "Noname Joint"); /// \brief Destructor virtual ~Joint(); - //---------------------------------------------------------------------------- + //------------------------------ Properties ---------------------------------- /// \brief Set joint name void setName(const std::string& _name); /// \brief Get joint name const std::string& getName() const; - //------------------------ Kinematical Properties ---------------------------- - /// \brief - const Eigen::Isometry3d& getLocalTransform() const; - - /// \brief - const math::Jacobian& getLocalJacobian() const; - - /// \brief - const math::Jacobian& getLocalJacobianTimeDeriv() const; - - /// \brief Get whether this joint contains _genCoord. - /// \param[in] Generalized coordinate to see. - /// \return True if this joint contains _genCoord. - bool contains(const GenCoord* _genCoord) const; + /// \brief Get skeleton that this joint belongs to + Skeleton* getSkeleton() const; - /// \brief Get local index of the dof at this joint; if the dof is not - /// presented at this joint, return -1. - int getGenCoordLocalIndex(int _dofSkelIndex) const; - - //------------------------ Dynamics Properties ------------------------------- - /// \brief - void setPositionLimited(bool _isPositionLimited); - - /// \brief - bool isPositionLimited() const; - - //----------------------- Structueral Properties ----------------------------- - /// \brief + /// \brief Get index of this joint in the skeleton that this joint belongs to int getSkeletonIndex() const; - /// \brief + /// \brief Set transformation from parent body node to this joint void setTransformFromParentBodyNode(const Eigen::Isometry3d& _T); - /// \brief + /// \brief Set transformation from child body node to this joint void setTransformFromChildBodyNode(const Eigen::Isometry3d& _T); - /// \brief + /// \brief Get transformation from parent body node to this joint const Eigen::Isometry3d& getTransformFromParentBodyNode() const; - /// \brief + /// \brief Get transformation from child body node to this joint const Eigen::Isometry3d& getTransformFromChildBodyNode() const; - /// \brief Set damping coefficient for viscous force. - /// \param[in] _idx Index of joint axis. - /// \param[in] _d Damping coefficient. - void setDampingCoefficient(int _idx, double _d); - - /// \brief Get damping coefficient for viscous force. - /// \param[in] _idx Index of joint axis. - double getDampingCoefficient(int _idx) const; + /// \brief Set to enforce joint position limit + void setPositionLimited(bool _isPositionLimited); - /// \brief Get damping force. - /// - /// We apply the damping force in implicit manner. The damping force is - /// F = -(dampingCoefficient * dq(k+1)), where dq(k+1) is approximated as - /// dq(k) + h * ddq(k). Since, in the recursive forward dynamics algorithm, - /// ddq(k) is unknown variable that we want to obtain as the result, the - /// damping force here is just F = -(dampingCoefficient * dq(k)) and - /// -dampingCoefficient * h * ddq(k) term is rearranged at the recursive - /// forward dynamics algorithm, and it affects on the articulated inertia. - /// \sa BodyNode::updateArticulatedInertia(double). - Eigen::VectorXd getDampingForces() const; + /// \brief Get whether enforcing joint position limit + bool isPositionLimited() const; /// \brief Set spring stiffness for spring force. /// \param[in] _idx Index of joint axis. @@ -158,6 +121,38 @@ class Joint : public GenCoordSystem /// \return Rest position. double getRestPosition(int _idx) const; + /// \brief Set damping coefficient for viscous force. + /// \param[in] _idx Index of joint axis. + /// \param[in] _d Damping coefficient. + void setDampingCoefficient(int _idx, double _d); + + /// \brief Get damping coefficient for viscous force. + /// \param[in] _idx Index of joint axis. + double getDampingCoefficient(int _idx) const; + + /// \brief Get potential energy. + double getPotentialEnergy() const; + + /// \brief Get transformation from parent body node to child body node + const Eigen::Isometry3d& getLocalTransform() const; + + /// \brief Get generalized Jacobian from parent body node to child body node + /// w.r.t. local generalized coordinate + const math::Jacobian& getLocalJacobian() const; + + /// \brief Get time derivative of generalized Jacobian from parent body node + /// to child body node w.r.t. local generalized coordinate + const math::Jacobian& getLocalJacobianTimeDeriv() const; + + /// \brief Get whether this joint contains _genCoord. + /// \param[in] Generalized coordinate to see. + /// \return True if this joint contains _genCoord. + bool contains(const GenCoord* _genCoord) const; + + /// \brief Get local index of the dof at this joint; if the dof is not + /// presented at this joint, return -1. + int getGenCoordLocalIndex(int _dofSkelIndex) const; + /// \brief Get spring force. /// /// We apply spring force in implicit manner. The spring force is @@ -173,57 +168,67 @@ class Joint : public GenCoordSystem /// \param[in] _timeStep Time step used for approximating q(k+1). Eigen::VectorXd getSpringForces(double _timeStep) const; - /// \brief Get potential energy. - double getPotentialEnergy() const; + /// \brief Get damping force. + /// + /// We apply the damping force in implicit manner. The damping force is + /// F = -(dampingCoefficient * dq(k+1)), where dq(k+1) is approximated as + /// dq(k) + h * ddq(k). Since, in the recursive forward dynamics algorithm, + /// ddq(k) is unknown variable that we want to obtain as the result, the + /// damping force here is just F = -(dampingCoefficient * dq(k)) and + /// -dampingCoefficient * h * ddq(k) term is rearranged at the recursive + /// forward dynamics algorithm, and it affects on the articulated inertia. + /// \sa BodyNode::updateArticulatedInertia(double). + Eigen::VectorXd getDampingForces() const; + //----------------------------- Rendering ------------------------------------ /// \brief void applyGLTransform(renderer::RenderInterface* _ri); protected: - /// \brief - /// q --> T(q) + /// \brief Initialize this joint. This function is called by BodyNode::init() + virtual void init(Skeleton* _skel, int _skelIdx); + + /// \brief Update transformation from parent body node to child body node virtual void updateTransform() = 0; - /// @brief TODO(JS): This is workaround for Issue #122. + // TODO(JS): This is workaround for Issue #122. + /// \brief Update transformation from parent body node to child body node virtual void updateTransform_Issue122(double _timeStep) {} - /// \brief - /// q, dq --> S(q), V(q, dq) - /// V(q, dq) = S(q) * dq + /// \brief Update generalized Jacobian from parent body node to child body + /// node w.r.t. local generalized coordinate virtual void updateJacobian() = 0; - /// @brief TODO(JS): This is workaround for Issue #122. + // TODO(JS): This is workaround for Issue #122. + /// \brief Update generalized Jacobian from parent body node to child body + /// node w.r.t. local generalized coordinate virtual void updateJacobian_Issue122() {} - /// \brief - /// dq, ddq, S(q) --> dS(q), dV(q, dq, ddq) - /// dV(q, dq, ddq) = dS(q) * dq + S(q) * ddq + /// \brief Update time derivative of generalized Jacobian from parent body + /// node to child body node w.r.t. local generalized coordinate virtual void updateJacobianTimeDeriv() = 0; - /// @brief TODO(JS): This is workaround for Issue #122. + // TODO(JS): This is workaround for Issue #122. + /// \brief Update time derivative of generalized Jacobian from parent body + /// node to child body node w.r.t. local generalized coordinate virtual void updateJacobianTimeDeriv_Issue122() {} - //-------------------------------------------------------------------------- - // - //-------------------------------------------------------------------------- - /// \brief +protected: + /// \brief Joint name std::string mName; - //-------------------------------------------------------------------------- - // Structueral Properties - //-------------------------------------------------------------------------- + /// \brief Skeleton pointer that this joint belongs to + Skeleton* mSkeleton; + /// \brief Unique dof id in skeleton int mSkelIndex; - /// \brief + /// \brief Transformation from parent body node to this joint Eigen::Isometry3d mT_ParentBodyToJoint; - /// \brief + /// \brief Transformation from child body node to this joint Eigen::Isometry3d mT_ChildBodyToJoint; - //-------------------------------------------------------------------------- - // Kinematics variables - //-------------------------------------------------------------------------- /// \brief Local transformation. Eigen::Isometry3d mT; @@ -233,20 +238,17 @@ class Joint : public GenCoordSystem /// \brief Time derivative of local Jacobian. math::Jacobian mdS; - //-------------------------------------------------------------------------- - // Dynamics variables - //-------------------------------------------------------------------------- /// \brief True if the joint limits are enforced in dynamic simulation. bool mIsPositionLimited; - /// \brief - std::vector mDampingCoefficient; - - /// \brief + /// \brief Joint spring stiffness std::vector mSpringStiffness; - /// \brief + /// \brief Rest joint position for joint spring std::vector mRestPosition; + + /// \brief Joint damping coefficient + std::vector mDampingCoefficient; }; } // namespace dynamics From b4b0b9f114266bede35e7ce86857506d1a12cbc4 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 20 Mar 2014 06:40:46 -0400 Subject: [PATCH 03/11] Clean up GenCoord --- apps/balance/Controller.cpp | 2 +- apps/hardcodedDesign/Main.cpp | 6 +- apps/harnessTest/Controller.cpp | 2 +- dart/constraint/ConstraintDynamics.cpp | 6 +- dart/dynamics/BallJoint.cpp | 24 +-- dart/dynamics/EulerJoint.cpp | 32 +-- dart/dynamics/FreeJoint.cpp | 26 +-- dart/dynamics/GenCoord.cpp | 273 ++++++++++++++++++++++--- dart/dynamics/GenCoord.h | 250 ++++++++++++++-------- dart/dynamics/GenCoordSystem.cpp | 55 ++--- dart/dynamics/GenCoordSystem.h | 60 ++++-- dart/dynamics/Joint.cpp | 14 +- dart/dynamics/PlanarJoint.cpp | 16 +- dart/dynamics/PrismaticJoint.cpp | 2 +- dart/dynamics/RevoluteJoint.cpp | 2 +- dart/dynamics/ScrewJoint.cpp | 10 +- dart/dynamics/Skeleton.cpp | 4 +- dart/dynamics/UniversalJoint.cpp | 18 +- dart/math/MathTypes.h | 3 + dart/planning/RRT.cpp | 2 +- dart/utils/SkelParser.cpp | 32 +-- dart/utils/sdf/SdfParser.cpp | 20 +- dart/utils/urdf/DartLoader.cpp | 16 +- unittests/TestHelpers.h | 6 +- unittests/testInverseKinematics.cpp | 8 +- unittests/testJoints.cpp | 16 +- unittests/testSoftDynamics.cpp | 4 +- 27 files changed, 619 insertions(+), 290 deletions(-) diff --git a/apps/balance/Controller.cpp b/apps/balance/Controller.cpp index 08cba3f54511d..a01f028ed5769 100644 --- a/apps/balance/Controller.cpp +++ b/apps/balance/Controller.cpp @@ -61,7 +61,7 @@ Controller::Controller(dart::dynamics::Skeleton* _skel, mDesiredDofs.resize(nDof); for (int i = 0; i < nDof; i++) { mTorques[i] = 0.0; - mDesiredDofs[i] = mSkel->getGenCoord(i)->get_q(); + mDesiredDofs[i] = mSkel->getGenCoord(i)->getConfig(); } // using SPD results in simple Kp coefficients diff --git a/apps/hardcodedDesign/Main.cpp b/apps/hardcodedDesign/Main.cpp index a4e18f5316085..328af6d5e5878 100644 --- a/apps/hardcodedDesign/Main.cpp +++ b/apps/hardcodedDesign/Main.cpp @@ -87,9 +87,9 @@ dart::dynamics::Joint* create1DOFJoint(double val, double min, double max, Eigen::Vector3d(1.0, 0.0, 0.0)); // Add the transformation to the joint, set the min/max values and set it to // the skeleton - newJoint->getGenCoord(0)->set_q(val); - newJoint->getGenCoord(0)->set_qMin(min); - newJoint->getGenCoord(0)->set_qMax(max); + newJoint->getGenCoord(0)->setConfig(val); + newJoint->getGenCoord(0)->setConfigMin(min); + newJoint->getGenCoord(0)->setConfigMax(max); return newJoint; } diff --git a/apps/harnessTest/Controller.cpp b/apps/harnessTest/Controller.cpp index 123341160eefb..24398310b813d 100644 --- a/apps/harnessTest/Controller.cpp +++ b/apps/harnessTest/Controller.cpp @@ -62,7 +62,7 @@ Controller::Controller(dynamics::Skeleton* _skel, constraint::ConstraintDynamics mDesiredDofs.resize(nDof); for (int i = 0; i < nDof; i++){ mTorques[i] = 0.0; - mDesiredDofs[i] = mSkel->getGenCoord(i)->get_q(); + mDesiredDofs[i] = mSkel->getGenCoord(i)->getConfig(); } // using SPD results in simple Kp coefficients diff --git a/dart/constraint/ConstraintDynamics.cpp b/dart/constraint/ConstraintDynamics.cpp index 6786ff5169b58..5cdbf18e1e227 100644 --- a/dart/constraint/ConstraintDynamics.cpp +++ b/dart/constraint/ConstraintDynamics.cpp @@ -113,9 +113,9 @@ void ConstraintDynamics::computeConstraintForces() for (int k = 0; k < mSkeletons[i]->getJoint(j)->getNumGenCoords(); k++) { dynamics::GenCoord* genCoord = joint->getGenCoord(k); - double val = genCoord->get_q(); - double ub = genCoord->get_qMax(); - double lb = genCoord->get_qMin(); + double val = genCoord->getConfig(); + double ub = genCoord->getConfigMax(); + double lb = genCoord->getConfigMin(); double violation = 0.0; if (val >= ub) diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 5160e91c055ea..33f0583de7e71 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -64,9 +64,9 @@ BallJoint::~BallJoint() { } inline void BallJoint::updateTransform() { - Eigen::Vector3d q(mCoordinate[0].get_q(), - mCoordinate[1].get_q(), - mCoordinate[2].get_q()); + Eigen::Vector3d q(mCoordinate[0].getConfig(), + mCoordinate[1].getConfig(), + mCoordinate[2].getConfig()); // TODO(JS): This is workaround for Issue #122. mT_Joint = math::expAngular(q); @@ -87,9 +87,9 @@ void BallJoint::updateTransform_Issue122(double _timeStep) { } inline void BallJoint::updateJacobian() { - Eigen::Vector3d q(mCoordinate[0].get_q(), - mCoordinate[1].get_q(), - mCoordinate[2].get_q()); + Eigen::Vector3d q(mCoordinate[0].getConfig(), + mCoordinate[1].getConfig(), + mCoordinate[2].getConfig()); Eigen::Matrix3d J = math::expMapJac(q); @@ -125,12 +125,12 @@ void BallJoint::updateJacobian_Issue122() { } inline void BallJoint::updateJacobianTimeDeriv() { - Eigen::Vector3d q(mCoordinate[0].get_q(), - mCoordinate[1].get_q(), - mCoordinate[2].get_q()); - Eigen::Vector3d dq(mCoordinate[0].get_dq(), - mCoordinate[1].get_dq(), - mCoordinate[2].get_dq()); + Eigen::Vector3d q(mCoordinate[0].getConfig(), + mCoordinate[1].getConfig(), + mCoordinate[2].getConfig()); + Eigen::Vector3d dq(mCoordinate[0].getVel(), + mCoordinate[1].getVel(), + mCoordinate[2].getVel()); Eigen::Matrix3d dJ = math::expMapJacDot(q, dq); diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index fb404dea83df3..a499a619d80d6 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -99,9 +99,9 @@ void EulerJoint::updateTransform() { } void EulerJoint::updateJacobian() { - double q0 = mCoordinate[0].get_q(); - double q1 = mCoordinate[1].get_q(); - double q2 = mCoordinate[2].get_q(); + double q0 = mCoordinate[0].getConfig(); + double q1 = mCoordinate[1].getConfig(); + double q2 = mCoordinate[2].getConfig(); // double c0 = cos(q0); double c1 = cos(q1); @@ -131,12 +131,12 @@ void EulerJoint::updateJacobian() { J2 << 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; #ifndef NDEBUG - if (fabs(mCoordinate[1].get_q()) == DART_PI * 0.5) + if (fabs(mCoordinate[1].getConfig()) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << mName << "]. (" - << mCoordinate[0].get_q() << ", " - << mCoordinate[1].get_q() << ", " - << mCoordinate[2].get_q() << ")" + << mCoordinate[0].getConfig() << ", " + << mCoordinate[1].getConfig() << ", " + << mCoordinate[2].getConfig() << ")" << std::endl; #endif @@ -157,12 +157,12 @@ void EulerJoint::updateJacobian() { J2 << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; #ifndef NDEBUG - if (fabs(mCoordinate[1].get_q()) == DART_PI * 0.5) + if (fabs(mCoordinate[1].getConfig()) == DART_PI * 0.5) std::cout << "Singular configuration in ZYX-euler joint [" << mName << "]. (" - << mCoordinate[0].get_q() << ", " - << mCoordinate[1].get_q() << ", " - << mCoordinate[2].get_q() << ")" + << mCoordinate[0].getConfig() << ", " + << mCoordinate[1].getConfig() << ", " + << mCoordinate[2].getConfig() << ")" << std::endl; #endif @@ -198,13 +198,13 @@ void EulerJoint::updateJacobian() { } void EulerJoint::updateJacobianTimeDeriv() { - double q0 = mCoordinate[0].get_q(); - double q1 = mCoordinate[1].get_q(); - double q2 = mCoordinate[2].get_q(); + double q0 = mCoordinate[0].getConfig(); + double q1 = mCoordinate[1].getConfig(); + double q2 = mCoordinate[2].getConfig(); // double dq0 = mCoordinate[0].get_dq(); - double dq1 = mCoordinate[1].get_dq(); - double dq2 = mCoordinate[2].get_dq(); + double dq1 = mCoordinate[1].getVel(); + double dq2 = mCoordinate[2].getVel(); // double c0 = cos(q0); double c1 = cos(q1); diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 082ae579dab6a..79b5019f604f5 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -86,9 +86,9 @@ void FreeJoint::updateTransform_Issue122(double _timeStep) { } void FreeJoint::updateJacobian() { - Eigen::Vector3d q(mCoordinate[0].get_q(), - mCoordinate[1].get_q(), - mCoordinate[2].get_q()); + Eigen::Vector3d q(mCoordinate[0].getConfig(), + mCoordinate[1].getConfig(), + mCoordinate[2].getConfig()); Eigen::Matrix3d J = math::expMapJac(q); @@ -142,12 +142,12 @@ void FreeJoint::updateJacobian_Issue122() { } void FreeJoint::updateJacobianTimeDeriv() { - Eigen::Vector3d q(mCoordinate[0].get_q(), - mCoordinate[1].get_q(), - mCoordinate[2].get_q()); - Eigen::Vector3d dq(mCoordinate[0].get_dq(), - mCoordinate[1].get_dq(), - mCoordinate[2].get_dq()); + Eigen::Vector3d q(mCoordinate[0].getConfig(), + mCoordinate[1].getConfig(), + mCoordinate[2].getConfig()); + Eigen::Vector3d dq(mCoordinate[0].getVel(), + mCoordinate[1].getVel(), + mCoordinate[2].getVel()); Eigen::Matrix3d dJ = math::expMapJacDot(q, dq); @@ -188,10 +188,10 @@ void FreeJoint::updateJacobianTimeDeriv_Issue122() { void FreeJoint::clampRotation() { for (int i = 0; i < 3; i++) { - if (mCoordinate[i].get_q() > M_PI) - mCoordinate[i].set_q(mCoordinate[i].get_q() - 2*M_PI); - if (mCoordinate[i].get_q() < -M_PI) - mCoordinate[i].set_q(mCoordinate[i].get_q() + 2*M_PI); + if (mCoordinate[i].getConfig() > M_PI) + mCoordinate[i].setConfig(mCoordinate[i].getConfig() - 2*M_PI); + if (mCoordinate[i].getConfig() < -M_PI) + mCoordinate[i].setConfig(mCoordinate[i].getConfig() + 2*M_PI); } } diff --git a/dart/dynamics/GenCoord.cpp b/dart/dynamics/GenCoord.cpp index 1ffa75154695b..400ffce40dfda 100644 --- a/dart/dynamics/GenCoord.cpp +++ b/dart/dynamics/GenCoord.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2011-2013, Georgia Tech Research Corporation + * Copyright (c) 2011-2014, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Sehoon Ha @@ -35,52 +35,263 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include +#include "dart/math/MathTypes.h" #include "dart/dynamics/GenCoord.h" namespace dart { namespace dynamics { +//============================================================================== GenCoord::GenCoord() - : q(0.0), - dq(0.0), - ddq(0.0), - tau(0.0), - qMin(-std::numeric_limits::infinity()), - dqMin(-std::numeric_limits::infinity()), - ddqMin(-std::numeric_limits::infinity()), - tauMin(-std::numeric_limits::infinity()), - qMax(std::numeric_limits::infinity()), - dqMax(std::numeric_limits::infinity()), - ddqMax(std::numeric_limits::infinity()), - tauMax(std::numeric_limits::infinity()), - DqDp(0.0), - DdqDp(0.0), - DddqDp(0.0), - DtauDp(0.0), - // mSkelIndex(-1), - // mJoint(NULL), - mName("dof") { -} - -GenCoord::~GenCoord() { -} - -void GenCoord::setName(const std::string& _name) { + : mConfig(0.0), + mVel(0.0), + mAcc(0.0), + mForce(0.0), + mConfigMin(-DART_DBL_INF), + mVelMin(-DART_DBL_INF), + mAccMin(-DART_DBL_INF), + mForceMin(-DART_DBL_INF), + mConfigMax(DART_DBL_INF), + mVelMax(DART_DBL_INF), + mAccMax(DART_DBL_INF), + mForceMax(DART_DBL_INF), + mConfigDeriv(0.0), + mVelDeriv(0.0), + mAccDeriv(0.0), + mForceDeriv(0.0), + mSkelIndex(0u), + mName("dof") +{ +} + +//============================================================================== +GenCoord::~GenCoord() +{ +} + +//============================================================================== +void GenCoord::setName(const std::string& _name) +{ mName = _name; } -const std::string& GenCoord::getName() const { +//============================================================================== +const std::string& GenCoord::getName() const +{ return mName; } -int GenCoord::getSkeletonIndex() const { +//============================================================================== +size_t GenCoord::getSkeletonIndex() const +{ return mSkelIndex; } -void GenCoord::setSkeletonIndex(int _idx) { +//============================================================================== +double GenCoord::getConfig() const +{ + return mConfig; +} + +//============================================================================== +double GenCoord::getVel() const +{ + return mVel; +} + +//============================================================================== +double GenCoord::getAcc() const +{ + return mAcc; +} + +//============================================================================== +double GenCoord::getForce() const +{ + return mForce; +} + +//============================================================================== +double GenCoord::getConfigMin() const +{ + return mConfigMin; +} + +//============================================================================== +double GenCoord::getVelMin() const +{ + return mVelMin; +} + +//============================================================================== +double GenCoord::getAccMin() const +{ + return mAccMin; +} + +//============================================================================== +double GenCoord::getForceMin() const +{ + return mForceMin; +} + +//============================================================================== +double GenCoord::getConfigMax() const +{ + return mConfigMax; +} + +//============================================================================== +void GenCoord::setConfigDeriv(double _configDeriv) +{ + assert(_configDeriv == _configDeriv); + mConfigDeriv = _configDeriv; +} + +//============================================================================== +double GenCoord::getConfigDeriv() const +{ + return mConfigDeriv; +} + +//============================================================================== +double GenCoord::getVelMax() const +{ + return mVelMax; +} + +//============================================================================== +void GenCoord::setVelDeriv(double _velDeriv) +{ + assert(_velDeriv == _velDeriv); + mVelDeriv = _velDeriv; +} + +//============================================================================== +double GenCoord::getVelDeriv() const +{ + return mVelDeriv; +} + +//============================================================================== +double GenCoord::getAccMax() const +{ + return mAccMax; +} + +//============================================================================== +void GenCoord::setAccDeriv(double _accDeriv) +{ + assert(_accDeriv == _accDeriv); + mAccDeriv = _accDeriv; +} + +//============================================================================== +double GenCoord::getAccDeriv() const +{ + return mAccDeriv; +} + +//============================================================================== +double GenCoord::getForceMax() const +{ + return mForceMax; +} + +//============================================================================== +void GenCoord::setForceDeriv(double _forceDeriv) +{ + assert(_forceDeriv == _forceDeriv); + mForceDeriv = _forceDeriv; +} + +//============================================================================== +double GenCoord::getForceDeriv() const +{ + return mForceDeriv; +} + +//============================================================================== +void GenCoord::setConfig(double _config) +{ + assert(_config == _config); + mConfig = _config; +} + +//============================================================================== +void GenCoord::setVel(double _vel) +{ + assert(_vel == _vel); + mVel = _vel; +} + +//============================================================================== +void GenCoord::setAcc(double _acc) +{ + assert(_acc == _acc); + mAcc = _acc; +} + +//============================================================================== +void GenCoord::setForce(double _force) +{ + assert(_force == _force); + mForce = _force; +} + +//============================================================================== +void GenCoord::setConfigMin(double _configMin) +{ + mConfigMin = _configMin; +} + +//============================================================================== +void GenCoord::setVelMin(double _velMin) +{ + mVelMin = _velMin; +} + +//============================================================================== +void GenCoord::setAccMin(double _accMin) +{ + mAccMin = _accMin; +} + +//============================================================================== +void GenCoord::setForceMin(double _forceMin) +{ + mForceMin = _forceMin; +} + +//============================================================================== +void GenCoord::setConfigMax(double _configMax) +{ + mConfigMax = _configMax; +} + +//============================================================================== +void GenCoord::setVelMax(double _velMax) +{ + mVelMax = _velMax; +} + +//============================================================================== +void GenCoord::setAccMax(double _accMax) +{ + mAccMax = _accMax; +} + +//============================================================================== +void GenCoord::setForceMax(double _forceMax) +{ + mForceMax = _forceMax; +} + +//============================================================================== +void GenCoord::setSkeletonIndex(size_t _idx) +{ mSkelIndex = _idx; } diff --git a/dart/dynamics/GenCoord.h b/dart/dynamics/GenCoord.h index b8e23526536ec..ea65adf2982b0 100644 --- a/dart/dynamics/GenCoord.h +++ b/dart/dynamics/GenCoord.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2011-2013, Georgia Tech Research Corporation + * Copyright (c) 2011-2014, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Sehoon Ha , @@ -38,7 +38,7 @@ #ifndef DART_DYNAMICS_GENCOORD_H_ #define DART_DYNAMICS_GENCOORD_H_ -#include +#include #include #include "dart/math/Geometry.h" @@ -46,102 +46,186 @@ namespace dart { namespace dynamics { -class Joint; - -/// \brief Generalized coordinate. -/// A set of generalized coordiante describes the configuration of a system. -class GenCoord { +/// \brief Generalized coordinate +class GenCoord +{ public: - /// \brief + /// \brief Constructor GenCoord(); - /// \brief + /// \brief Destructor virtual ~GenCoord(); - /// \brief + /// \brief Set name void setName(const std::string& _name); - /// \brief + /// \brief Get name const std::string& getName() const; - /// \brief - int getSkeletonIndex() const; + /// \brief Set skeleton index + void setSkeletonIndex(size_t _idx); - /// \brief - void setSkeletonIndex(int _idx); + /// \brief Get skeleton index + size_t getSkeletonIndex() const; -public: - double get_q() const { return q; } ///< Configuration - double get_dq() const { return dq; } ///< Velocity - double get_ddq() const { return ddq; } ///< Acceleration - double get_tau() const { return tau; } ///< torque or force - - double get_qMin() const { return qMin; } ///< Minimum value of q - double get_dqMin() const { return dqMin; } ///< Minimum value of dq - double get_ddqMin() const { return ddqMin; } ///< Minimum value of ddq - double get_tauMin() const { return tauMin; } ///< Minimum value of tau - - double get_qMax() const { return qMax; } ///< Maximum value of q - double get_dqMax() const { return dqMax; } ///< Maximum value of dq - double get_ddqMax() const { return ddqMax; } ///< Maximum value of ddq - double get_tauMax() const { return tauMax; } ///< Maximum value of tau - - void set_q(double _q) { assert(_q == _q); q = _q; } - void set_dq(double _dq) { assert(_dq == _dq); dq = _dq; } - void set_ddq(double _ddq) { assert(_ddq == _ddq); ddq = _ddq; } - void set_tau(double _tau) { assert(_tau == _tau); tau = _tau; } - - void set_qMin(double _qMin) { qMin = _qMin; } - void set_dqMin(double _dqMin) { dqMin = _dqMin; } - void set_ddqMin(double _ddqMin) { ddqMin = _ddqMin; } - void set_tauMin(double _tauMin) { tauMin = _tauMin; } - - void set_qMax(double _qMax) { qMax = _qMax; } - void set_dqMax(double _dqMax) { dqMax = _dqMax; } - void set_ddqMax(double _ddqMax) { ddqMax = _ddqMax; } - void set_tauMax(double _tauMax) { tauMax = _tauMax; } + //---------------------------- Configuration --------------------------------- + /// \brief Set configuration + void setConfig(double _config); + + /// \brief Get configuration + double getConfig() const; + + /// \brief Set lower bound for configuration + void setConfigMin(double _configMin); + + /// \brief Get lower bound for configuration + double getConfigMin() const; + + /// \brief Set upper bound for configuration + void setConfigMax(double _configMax); + + /// \brief Get upper bound for configuration + double getConfigMax() const; + + /// \brief Set derivative w.r.t. arbitrary scalar value + void setConfigDeriv(double _configDeriv); + + /// \brief Get derivative w.r.t. arbitrary scalar value + double getConfigDeriv() const; + + //------------------------------ Velocity ------------------------------------ + /// \brief Set generalized velocity + void setVel(double _vel); + + /// \brief Get generalized velocity + double getVel() const; + + /// \brief Set lower bound for generalized velocity + void setVelMin(double _velMin); + + /// \brief Get lower bound for generalized velocity + double getVelMin() const; + + /// \brief Set upper bound for generalized velocity + void setVelMax(double _velMax); + + /// \brief Get upper bound for generalized velocity + double getVelMax() const; + + /// \brief Set derivative w.r.t. arbitrary scalar value + void setVelDeriv(double _velDeriv); + + /// \brief Get derivative w.r.t. arbitrary scalar value + double getVelDeriv() const; + + //---------------------------- Acceleration ---------------------------------- + /// \brief Set generalized acceleration + void setAcc(double _acc); + + /// \brief Get generalized acceleration + double getAcc() const; + + /// \brief Set lower bound for generalized acceleration + void setAccMin(double _accMin); + + /// \brief Get lower bound for generalized acceleration + double getAccMin() const; + + /// \brief Set upper bound for generalized acceleration + void setAccMax(double _accMax); + + /// \brief Get upper bound for generalized acceleration + double getAccMax() const; + + /// \brief Set derivative w.r.t. arbitrary scalar value + void setAccDeriv(double _accDeriv); + + /// \brief Get derivative w.r.t. arbitrary scalar value + double getAccDeriv() const; + + //------------------------------- Force -------------------------------------- + /// \brief Set generalized force + void setForce(double _force); + + /// \brief Get generalized force + double getForce() const; + + /// \brief Set lower bound for generalized force + void setForceMin(double _forceMin); + + /// \brief Get lower bound for generalized force + double getForceMin() const; + + /// \brief Set upper bound for generalized force + void setForceMax(double _forceMax); + + /// \brief Get upper bound for generalized force + double getForceMax() const; + + /// \brief Set derivative w.r.t. arbitrary scalar value + void setForceDeriv(double _forceDeriv); + + /// \brief Get derivative w.r.t. arbitrary scalar value + double getForceDeriv() const; protected: - /// \brief + /// \brief Name std::string mName; - /// \brief Unique to dof in model. - int mSkelIndex; - - /// \brief Joint to which it belongs. - // Joint *mJoint; - - //-------------------------------------------------------------------------- - // Position - //-------------------------------------------------------------------------- - double q; ///< Position - double qMin; ///< Min value allowed. - double qMax; ///< Max value allowed. - double DqDp; ///< derivatives w.r.t. an arbitrary scalr variable p - - //-------------------------------------------------------------------------- - // Velocity - //-------------------------------------------------------------------------- - double dq; ///< Velocity - double dqMin; ///< Min value allowed. - double dqMax; ///< Max value allowed. - double DdqDp; ///< derivatives w.r.t. an arbitrary scalr variable p - - //-------------------------------------------------------------------------- - // Force (torque) - //-------------------------------------------------------------------------- - double ddq; ///< Acceleration - double ddqMin; ///< Min value allowed. - double ddqMax; ///< Max value allowed. - double DddqDp; ///< derivatives w.r.t. an arbitrary scalr variable p - - //-------------------------------------------------------------------------- - // Force (torque) - //-------------------------------------------------------------------------- - double tau; ///< Force (torque) - double tauMin; ///< Min value allowed. - double tauMax; ///< Max value allowed. - double DtauDp; ///< derivatives w.r.t. an arbitrary scalr variable p + /// \brief Index in Skeleton + size_t mSkelIndex; + + //----------------------------- Configuration -------------------------------- + /// \brief Configuration + double mConfig; + + /// \brief Lower bound for configuration + double mConfigMin; + + /// \brief Upper bound for configuration + double mConfigMax; + + /// \brief Derivatives w.r.t. an arbitrary scalr variable + double mConfigDeriv; + + //------------------------------- Velocity ----------------------------------- + /// \brief Generalized velocity + double mVel; + + /// \brief Min value allowed. + double mVelMin; + + /// \brief Max value allowed. + double mVelMax; + + /// \brief Derivatives w.r.t. an arbitrary scalr variable + double mVelDeriv; + + //----------------------------- Acceleration --------------------------------- + /// \brief Generalized acceleration + double mAcc; + + /// \brief Min value allowed. + double mAccMin; + + /// \brief upper bound for generalized acceleration + double mAccMax; + + /// \brief Derivatives w.r.t. an arbitrary scalr variable + double mAccDeriv; + + //-------------------------------- Force ------------------------------------- + /// \brief Generalized force + double mForce; + + /// \brief Min value allowed. + double mForceMin; + + /// \brief Max value allowed. + double mForceMax; + + /// \brief Derivatives w.r.t. an arbitrary scalr variable + double mForceDeriv; }; } // namespace dynamics diff --git a/dart/dynamics/GenCoordSystem.cpp b/dart/dynamics/GenCoordSystem.cpp index 0739aa3e48f69..e2c0ca4742e89 100644 --- a/dart/dynamics/GenCoordSystem.cpp +++ b/dart/dynamics/GenCoordSystem.cpp @@ -62,13 +62,14 @@ GenCoord* GenCoordSystem::getGenCoord(const std::string& _name) const { return NULL; } -void GenCoordSystem::set_q(const Eigen::VectorXd& _q) { +void GenCoordSystem::set_q(const Eigen::VectorXd& _q) +{ assert(_q.size() == getNumGenCoords()); int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_q(_q[i]); + mGenCoords[i]->setConfig(_q[i]); } void GenCoordSystem::set_dq(const Eigen::VectorXd& _dq) { @@ -77,7 +78,7 @@ void GenCoordSystem::set_dq(const Eigen::VectorXd& _dq) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_dq(_dq[i]); + mGenCoords[i]->setVel(_dq[i]); } void GenCoordSystem::set_ddq(const Eigen::VectorXd& _ddq) { @@ -86,7 +87,7 @@ void GenCoordSystem::set_ddq(const Eigen::VectorXd& _ddq) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_ddq(_ddq[i]); + mGenCoords[i]->setAcc(_ddq[i]); } void GenCoordSystem::set_tau(const Eigen::VectorXd& _tau) { @@ -95,16 +96,16 @@ void GenCoordSystem::set_tau(const Eigen::VectorXd& _tau) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_tau(_tau[i]); + mGenCoords[i]->setForce(_tau[i]); } -void GenCoordSystem::set_qMin(const Eigen::VectorXd& _qMin) { +void GenCoordSystem::setGenPosMin(const Eigen::VectorXd& _qMin) { assert(_qMin.size() == getNumGenCoords()); int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_qMin(_qMin[i]); + mGenCoords[i]->setConfigMin(_qMin[i]); } void GenCoordSystem::set_dqMin(const Eigen::VectorXd& _dqMin) { @@ -113,7 +114,7 @@ void GenCoordSystem::set_dqMin(const Eigen::VectorXd& _dqMin) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_dqMin(_dqMin[i]); + mGenCoords[i]->setVelMin(_dqMin[i]); } void GenCoordSystem::set_ddqMin(const Eigen::VectorXd& _ddqMin) { @@ -122,7 +123,7 @@ void GenCoordSystem::set_ddqMin(const Eigen::VectorXd& _ddqMin) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_ddqMin(_ddqMin[i]); + mGenCoords[i]->setAccMin(_ddqMin[i]); } void GenCoordSystem::set_tauMin(const Eigen::VectorXd& _tauMin) { @@ -131,17 +132,17 @@ void GenCoordSystem::set_tauMin(const Eigen::VectorXd& _tauMin) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_tauMin(_tauMin[i]); + mGenCoords[i]->setForceMin(_tauMin[i]); } -void GenCoordSystem::set_qMax(const Eigen::VectorXd& _qMax) { +void GenCoordSystem::setGenPosMax(const Eigen::VectorXd& _qMax) { assert(_qMax.size() == getNumGenCoords()); int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_qMax(_qMax[i]); + mGenCoords[i]->setConfigMax(_qMax[i]); } void GenCoordSystem::set_dqMax(const Eigen::VectorXd& _dqMax) { @@ -150,7 +151,7 @@ void GenCoordSystem::set_dqMax(const Eigen::VectorXd& _dqMax) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_dqMax(_dqMax[i]); + mGenCoords[i]->setVelMax(_dqMax[i]); } void GenCoordSystem::set_ddqMax(const Eigen::VectorXd& _ddqMax) { @@ -159,7 +160,7 @@ void GenCoordSystem::set_ddqMax(const Eigen::VectorXd& _ddqMax) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_ddqMax(_ddqMax[i]); + mGenCoords[i]->setAccMax(_ddqMax[i]); } void GenCoordSystem::set_tauMax(const Eigen::VectorXd& _tauMax) { @@ -168,7 +169,7 @@ void GenCoordSystem::set_tauMax(const Eigen::VectorXd& _tauMax) { int size = getNumGenCoords(); for (int i = 0; i < size; ++i) - mGenCoords[i]->set_tauMax(_tauMax[i]); + mGenCoords[i]->setForceMax(_tauMax[i]); } Eigen::VectorXd GenCoordSystem::get_q() const { @@ -176,7 +177,7 @@ Eigen::VectorXd GenCoordSystem::get_q() const { Eigen::VectorXd q = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - q(i) = mGenCoords[i]->get_q(); + q(i) = mGenCoords[i]->getConfig(); } return q; @@ -187,7 +188,7 @@ Eigen::VectorXd GenCoordSystem::get_dq() const { Eigen::VectorXd dq = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - dq(i) = mGenCoords[i]->get_dq(); + dq(i) = mGenCoords[i]->getVel(); } return dq; @@ -198,7 +199,7 @@ Eigen::VectorXd GenCoordSystem::get_ddq() const { Eigen::VectorXd ddq = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - ddq(i) = mGenCoords[i]->get_ddq(); + ddq(i) = mGenCoords[i]->getAcc(); } return ddq; @@ -209,7 +210,7 @@ Eigen::VectorXd GenCoordSystem::get_tau() const { Eigen::VectorXd tau = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - tau(i) = mGenCoords[i]->get_tau(); + tau(i) = mGenCoords[i]->getForce(); } return tau; @@ -220,7 +221,7 @@ Eigen::VectorXd GenCoordSystem::get_qMax() const { Eigen::VectorXd q = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - q(i) = mGenCoords[i]->get_qMax(); + q(i) = mGenCoords[i]->getConfigMax(); } return q; @@ -231,7 +232,7 @@ Eigen::VectorXd GenCoordSystem::get_dqMax() const { Eigen::VectorXd dq = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - dq(i) = mGenCoords[i]->get_dqMax(); + dq(i) = mGenCoords[i]->getVelMax(); } return dq; @@ -242,7 +243,7 @@ Eigen::VectorXd GenCoordSystem::get_ddqMax() const { Eigen::VectorXd ddq = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - ddq(i) = mGenCoords[i]->get_ddqMax(); + ddq(i) = mGenCoords[i]->getAccMax(); } return ddq; @@ -253,7 +254,7 @@ Eigen::VectorXd GenCoordSystem::get_tauMax() const { Eigen::VectorXd tau = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - tau(i) = mGenCoords[i]->get_tauMax(); + tau(i) = mGenCoords[i]->getForceMax(); } return tau; @@ -264,7 +265,7 @@ Eigen::VectorXd GenCoordSystem::get_qMin() const { Eigen::VectorXd q = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - q(i) = mGenCoords[i]->get_qMin(); + q(i) = mGenCoords[i]->getConfigMin(); } return q; @@ -275,7 +276,7 @@ Eigen::VectorXd GenCoordSystem::get_dqMin() const { Eigen::VectorXd dq = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - dq(i) = mGenCoords[i]->get_dqMin(); + dq(i) = mGenCoords[i]->getVelMin(); } return dq; @@ -286,7 +287,7 @@ Eigen::VectorXd GenCoordSystem::get_ddqMin() const { Eigen::VectorXd ddq = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - ddq(i) = mGenCoords[i]->get_ddqMin(); + ddq(i) = mGenCoords[i]->getAccMin(); } return ddq; @@ -297,7 +298,7 @@ Eigen::VectorXd GenCoordSystem::get_tauMin() const { Eigen::VectorXd tau = Eigen::VectorXd::Zero(size); for (int i = 0; i < size; ++i) { - tau(i) = mGenCoords[i]->get_tauMin(); + tau(i) = mGenCoords[i]->getForceMin(); } return tau; diff --git a/dart/dynamics/GenCoordSystem.h b/dart/dynamics/GenCoordSystem.h index dde13ff992acc..4fa72b92c3280 100644 --- a/dart/dynamics/GenCoordSystem.h +++ b/dart/dynamics/GenCoordSystem.h @@ -1,5 +1,5 @@ /* - * Copyright (c) 2013, Georgia Tech Research Corporation + * Copyright (c) 2013-2014, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Jeongseok Lee @@ -48,26 +48,24 @@ namespace dart { namespace dynamics { /// \brief System is a base class for every classes that has Dofs. -class GenCoordSystem { +class GenCoordSystem +{ public: - /// \brief + /// \brief Constructor GenCoordSystem(); - /// \brief + /// \brief Destructor virtual ~GenCoordSystem(); - /// \brief + /// \brief Get number of generalized coordinates in this system int getNumGenCoords() const; - /// \brief + /// \brief Get generalized coordinate whose index is _idx GenCoord* getGenCoord(int _idx) const; - /// \brief + /// \brief Get gneralized coordinate whose name is _name GenCoord* getGenCoord(const std::string& _name) const; - //-------------------------------------------------------------------------- - // - //-------------------------------------------------------------------------- /// \brief Set generalized coordinate vector void set_q(const Eigen::VectorXd& _q); @@ -80,32 +78,64 @@ class GenCoordSystem { /// \brief Set generalized force vector (internal forces) void set_tau(const Eigen::VectorXd& _tau); - void set_qMin(const Eigen::VectorXd& _qMin); + /// \brief + void setGenPosMin(const Eigen::VectorXd& _qMin); + + /// \brief void set_dqMin(const Eigen::VectorXd& _dqMin); + + /// \brief void set_ddqMin(const Eigen::VectorXd& _ddqMin); + + /// \brief void set_tauMin(const Eigen::VectorXd& _tauMin); - void set_qMax(const Eigen::VectorXd& _qMax); + /// \brief + void setGenPosMax(const Eigen::VectorXd& _qMax); + + /// \brief void set_dqMax(const Eigen::VectorXd& _dqMax); + + /// \brief void set_ddqMax(const Eigen::VectorXd& _ddqMax); + + /// \brief void set_tauMax(const Eigen::VectorXd& _tauMax); - //-------------------------------------------------------------------------- - // - //-------------------------------------------------------------------------- + /// \brief Eigen::VectorXd get_q() const; + + /// \brief Eigen::VectorXd get_dq() const; + + /// \brief Eigen::VectorXd get_ddq() const; + + /// \brief Eigen::VectorXd get_tau() const; + /// \brief Eigen::VectorXd get_qMin() const; + + /// \brief Eigen::VectorXd get_dqMin() const; + + /// \brief Eigen::VectorXd get_ddqMin() const; + + /// \brief Eigen::VectorXd get_tauMin() const; + /// \brief Eigen::VectorXd get_qMax() const; + + /// \brief Eigen::VectorXd get_dqMax() const; + + /// \brief Eigen::VectorXd get_ddqMax() const; + + /// \brief Eigen::VectorXd get_tauMax() const; protected: diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 034f5af900c9a..69021cd3fc806 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -152,7 +152,7 @@ Eigen::VectorXd Joint::getDampingForces() const { Eigen::VectorXd dampingForce(numDofs); for (int i = 0; i < numDofs; ++i) - dampingForce(i) = -mDampingCoefficient[i] * getGenCoord(i)->get_dq(); + dampingForce(i) = -mDampingCoefficient[i] * getGenCoord(i)->getVel(); return dampingForce; } @@ -171,13 +171,13 @@ double Joint::getSpringStiffness(int _idx) const { void Joint::setRestPosition(int _idx, double _q0) { assert(0 <= _idx && _idx < getNumGenCoords()); - if (getGenCoord(_idx)->get_qMin() > _q0 - || getGenCoord(_idx)->get_qMax() < _q0) + if (getGenCoord(_idx)->getConfigMin() > _q0 + || getGenCoord(_idx)->getConfigMax() < _q0) { dtwarn << "Rest position of joint[" << getName() << "], " << _q0 << ", is out of the limit range[" - << getGenCoord(_idx)->get_qMin() << ", " - << getGenCoord(_idx)->get_qMax() << "] in index[" << _idx + << getGenCoord(_idx)->getConfigMin() << ", " + << getGenCoord(_idx)->getConfigMax() << "] in index[" << _idx << "].\n"; } @@ -194,8 +194,8 @@ Eigen::VectorXd Joint::getSpringForces(double _timeStep) const { Eigen::VectorXd springForce(dof); for (int i = 0; i < dof; ++i) { springForce(i) = - -mSpringStiffness[i] * (getGenCoord(i)->get_q() - + getGenCoord(i)->get_dq() * _timeStep + -mSpringStiffness[i] * (getGenCoord(i)->getConfig() + + getGenCoord(i)->getVel() * _timeStep - mRestPosition[i]); } return springForce; diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 1059feaccaa23..1553836a4791f 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -68,9 +68,9 @@ PlanarJoint::~PlanarJoint() void PlanarJoint::updateTransform() { mT = mT_ParentBodyToJoint - * Eigen::Translation3d(mTranAxis1 * mCoordinate[1].get_q()) - * Eigen::Translation3d(mTranAxis2 * mCoordinate[2].get_q()) - * math::expAngular (mRotAxis * mCoordinate[0].get_q()) + * Eigen::Translation3d(mTranAxis1 * mCoordinate[1].getConfig()) + * Eigen::Translation3d(mTranAxis2 * mCoordinate[2].getConfig()) + * math::expAngular (mRotAxis * mCoordinate[0].getConfig()) * mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); @@ -86,7 +86,7 @@ void PlanarJoint::updateJacobian() mS.col(0) = math::AdTJac(mT_ChildBodyToJoint, J.col(0)); mS.rightCols<2>() = math::AdTJac(mT_ChildBodyToJoint - * math::expAngular(mRotAxis * -mCoordinate[0].get_q()), + * math::expAngular(mRotAxis * -mCoordinate[0].getConfig()), J.rightCols<2>()); assert(!math::isNan(mS)); @@ -100,17 +100,17 @@ void PlanarJoint::updateJacobianTimeDeriv() J.block<3, 1>(3, 2) = mTranAxis2; mdS.col(1) - = -math::ad(mS.col(0)*mCoordinate[0].get_dq(), + = -math::ad(mS.col(0)*mCoordinate[0].getVel(), math::AdT(mT_ChildBodyToJoint * math::expAngular(mRotAxis - * -mCoordinate[0].get_q()), + * -mCoordinate[0].getConfig()), J.col(1))); mdS.col(2) - = -math::ad(mS.col(0)*mCoordinate[0].get_dq(), + = -math::ad(mS.col(0)*mCoordinate[0].getVel(), math::AdT(mT_ChildBodyToJoint * math::expAngular(mRotAxis - * -mCoordinate[0].get_q()), + * -mCoordinate[0].getConfig()), J.col(2))); assert(mdS.col(0) == Eigen::Vector6d::Zero()); diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index aafbdb310c3ee..c2bdb08fc17e9 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -72,7 +72,7 @@ const Eigen::Vector3d&PrismaticJoint::getAxis() const { void PrismaticJoint::updateTransform() { mT = mT_ParentBodyToJoint - * Eigen::Translation3d(mAxis * mCoordinate.get_q()) + * Eigen::Translation3d(mAxis * mCoordinate.getConfig()) * mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 7a6b02b370830..e595f3c95afda 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -73,7 +73,7 @@ const Eigen::Vector3d&RevoluteJoint::getAxis() const { void RevoluteJoint::updateTransform() { mT = mT_ParentBodyToJoint - * math::expAngular(mAxis * mCoordinate.get_q()) + * math::expAngular(mAxis * mCoordinate.getConfig()) * mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index a04041cc7f1b5..0a62f4ff4fb7d 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -85,7 +85,7 @@ void ScrewJoint::updateTransform() { S.head<3>() = mAxis; S.tail<3>() = mAxis*mPitch/DART_2PI; mT = mT_ParentBodyToJoint - * math::expMap(S*mCoordinate.get_q()) + * math::expMap(S*mCoordinate.getConfig()) * mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -104,10 +104,10 @@ void ScrewJoint::updateJacobianTimeDeriv() { } void ScrewJoint::clampRotation() { - if (mCoordinate.get_q() > M_PI) - mCoordinate.set_q(mCoordinate.get_q() - 2*M_PI); - if (mCoordinate.get_q() < -M_PI) - mCoordinate.set_q(mCoordinate.get_q() + 2*M_PI); + if (mCoordinate.getConfig() > M_PI) + mCoordinate.setConfig(mCoordinate.getConfig() - 2*M_PI); + if (mCoordinate.getConfig() < -M_PI) + mCoordinate.setConfig(mCoordinate.getConfig() + 2*M_PI); } } // namespace dynamics diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index f8bf6736be8eb..8e530d2d794ba 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -243,7 +243,7 @@ Eigen::VectorXd Skeleton::getConfig(const std::vector& _id) const { Eigen::VectorXd q(_id.size()); for (unsigned int i = 0; i < _id.size(); i++) - q[i] = mGenCoords[_id[i]]->get_q(); + q[i] = mGenCoords[_id[i]]->getConfig(); return q; } @@ -255,7 +255,7 @@ Eigen::VectorXd Skeleton::getConfig() const { void Skeleton::setConfig(const std::vector& _id, const Eigen::VectorXd& _config) { for ( unsigned int i = 0; i < _id.size(); i++ ) - mGenCoords[_id[i]]->set_q(_config(i)); + mGenCoords[_id[i]]->setConfig(_config(i)); for (std::vector::iterator it = mBodyNodes.begin(); it != mBodyNodes.end(); ++it) { diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 2e526b4304600..aa29ae2af8be0 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -84,8 +84,8 @@ const Eigen::Vector3d& UniversalJoint::getAxis2() const { void UniversalJoint::updateTransform() { mT = mT_ParentBodyToJoint - * Eigen::AngleAxisd(mCoordinate[0].get_q(), mAxis[0]) - * Eigen::AngleAxisd(mCoordinate[1].get_q(), mAxis[1]) + * Eigen::AngleAxisd(mCoordinate[0].getConfig(), mAxis[0]) + * Eigen::AngleAxisd(mCoordinate[1].getConfig(), mAxis[1]) * mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } @@ -93,15 +93,15 @@ void UniversalJoint::updateTransform() { void UniversalJoint::updateJacobian() { mS.col(0) = math::AdTAngular(mT_ChildBodyToJoint * math::expAngular( - -mAxis[1]*mCoordinate[1].get_q()), mAxis[0]); + -mAxis[1]*mCoordinate[1].getConfig()), mAxis[0]); mS.col(1) = math::AdTAngular(mT_ChildBodyToJoint, mAxis[1]); assert(!math::isNan(mS)); } void UniversalJoint::updateJacobianTimeDeriv() { - mdS.col(0) = -math::ad(mS.col(1)*mCoordinate[1].get_dq(), + mdS.col(0) = -math::ad(mS.col(1)*mCoordinate[1].getVel(), math::AdTAngular(mT_ChildBodyToJoint - * math::expAngular(-mAxis[1]*mCoordinate[1].get_q()), + * math::expAngular(-mAxis[1]*mCoordinate[1].getConfig()), mAxis[0])); // mdS.col(1) = setZero(); assert(!math::isNan(mdS.col(0))); @@ -110,10 +110,10 @@ void UniversalJoint::updateJacobianTimeDeriv() { void UniversalJoint::clampRotation() { for (int i = 0; i < 2; i++) { - if (mCoordinate[i].get_q() > M_PI) - mCoordinate[i].set_q(mCoordinate[i].get_q() - 2*M_PI); - if (mCoordinate[i].get_q() < -M_PI) - mCoordinate[i].set_q(mCoordinate[i].get_q() + 2*M_PI); + if (mCoordinate[i].getConfig() > M_PI) + mCoordinate[i].setConfig(mCoordinate[i].getConfig() - 2*M_PI); + if (mCoordinate[i].getConfig() < -M_PI) + mCoordinate[i].setConfig(mCoordinate[i].getConfig() + 2*M_PI); } } diff --git a/dart/math/MathTypes.h b/dart/math/MathTypes.h index b113bd32aaf5f..0d5785debb9d3 100644 --- a/dart/math/MathTypes.h +++ b/dart/math/MathTypes.h @@ -37,6 +37,7 @@ #ifndef DART_MATH_MATHTYPES_H_ #define DART_MATH_MATHTYPES_H_ +#include #include #include @@ -65,6 +66,8 @@ #define DART_1_1260 (0.000793650793650793650794) // = 1 / 1260 #define DART_4_3 (1.33333333333333333333) // = 4 / 3 +#define DART_DBL_INF (std::numeric_limits::infinity()) + //------------------------------------------------------------------------------ // Macros //------------------------------------------------------------------------------ diff --git a/dart/planning/RRT.cpp b/dart/planning/RRT.cpp index 7e752d93481ce..bea35f118cde9 100644 --- a/dart/planning/RRT.cpp +++ b/dart/planning/RRT.cpp @@ -198,7 +198,7 @@ VectorXd RRT::getRandomConfig() { // configuration vectors (and returns ref to it) VectorXd config(ndim); for (int i = 0; i < ndim; ++i) { - config[i] = randomInRange(robot->getGenCoord(dofs[i])->get_qMin(), robot->getGenCoord(dofs[i])->get_qMax()); + config[i] = randomInRange(robot->getGenCoord(dofs[i])->getConfigMin(), robot->getGenCoord(dofs[i])->getConfigMax()); } return config; } diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index c8eeec3fac445..6ba50d122b31a 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -567,13 +567,13 @@ dynamics::RevoluteJoint* SkelParser::readRevoluteJoint( // lower if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newRevoluteJoint->getGenCoord(0)->set_qMin(lower); + newRevoluteJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newRevoluteJoint->getGenCoord(0)->set_qMax(upper); + newRevoluteJoint->getGenCoord(0)->setConfigMax(upper); } } } else { @@ -631,13 +631,13 @@ dynamics::PrismaticJoint* SkelParser::readPrismaticJoint( // lower if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newPrismaticJoint->getGenCoord(0)->set_qMin(lower); + newPrismaticJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newPrismaticJoint->getGenCoord(0)->set_qMax(upper); + newPrismaticJoint->getGenCoord(0)->setConfigMax(upper); } } } else { @@ -701,13 +701,13 @@ dynamics::ScrewJoint* SkelParser::readScrewJoint( // lower if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newScrewJoint->getGenCoord(0)->set_qMin(lower); + newScrewJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newScrewJoint->getGenCoord(0)->set_qMax(upper); + newScrewJoint->getGenCoord(0)->setConfigMax(upper); } } } else { @@ -765,13 +765,13 @@ dynamics::UniversalJoint* SkelParser::readUniversalJoint( // lower if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newUniversalJoint->getGenCoord(0)->set_qMin(lower); + newUniversalJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newUniversalJoint->getGenCoord(0)->set_qMax(upper); + newUniversalJoint->getGenCoord(0)->setConfigMax(upper); } } } else { @@ -802,13 +802,13 @@ dynamics::UniversalJoint* SkelParser::readUniversalJoint( // lower if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newUniversalJoint->getGenCoord(1)->set_qMin(lower); + newUniversalJoint->getGenCoord(1)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newUniversalJoint->getGenCoord(1)->set_qMax(upper); + newUniversalJoint->getGenCoord(1)->setConfigMax(upper); } } } else { @@ -893,13 +893,13 @@ dynamics::EulerJoint* SkelParser::readEulerJoint( // lower if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newEulerJoint->getGenCoord(0)->set_qMin(lower); + newEulerJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newEulerJoint->getGenCoord(0)->set_qMax(upper); + newEulerJoint->getGenCoord(0)->setConfigMax(upper); } } } @@ -924,13 +924,13 @@ dynamics::EulerJoint* SkelParser::readEulerJoint( // lower if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newEulerJoint->getGenCoord(1)->set_qMin(lower); + newEulerJoint->getGenCoord(1)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newEulerJoint->getGenCoord(1)->set_qMax(upper); + newEulerJoint->getGenCoord(1)->setConfigMax(upper); } } } @@ -955,13 +955,13 @@ dynamics::EulerJoint* SkelParser::readEulerJoint( // lower if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newEulerJoint->getGenCoord(2)->set_qMin(lower); + newEulerJoint->getGenCoord(2)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newEulerJoint->getGenCoord(2)->set_qMax(upper); + newEulerJoint->getGenCoord(2)->setConfigMax(upper); } } } diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index c543a9ba6435d..fcf673987ceba 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -642,14 +642,14 @@ dynamics::RevoluteJoint* SdfParser::readRevoluteJoint( if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newRevoluteJoint->getGenCoord(0)->set_qMin(lower); + newRevoluteJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newRevoluteJoint->getGenCoord(0)->set_qMax(upper); + newRevoluteJoint->getGenCoord(0)->setConfigMax(upper); } } } @@ -703,14 +703,14 @@ dynamics::PrismaticJoint* SdfParser::readPrismaticJoint( if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newPrismaticJoint->getGenCoord(0)->set_qMin(lower); + newPrismaticJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newPrismaticJoint->getGenCoord(0)->set_qMax(upper); + newPrismaticJoint->getGenCoord(0)->setConfigMax(upper); } } } @@ -764,14 +764,14 @@ dynamics::ScrewJoint* SdfParser::readScrewJoint( if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newScrewJoint->getGenCoord(0)->set_qMin(lower); + newScrewJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newScrewJoint->getGenCoord(0)->set_qMax(upper); + newScrewJoint->getGenCoord(0)->setConfigMax(upper); } } } @@ -832,14 +832,14 @@ dynamics::UniversalJoint* SdfParser::readUniversalJoint( if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newUniversalJoint->getGenCoord(0)->set_qMin(lower); + newUniversalJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newUniversalJoint->getGenCoord(0)->set_qMax(upper); + newUniversalJoint->getGenCoord(0)->setConfigMax(upper); } } } @@ -883,14 +883,14 @@ dynamics::UniversalJoint* SdfParser::readUniversalJoint( if (hasElement(limitElement, "lower")) { double lower = getValueDouble(limitElement, "lower"); - newUniversalJoint->getGenCoord(0)->set_qMin(lower); + newUniversalJoint->getGenCoord(0)->setConfigMin(lower); } // upper if (hasElement(limitElement, "upper")) { double upper = getValueDouble(limitElement, "upper"); - newUniversalJoint->getGenCoord(1)->set_qMax(upper); + newUniversalJoint->getGenCoord(1)->setConfigMax(upper); } } } diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 445f33fe5bbbf..5088cce44c3d3 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -240,8 +240,8 @@ dynamics::Joint* DartLoader::createDartJoint(const urdf::Joint* _jt) switch(_jt->type) { case urdf::Joint::REVOLUTE: joint = new dynamics::RevoluteJoint(toEigen(_jt->axis)); - joint->getGenCoord(0)->set_qMin(_jt->limits->lower); - joint->getGenCoord(0)->set_qMax(_jt->limits->upper); + joint->getGenCoord(0)->setConfigMin(_jt->limits->lower); + joint->getGenCoord(0)->setConfigMax(_jt->limits->upper); joint->setDampingCoefficient(0, _jt->dynamics->damping); break; case urdf::Joint::CONTINUOUS: @@ -250,8 +250,8 @@ dynamics::Joint* DartLoader::createDartJoint(const urdf::Joint* _jt) break; case urdf::Joint::PRISMATIC: joint = new dynamics::PrismaticJoint(toEigen(_jt->axis)); - joint->getGenCoord(0)->set_qMin(_jt->limits->lower); - joint->getGenCoord(0)->set_qMax(_jt->limits->upper); + joint->getGenCoord(0)->setConfigMin(_jt->limits->lower); + joint->getGenCoord(0)->setConfigMax(_jt->limits->upper); joint->setDampingCoefficient(0, _jt->dynamics->damping); break; case urdf::Joint::FIXED: @@ -273,10 +273,10 @@ dynamics::Joint* DartLoader::createDartJoint(const urdf::Joint* _jt) joint->setTransformFromParentBodyNode(toEigen(_jt->parent_to_joint_origin_transform)); joint->setTransformFromChildBodyNode(Eigen::Isometry3d::Identity()); if(joint->getNumGenCoords() == 1 && _jt->limits) { - joint->getGenCoord(0)->set_dqMin(-_jt->limits->velocity); - joint->getGenCoord(0)->set_dqMax(_jt->limits->velocity); - joint->getGenCoord(0)->set_tauMin(-_jt->limits->effort); - joint->getGenCoord(0)->set_tauMax(_jt->limits->effort); + joint->getGenCoord(0)->setVelMin(-_jt->limits->velocity); + joint->getGenCoord(0)->setVelMax(_jt->limits->velocity); + joint->getGenCoord(0)->setForceMin(-_jt->limits->effort); + joint->getGenCoord(0)->setForceMax(_jt->limits->effort); } return joint; } diff --git a/unittests/TestHelpers.h b/unittests/TestHelpers.h index 40f11394dcd42..688d54556b866 100644 --- a/unittests/TestHelpers.h +++ b/unittests/TestHelpers.h @@ -104,9 +104,9 @@ Joint* create1DOFJoint(double val, double min, double max, int type) else if(type == DOF_ROLL) newJoint = new RevoluteJoint(Eigen::Vector3d(1.0, 0.0, 0.0)); // Add the transformation to the joint, set the min/max values and set it to the skeleton - newJoint->getGenCoord(0)->set_q(val); - newJoint->getGenCoord(0)->set_qMin(min); - newJoint->getGenCoord(0)->set_qMax(max); + newJoint->getGenCoord(0)->setConfig(val); + newJoint->getGenCoord(0)->setConfigMin(min); + newJoint->getGenCoord(0)->setConfigMax(max); return newJoint; } diff --git a/unittests/testInverseKinematics.cpp b/unittests/testInverseKinematics.cpp index 7f0ef512a5049..8d2b2f06a6cf4 100644 --- a/unittests/testInverseKinematics.cpp +++ b/unittests/testInverseKinematics.cpp @@ -168,8 +168,8 @@ TEST(InverseKinematics, FittingTransformation) for (size_t i = 0; i < numRandomTests; ++i) { // Set joint limit - joint2->getGenCoord(0)->set_qMin(DART_RADIAN * 0.0); - joint2->getGenCoord(0)->set_qMax(DART_RADIAN * 15.0); + joint2->getGenCoord(0)->setConfigMin(DART_RADIAN * 0.0); + joint2->getGenCoord(0)->setConfigMax(DART_RADIAN * 15.0); // Store the original transformation and joint angle Isometry3d oldT2 = body2->getWorldTransform(); @@ -177,7 +177,7 @@ TEST(InverseKinematics, FittingTransformation) // Get desiredT2 by rotating the revolute joint with random angle out of // the joint limit range - joint2->getGenCoord(0)->set_q(math::random(DART_RADIAN * 15.5, DART_PI)); + joint2->getGenCoord(0)->setConfig(math::random(DART_RADIAN * 15.5, DART_PI)); robot->setConfig(robot->getConfig()); Isometry3d desiredT2 = body2->getWorldTransform(); @@ -201,7 +201,7 @@ TEST(InverseKinematics, FittingTransformation) body2->fitWorldTransform(desiredT2, BodyNode::IKP_PARENT_JOINT, true); // Check if the optimal joint anlge is in the range - double newQ2 = joint2->getGenCoord(0)->get_q(); + double newQ2 = joint2->getGenCoord(0)->getConfig(); EXPECT_GE(newQ2, DART_RADIAN * 0.0); EXPECT_LE(newQ2, DART_RADIAN * 15.0); } diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 4a24e02a8d512..26881a31aa4ae 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -323,12 +323,12 @@ TEST_F(JOINTS, POSITION_LIMIT) double limit1 = DART_PI / 6.0; joint0->setPositionLimited(true); - joint0->getGenCoord(0)->set_qMin(-limit0); - joint0->getGenCoord(0)->set_qMax(limit0); + joint0->getGenCoord(0)->setConfigMin(-limit0); + joint0->getGenCoord(0)->setConfigMax(limit0); joint1->setPositionLimited(true); - joint1->getGenCoord(0)->set_qMin(-limit1); - joint1->getGenCoord(0)->set_qMax(limit1); + joint1->getGenCoord(0)->setConfigMin(-limit1); + joint1->getGenCoord(0)->setConfigMax(limit1); double simTime = 2.0; double timeStep = myWorld->getTimeStep(); @@ -339,11 +339,11 @@ TEST_F(JOINTS, POSITION_LIMIT) myWorld->step(); } - double jointPos0 = joint0->getGenCoord(0)->get_q(); - double jointPos1 = joint1->getGenCoord(0)->get_q(); + double jointPos0 = joint0->getGenCoord(0)->getConfig(); + double jointPos1 = joint1->getGenCoord(0)->getConfig(); - double jointVel0 = joint0->getGenCoord(0)->get_dq(); - double jointVel1 = joint1->getGenCoord(0)->get_dq(); + double jointVel0 = joint0->getGenCoord(0)->getVel(); + double jointVel1 = joint1->getGenCoord(0)->getVel(); // NOTE: The ideal result is that the joint position limit was obeyed with // zero tolerance. To do so, DART should correct the joint limit diff --git a/unittests/testSoftDynamics.cpp b/unittests/testSoftDynamics.cpp index f622c723a07b0..570b9f50572d7 100644 --- a/unittests/testSoftDynamics.cpp +++ b/unittests/testSoftDynamics.cpp @@ -346,8 +346,8 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) joint->setDampingCoefficient(l, random(lbD, ubD)); joint->setSpringStiffness (l, random(lbK, ubK)); - double lbRP = joint->getGenCoord(l)->get_qMin(); - double ubRP = joint->getGenCoord(l)->get_qMax(); + double lbRP = joint->getGenCoord(l)->getConfigMin(); + double ubRP = joint->getGenCoord(l)->getConfigMax(); joint->setRestPosition (l, random(lbRP, ubRP)); } } From f6b5a095a54f4531971917996aff1865c23d210f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 20 Mar 2014 09:33:06 -0400 Subject: [PATCH 04/11] Clean up GenCoordSystem --- apps/atlasRobot/Controller.cpp | 2 +- apps/atlasRobot/Main.cpp | 4 +- apps/atlasRobot/State.cpp | 4 +- apps/balance/Main.cpp | 2 +- apps/balance/MyWindow.cpp | 4 +- apps/ballJointConstraintTest/Main.cpp | 2 +- apps/ballJointConstraintTest/MyWindow.cpp | 2 +- apps/closedLoop/Main.cpp | 2 +- apps/closedLoop/MyWindow.cpp | 2 +- apps/forwardSim/Main.cpp | 2 +- apps/forwardSim/MyWindow.cpp | 2 +- apps/hardcodedDesign/MyWindow.cpp | 4 +- apps/harnessTest/Main.cpp | 2 +- apps/harnessTest/MyWindow.cpp | 2 +- apps/meshCollision/Main.cpp | 2 +- apps/softArticulatedBodiesTest/Main.cpp | 2 +- apps/softOpenChain/Main.cpp | 2 +- apps/vehicle/MyWindow.cpp | 6 +- dart/constraint/BallJointConstraint.cpp | 6 +- dart/constraint/ConstraintDynamics.cpp | 4 +- dart/constraint/RevoluteJointConstraint.cpp | 6 +- dart/dynamics/BallJoint.cpp | 4 +- dart/dynamics/BodyNode.cpp | 60 ++--- dart/dynamics/EulerJoint.cpp | 4 +- dart/dynamics/FreeJoint.cpp | 12 +- dart/dynamics/GenCoordSystem.cpp | 281 ++++++++++++-------- dart/dynamics/GenCoordSystem.h | 129 +++++---- dart/dynamics/Joint.cpp | 2 +- dart/dynamics/PointMass.cpp | 34 +-- dart/dynamics/Skeleton.cpp | 68 ++--- dart/dynamics/Skeleton.h | 21 +- dart/dynamics/SoftBodyNode.cpp | 10 +- dart/dynamics/SoftSkeleton.cpp | 10 +- dart/dynamics/TranslationalJoint.cpp | 2 +- dart/gui/SimWindow.cpp | 4 +- dart/planning/PathPlanner.h | 8 +- dart/planning/PathShortener.cpp | 6 +- dart/planning/RRT.cpp | 2 +- dart/simulation/World.cpp | 10 +- dart/utils/SkelParser.cpp | 32 +-- dart/utils/urdf/DartLoader.cpp | 2 +- unittests/testDynamics.cpp | 32 +-- unittests/testForwardKinematics.cpp | 4 +- unittests/testInverseKinematics.cpp | 26 +- unittests/testJoints.cpp | 18 +- unittests/testSoftDynamics.cpp | 14 +- 46 files changed, 462 insertions(+), 397 deletions(-) diff --git a/apps/atlasRobot/Controller.cpp b/apps/atlasRobot/Controller.cpp index 5e1ab8d8270ba..254db63cdfdc9 100644 --- a/apps/atlasRobot/Controller.cpp +++ b/apps/atlasRobot/Controller.cpp @@ -347,7 +347,7 @@ void Controller::unharnessRightFoot() void Controller::resetRobot() { int dof = mAtlasRobot->getNumGenCoords(); - mAtlasRobot->setConfig(mInitialState.head(dof)); // See #122 + mAtlasRobot->setConfigs(mInitialState.head(dof)); // See #122 mAtlasRobot->setState(mInitialState); dtmsg << "Robot is reset." << std::endl; diff --git a/apps/atlasRobot/Main.cpp b/apps/atlasRobot/Main.cpp index fe9fe01a320b7..0d34e7fd9c02b 100644 --- a/apps/atlasRobot/Main.cpp +++ b/apps/atlasRobot/Main.cpp @@ -74,9 +74,9 @@ int main(int argc, char* argv[]) myWorld->addSkeleton(ground); // Set initial configuration for Atlas robot - VectorXd q = atlas->getConfig(); + VectorXd q = atlas->getConfigs(); q[0] = -0.5 * DART_PI; - atlas->setConfig(q); + atlas->setConfigs(q); // Set gravity of the world myWorld->setGravity(Vector3d(0.0, -9.81, 0.0)); diff --git a/apps/atlasRobot/State.cpp b/apps/atlasRobot/State.cpp index 04935eb9153d6..9aa5018acd83b 100644 --- a/apps/atlasRobot/State.cpp +++ b/apps/atlasRobot/State.cpp @@ -155,8 +155,8 @@ void State::computeControlForce(double _timestep) assert(mNextState != NULL && "Next state should be set."); int dof = mSkeleton->getNumGenCoords(); - VectorXd q = mSkeleton->get_q(); - VectorXd dq = mSkeleton->get_dq(); + VectorXd q = mSkeleton->getConfigs(); + VectorXd dq = mSkeleton->getGenVels(); // Compute relative joint angles from desired global angles of the pelvis and // the swing leg diff --git a/apps/balance/Main.cpp b/apps/balance/Main.cpp index eb9aebf0d953b..4fee8fd4ef1d4 100644 --- a/apps/balance/Main.cpp +++ b/apps/balance/Main.cpp @@ -64,7 +64,7 @@ int main(int argc, char* argv[]) { genCoordIds.push_back(13); // lower back Eigen::VectorXd initConfig(8); initConfig << -0.1, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1; - myWorld->getSkeleton(1)->setConfig(genCoordIds, initConfig); + myWorld->getSkeleton(1)->setConfigs(genCoordIds, initConfig); // create controller Controller* myController = new Controller(myWorld->getSkeleton(1), diff --git a/apps/balance/MyWindow.cpp b/apps/balance/MyWindow.cpp index 893cb620f11c9..1ca768c798c7e 100644 --- a/apps/balance/MyWindow.cpp +++ b/apps/balance/MyWindow.cpp @@ -58,8 +58,8 @@ void MyWindow::timeStepping() { mController->setConstrForces( mWorld->getConstraintHandler()->getTotalConstraintForce(1)); - mController->computeTorques(mWorld->getSkeleton(1)->get_q(), - mWorld->getSkeleton(1)->get_dq()); + mController->computeTorques(mWorld->getSkeleton(1)->getConfigs(), + mWorld->getSkeleton(1)->getGenVels()); mWorld->getSkeleton(1)->setInternalForceVector(mController->getTorques()); mWorld->step(); diff --git a/apps/ballJointConstraintTest/Main.cpp b/apps/ballJointConstraintTest/Main.cpp index a9e6f0e7cd08a..71b163730bade 100644 --- a/apps/ballJointConstraintTest/Main.cpp +++ b/apps/ballJointConstraintTest/Main.cpp @@ -69,7 +69,7 @@ int main(int argc, char* argv[]) Eigen::VectorXd initPose(dof); for (int i = 0; i < dof; i++) initPose[i] = random(-0.5, 0.5); - myWorld->getSkeleton(0)->setConfig(initPose); + myWorld->getSkeleton(0)->setConfigs(initPose); // Add damping to every joint for (int i = 0; i < myWorld->getSkeleton(0)->getNumBodyNodes(); i++) { diff --git a/apps/ballJointConstraintTest/MyWindow.cpp b/apps/ballJointConstraintTest/MyWindow.cpp index c7a18d04156e7..1273aa3b66d93 100644 --- a/apps/ballJointConstraintTest/MyWindow.cpp +++ b/apps/ballJointConstraintTest/MyWindow.cpp @@ -57,7 +57,7 @@ Eigen::VectorXd MyWindow::computeDamping() int nDof = mWorld->getSkeleton(0)->getNumGenCoords(); Eigen::VectorXd damping = Eigen::VectorXd::Zero(nDof); // add damping to each joint; twist-dof has smaller damping - damping = -0.01 * mWorld->getSkeleton(0)->get_dq(); + damping = -0.01 * mWorld->getSkeleton(0)->getGenVels(); for (int i = 0; i < nDof; i++) if (i % 3 == 1) damping[i] *= 0.1; diff --git a/apps/closedLoop/Main.cpp b/apps/closedLoop/Main.cpp index 1c898692cad27..147314e99a790 100644 --- a/apps/closedLoop/Main.cpp +++ b/apps/closedLoop/Main.cpp @@ -73,7 +73,7 @@ int main(int argc, char* argv[]) initPose[23] = 3.14159 * 0.4; initPose[26] = 3.14159 * 0.4; initPose[29] = 3.14159 * 0.4; - myWorld->getSkeleton(0)->setConfig(initPose); + myWorld->getSkeleton(0)->setConfigs(initPose); // create a ball joint constraint BodyNode *bd1 = myWorld->getSkeleton(0)->getBodyNode("link 6"); diff --git a/apps/closedLoop/MyWindow.cpp b/apps/closedLoop/MyWindow.cpp index 29c4ca2904d2d..7b2ffef4dbf4c 100644 --- a/apps/closedLoop/MyWindow.cpp +++ b/apps/closedLoop/MyWindow.cpp @@ -51,7 +51,7 @@ Eigen::VectorXd MyWindow::computeDamping() int nDof = mWorld->getSkeleton(0)->getNumGenCoords(); Eigen::VectorXd damping = Eigen::VectorXd::Zero(nDof); // add damping to each joint; twist-dof has smaller damping - damping = -0.01 * mWorld->getSkeleton(0)->get_dq(); + damping = -0.01 * mWorld->getSkeleton(0)->getGenVels(); for (int i = 0; i < nDof; i++) if (i % 3 == 1) damping[i] *= 0.1; diff --git a/apps/forwardSim/Main.cpp b/apps/forwardSim/Main.cpp index 6891598963ae7..fddeb0cd15dba 100644 --- a/apps/forwardSim/Main.cpp +++ b/apps/forwardSim/Main.cpp @@ -57,7 +57,7 @@ int main(int argc, char* argv[]) { Eigen::VectorXd initPose(dof); for (int i = 0; i < dof; i++) initPose[i] = dart::math::random(-0.5, 0.5); - myWorld->getSkeleton(0)->setConfig(initPose); + myWorld->getSkeleton(0)->setConfigs(initPose); // create a window and link it to the world MyWindow window; diff --git a/apps/forwardSim/MyWindow.cpp b/apps/forwardSim/MyWindow.cpp index 62009b409edce..36ed3e8b2b04a 100644 --- a/apps/forwardSim/MyWindow.cpp +++ b/apps/forwardSim/MyWindow.cpp @@ -56,7 +56,7 @@ Eigen::VectorXd MyWindow::computeDamping() { int nDof = mWorld->getSkeleton(0)->getNumGenCoords(); Eigen::VectorXd damping = Eigen::VectorXd::Zero(nDof); // add damping to each joint; twist-dof has smaller damping - damping = -0.01 * mWorld->getSkeleton(0)->get_dq(); + damping = -0.01 * mWorld->getSkeleton(0)->getGenVels(); for (int i = 0; i < nDof; i++) if (i % 3 == 1) damping[i] *= 0.1; diff --git a/apps/hardcodedDesign/MyWindow.cpp b/apps/hardcodedDesign/MyWindow.cpp index f20f4f7dd3aa5..de71585d34bfe 100644 --- a/apps/hardcodedDesign/MyWindow.cpp +++ b/apps/hardcodedDesign/MyWindow.cpp @@ -61,9 +61,9 @@ void MyWindow::keyboard(unsigned char _key, int _x, int _y) { case '2': case '3': { size_t dofIdx = _key - 49; - Eigen::VectorXd pose = skel->get_q(); + Eigen::VectorXd pose = skel->getConfigs(); pose(dofIdx) = pose(dofIdx) + (inverse ? -dDOF : dDOF); - skel->setConfig(pose); + skel->setConfigs(pose); std::cout << "Updated pose DOF " << dofIdx << ": " << pose.transpose() << std::endl; glutPostRedisplay(); diff --git a/apps/harnessTest/Main.cpp b/apps/harnessTest/Main.cpp index 3377ccf9fe78d..5fe7e181a1f63 100644 --- a/apps/harnessTest/Main.cpp +++ b/apps/harnessTest/Main.cpp @@ -68,7 +68,7 @@ int main(int argc, char* argv[]) genCoordIds.push_back(13); // lower back Eigen::VectorXd initConfig(9); initConfig << -0.1, 0.2, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1; - myWorld->getSkeleton(1)->setConfig(genCoordIds, initConfig); + myWorld->getSkeleton(1)->setConfigs(genCoordIds, initConfig); // create controller Controller* myController = new Controller(myWorld->getSkeleton(1), diff --git a/apps/harnessTest/MyWindow.cpp b/apps/harnessTest/MyWindow.cpp index 8471a3c7eb909..1d4be4d6e7b20 100644 --- a/apps/harnessTest/MyWindow.cpp +++ b/apps/harnessTest/MyWindow.cpp @@ -58,7 +58,7 @@ void MyWindow::timeStepping() mWorld->getSkeleton(1)->getBodyNode("h_spine")->addExtForce(mForce); mController->setConstrForces(mWorld->getConstraintHandler()->getTotalConstraintForce(1)); - mController->computeTorques(mWorld->getSkeleton(1)->get_q(), mWorld->getSkeleton(1)->get_dq()); + mController->computeTorques(mWorld->getSkeleton(1)->getConfigs(), mWorld->getSkeleton(1)->getGenVels()); mWorld->getSkeleton(1)->setInternalForceVector(mController->getTorques()); mWorld->step(); diff --git a/apps/meshCollision/Main.cpp b/apps/meshCollision/Main.cpp index 0358b0c60f851..123f615f0a7b2 100644 --- a/apps/meshCollision/Main.cpp +++ b/apps/meshCollision/Main.cpp @@ -92,7 +92,7 @@ int main(int argc, char* argv[]) { // Verify that our skeleton has something inside :) std::printf("Our skeleton has %d nodes \n", MeshSkel->getNumBodyNodes()); // std::printf("Our skeleton has %d joints \n", MeshSkel->getNumJoints()); - std::printf("Our skeleton has %d DOFs \n", MeshSkel->getNumGenCoords()); + std::printf("Our skeleton has %zu DOFs \n", MeshSkel->getNumGenCoords()); MyWindow window; window.setWorld(myWorld); diff --git a/apps/softArticulatedBodiesTest/Main.cpp b/apps/softArticulatedBodiesTest/Main.cpp index ba3aeead2c9ae..bbbcf4e2e070a 100644 --- a/apps/softArticulatedBodiesTest/Main.cpp +++ b/apps/softArticulatedBodiesTest/Main.cpp @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) Eigen::VectorXd initPose = Eigen::VectorXd::Zero(dof); for (int i = 0; i < 3; i++) initPose[i] = dart::math::random(-0.5, 0.5); - myWorld->getSkeleton(1)->setConfig(initPose); + myWorld->getSkeleton(1)->setConfigs(initPose); // create a window and link it to the world MyWindow window; diff --git a/apps/softOpenChain/Main.cpp b/apps/softOpenChain/Main.cpp index be7b32fedd3cb..fc70c4d346eb3 100644 --- a/apps/softOpenChain/Main.cpp +++ b/apps/softOpenChain/Main.cpp @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) Eigen::VectorXd initPose = Eigen::VectorXd::Zero(dof); for (int i = 0; i < 3; i++) initPose[i] = dart::math::random(-0.5, 0.5); - myWorld->getSkeleton("skeleton 1")->setConfig(initPose); + myWorld->getSkeleton("skeleton 1")->setConfigs(initPose); // create a window and link it to the world MyWindow window; diff --git a/apps/vehicle/MyWindow.cpp b/apps/vehicle/MyWindow.cpp index 71900beead23d..d3404f0436a56 100644 --- a/apps/vehicle/MyWindow.cpp +++ b/apps/vehicle/MyWindow.cpp @@ -58,9 +58,9 @@ void MyWindow::timeStepping() { dart::dynamics::Skeleton* vehicle = mWorld->getSkeleton("car_skeleton"); assert(vehicle != 0); - Eigen::VectorXd q = vehicle->get_q(); - Eigen::VectorXd dq = vehicle->get_dq(); - Eigen::VectorXd tau = vehicle->get_tau(); + Eigen::VectorXd q = vehicle->getConfigs(); + Eigen::VectorXd dq = vehicle->getGenVels(); + Eigen::VectorXd tau = vehicle->getGenForces(); tau.setZero(); if (true) diff --git a/dart/constraint/BallJointConstraint.cpp b/dart/constraint/BallJointConstraint.cpp index 23699e5593307..944da7abcb19f 100644 --- a/dart/constraint/BallJointConstraint.cpp +++ b/dart/constraint/BallJointConstraint.cpp @@ -46,7 +46,7 @@ void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::VectorXd getJacobian(); _J1.block(_rowIndex, 0, 3, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1; Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1; - Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->get_dq(); + Eigen::VectorXd qDot1 = mBodyNode1->getSkeleton()->getGenVels(); _C.segment(_rowIndex, 3) = worldP1 - mOffset2; _CDot.segment(_rowIndex, 3) = mJ1 * qDot1; } @@ -58,9 +58,9 @@ void BallJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::MatrixXd _J2.block(_rowIndex, 0, 3, mBodyNode2->getSkeleton()->getNumGenCoords()) += mJ2; Eigen::Vector3d worldP1 = mBodyNode1->getWorldTransform() * mOffset1; - Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->get_dq(); + Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels(); Eigen::VectorXd worldP2 = mBodyNode2->getWorldTransform() * mOffset2; - Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->get_dq(); + Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->getGenVels(); _C.segment(_rowIndex, 3) = worldP1 - worldP2; _CDot.segment(_rowIndex, 3) = mJ1 * qDot1 + mJ2 * qDot2; diff --git a/dart/constraint/ConstraintDynamics.cpp b/dart/constraint/ConstraintDynamics.cpp index 5cdbf18e1e227..81ed8c72845c3 100644 --- a/dart/constraint/ConstraintDynamics.cpp +++ b/dart/constraint/ConstraintDynamics.cpp @@ -1057,7 +1057,7 @@ void ConstraintDynamics::updateTauStar() + mSkeletons[i]->getInternalForceVector() + mSkeletons[i]->getDampingForceVector(); Eigen::VectorXd tauStar = - (mSkeletons[i]->getAugMassMatrix() * mSkeletons[i]->get_dq()) + (mSkeletons[i]->getAugMassMatrix() * mSkeletons[i]->getGenVels()) - (mDt * (mSkeletons[i]->getCombinedVector() - tau)); mTauStar.block(startRow, 0, tauStar.rows(), 1) = tauStar; startRow += tauStar.rows(); @@ -1324,7 +1324,7 @@ void ConstraintDynamics::updateConstraintTerms() { if (!mSkeletons[i]->isMobile() || mSkeletons[i]->getNumGenCoords() == 0) continue; - Eigen::VectorXd qDot = mSkeletons[i]->get_dq(); + Eigen::VectorXd qDot = mSkeletons[i]->getGenVels(); mTauHat.noalias() += -(mJ[i] - mPreJ[i]) / mDt * qDot; mTauHat.noalias() -= mJMInv[i] * (mSkeletons[i]->getInternalForceVector() + mSkeletons[i]->getExternalForceVector() diff --git a/dart/constraint/RevoluteJointConstraint.cpp b/dart/constraint/RevoluteJointConstraint.cpp index a8c2f254e2ac1..7d0b3210fcec3 100644 --- a/dart/constraint/RevoluteJointConstraint.cpp +++ b/dart/constraint/RevoluteJointConstraint.cpp @@ -37,7 +37,7 @@ void RevoluteJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::Vecto getJacobian(); _J1.block(_rowIndex, 0, mNumRows, mBodyNode1->getSkeleton()->getNumGenCoords()) = mJ1; Eigen::Vector3d v1 = mBodyNode1->getWorldTransform().rotation() * mAxis1; - Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->get_dq(); + Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels(); _C.segment(_rowIndex, mNumRows) = v1 - mAxis2; _CDot.segment(_rowIndex, mNumRows) = mJ1 * qDot1; } @@ -49,9 +49,9 @@ void RevoluteJointConstraint::updateDynamics(Eigen::MatrixXd & _J1, Eigen::Matri _J2.block(_rowIndex, 0, mNumRows, mBodyNode2->getSkeleton()->getNumGenCoords()) += mJ2; Eigen::Vector3d v1 = mBodyNode1->getWorldTransform().rotation() * mAxis1; - Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->get_dq(); + Eigen::VectorXd qDot1 = ((dynamics::Skeleton*)mBodyNode1->getSkeleton())->getGenVels(); Eigen::VectorXd v2 = mBodyNode2->getWorldTransform().rotation() * mAxis2; - Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->get_dq(); + Eigen::VectorXd qDot2 = ((dynamics::Skeleton*)mBodyNode2->getSkeleton())->getGenVels(); _C.segment(_rowIndex, mNumRows) = v1 - v2; _CDot.segment(_rowIndex, mNumRows) = mJ1 * qDot1 + mJ2 * qDot2; diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 33f0583de7e71..0bbb4389880ff 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -77,9 +77,9 @@ inline void BallJoint::updateTransform() { } void BallJoint::updateTransform_Issue122(double _timeStep) { - mT_Joint = mT_Joint * math::expAngular(_timeStep * get_dq()); + mT_Joint = mT_Joint * math::expAngular(_timeStep * getGenVels()); - set_q(math::logMap(mT_Joint).head<3>()); + setConfigs(math::logMap(mT_Joint).head<3>()); mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse(); diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index f1902835acec3..023537575ad39 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -216,7 +216,7 @@ void BodyNode::fitWorldLinearVel(const Eigen::Vector3d& _targetLinVel, optimizer::Problem prob(dof); // Use the current joint configuration as initial guess - prob.setInitialGuess(parentJoint->get_dq()); + prob.setInitialGuess(parentJoint->getGenVels()); // Objective function VelocityObjFunc obj(this, _targetLinVel, VelocityObjFunc::VT_LINEAR, mSkeleton); @@ -225,8 +225,8 @@ void BodyNode::fitWorldLinearVel(const Eigen::Vector3d& _targetLinVel, // Joint limit if (_jointVelLimit) { - prob.setLowerBounds(parentJoint->get_dqMin()); - prob.setUpperBounds(parentJoint->get_dqMax()); + prob.setLowerBounds(parentJoint->getGenVelsMin()); + prob.setUpperBounds(parentJoint->getGenVelsMax()); } // Solve with gradient-free local minima algorithm @@ -235,7 +235,7 @@ void BodyNode::fitWorldLinearVel(const Eigen::Vector3d& _targetLinVel, // Set optimal configuration of the parent joint Eigen::VectorXd jointDQ = prob.getOptimalSolution(); - parentJoint->set_dq(jointDQ); + parentJoint->setGenVels(jointDQ); // Update forward kinematics information // TODO(JS): Need more efficient api for this @@ -257,7 +257,7 @@ void BodyNode::fitWorldAngularVel(const Eigen::Vector3d& _targetAngVel, optimizer::Problem prob(dof); // Use the current joint configuration as initial guess - prob.setInitialGuess(parentJoint->get_dq()); + prob.setInitialGuess(parentJoint->getGenVels()); // Objective function VelocityObjFunc obj(this, _targetAngVel, VelocityObjFunc::VT_ANGULAR, mSkeleton); @@ -266,8 +266,8 @@ void BodyNode::fitWorldAngularVel(const Eigen::Vector3d& _targetAngVel, // Joint limit if (_jointVelLimit) { - prob.setLowerBounds(parentJoint->get_dqMin()); - prob.setUpperBounds(parentJoint->get_dqMax()); + prob.setLowerBounds(parentJoint->getGenVelsMin()); + prob.setUpperBounds(parentJoint->getGenVelsMax()); } // Solve with gradient-free local minima algorithm @@ -276,7 +276,7 @@ void BodyNode::fitWorldAngularVel(const Eigen::Vector3d& _targetAngVel, // Set optimal configuration of the parent joint Eigen::VectorXd jointDQ = prob.getOptimalSolution(); - parentJoint->set_dq(jointDQ); + parentJoint->setGenVels(jointDQ); // Update forward kinematics information // TODO(JS): Need more efficient api for this @@ -510,7 +510,7 @@ void BodyNode::updateVelocity() { //-------------------------------------------------------------------------- if (mParentJoint->getNumGenCoords() > 0) { - mV.noalias() = mParentJoint->getLocalJacobian() * mParentJoint->get_dq(); + mV.noalias() = mParentJoint->getLocalJacobian() * mParentJoint->getGenVels(); if (mParentBodyNode) { mV += math::AdInvT(mParentJoint->getLocalTransform(), mParentBodyNode->getBodyVelocity()); @@ -525,9 +525,9 @@ void BodyNode::updateEta() { if (mParentJoint->getNumGenCoords() > 0) { mEta = math::ad(mV, mParentJoint->getLocalJacobian() * - mParentJoint->get_dq()); + mParentJoint->getGenVels()); mEta.noalias() += mParentJoint->getLocalJacobianTimeDeriv() * - mParentJoint->get_dq(); + mParentJoint->getGenVels(); assert(!math::isNan(mEta)); } } @@ -537,9 +537,9 @@ void BodyNode::updateEta_Issue122() { if (mParentJoint->getNumGenCoords() > 0) { mEta = math::ad(mV, mParentJoint->getLocalJacobian() * - mParentJoint->get_dq()); + mParentJoint->getGenVels()); mEta.noalias() += mParentJoint->getLocalJacobianTimeDeriv() * - mParentJoint->get_dq(); + mParentJoint->getGenVels(); assert(!math::isNan(mEta)); } } @@ -554,7 +554,7 @@ void BodyNode::updateAcceleration() { if (mParentJoint->getNumGenCoords() > 0) { mdV = mEta; - mdV.noalias() += mParentJoint->getLocalJacobian() * mParentJoint->get_ddq(); + mdV.noalias() += mParentJoint->getLocalJacobian() * mParentJoint->getGenAccs(); if (mParentBodyNode) { mdV += math::AdInvT(mParentJoint->getLocalTransform(), mParentBodyNode->getBodyAcceleration()); @@ -799,7 +799,7 @@ void BodyNode::updateGeneralizedForce(bool _withDampingForces) { assert(!math::isNan(J.transpose()*mF)); - mParentJoint->set_tau(J.transpose()*mF); + mParentJoint->setGenForces(J.transpose()*mF); } void BodyNode::updateArticulatedInertia(double _timeStep) { @@ -893,7 +893,7 @@ void BodyNode::updateBiasForce(double _timeStep, // Cache data: alpha int dof = mParentJoint->getNumGenCoords(); if (dof > 0) { - mAlpha = mParentJoint->get_tau() + mAlpha = mParentJoint->getGenForces() + mParentJoint->getSpringForces(_timeStep) + mParentJoint->getDampingForces(); for (int i = 0; i < dof; i++) { @@ -928,7 +928,7 @@ void BodyNode::update_ddq() { ddq.noalias() = mImplicitPsi * mAlpha; } - mParentJoint->set_ddq(ddq); + mParentJoint->setGenAccs(ddq); assert(!math::isNan(ddq)); updateAcceleration(); @@ -1026,7 +1026,7 @@ void BodyNode::updateMassMatrix() { int dof = mParentJoint->getNumGenCoords(); if (dof > 0) { mM_dV.noalias() += mParentJoint->getLocalJacobian() * - mParentJoint->get_ddq(); + mParentJoint->getGenAccs(); assert(!math::isNan(mM_dV)); } if (mParentBodyNode) @@ -1075,8 +1075,8 @@ void BodyNode::aggregateAugMassMatrix(Eigen::MatrixXd* _MCol, int _col, int iStart = mParentJoint->getGenCoord(0)->getSkeletonIndex(); _MCol->block(iStart, _col, dof, 1).noalias() = mParentJoint->getLocalJacobian().transpose() * mM_F - + D * (_timeStep * mParentJoint->get_ddq()) - + K * (_timeStep * _timeStep * mParentJoint->get_ddq()); + + D * (_timeStep * mParentJoint->getGenAccs()) + + K * (_timeStep * _timeStep * mParentJoint->getGenAccs()); } } @@ -1092,7 +1092,7 @@ void BodyNode::updateInvMassMatrix() { // Cache data: mInvM2_a int dof = mParentJoint->getNumGenCoords(); if (dof > 0) { - mInvM_a = mParentJoint->get_tau(); + mInvM_a = mParentJoint->getGenForces(); mInvM_a.noalias() -= mParentJoint->getLocalJacobian().transpose() * mInvM_c; assert(!math::isNan(mInvM_a)); } @@ -1118,7 +1118,7 @@ void BodyNode::updateInvAugMassMatrix() { // Cache data: mInvM2_a int dof = mParentJoint->getNumGenCoords(); if (dof > 0) { - mInvM_a = mParentJoint->get_tau(); + mInvM_a = mParentJoint->getGenForces(); mInvM_a.noalias() -= mParentJoint->getLocalJacobian().transpose() * mInvM_c; assert(!math::isNan(mInvM_a)); } @@ -1333,7 +1333,7 @@ void BodyNode::fitWorldTransformParentJointImpl( optimizer::Problem prob(dof); // Use the current joint configuration as initial guess - prob.setInitialGuess(parentJoint->get_q()); + prob.setInitialGuess(parentJoint->getConfigs()); // Objective function TransformObjFunc obj(this, _target, mSkeleton); @@ -1342,8 +1342,8 @@ void BodyNode::fitWorldTransformParentJointImpl( // Joint limit if (_jointLimit) { - prob.setLowerBounds(parentJoint->get_qMin()); - prob.setUpperBounds(parentJoint->get_qMax()); + prob.setLowerBounds(parentJoint->getConfigsMin()); + prob.setUpperBounds(parentJoint->getConfigsMax()); } // Solve with gradient-free local minima algorithm @@ -1352,11 +1352,11 @@ void BodyNode::fitWorldTransformParentJointImpl( // Set optimal configuration of the parent joint Eigen::VectorXd jointQ = prob.getOptimalSolution(); - parentJoint->set_q(jointQ); + parentJoint->setConfigs(jointQ); // Update forward kinematics information // TODO(JS): Need more efficient api for this - mSkeleton->setConfig(mSkeleton->getConfig()); + mSkeleton->setConfigs(mSkeleton->getConfigs()); } void BodyNode::fitWorldTransformAncestorJointsImpl( @@ -1387,8 +1387,8 @@ double BodyNode::TransformObjFunc::eval(Eigen::Map& _x) // Update forward kinematics information with _x // TODO(JS): Need more efficient api for this - mBodyNode->getParentJoint()->set_q(_x); - mSkeleton->setConfig(mSkeleton->getConfig()); + mBodyNode->getParentJoint()->setConfigs(_x); + mSkeleton->setConfigs(mSkeleton->getConfigs()); // Compute and return the geometric distance between body node transformation // and target transformation @@ -1428,7 +1428,7 @@ double BodyNode::VelocityObjFunc::eval(Eigen::Map& _x) // Update forward kinematics information with _x // TODO(JS): Need more efficient api for this - mBodyNode->getParentJoint()->set_dq(_x); + mBodyNode->getParentJoint()->setGenVels(_x); mSkeleton->setState(mSkeleton->getState()); // Compute and return the geometric distance between body node transformation diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index a499a619d80d6..a5b59674c7f79 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -77,14 +77,14 @@ void EulerJoint::updateTransform() { case AO_XYZ: { mT = mT_ParentBodyToJoint * - Eigen::Isometry3d(math::eulerXYZToMatrix(get_q())) * + Eigen::Isometry3d(math::eulerXYZToMatrix(getConfigs())) * mT_ChildBodyToJoint.inverse(); break; } case AO_ZYX: { mT = mT_ParentBodyToJoint * - Eigen::Isometry3d(math::eulerZYXToMatrix(get_q())) * + Eigen::Isometry3d(math::eulerZYXToMatrix(getConfigs())) * mT_ChildBodyToJoint.inverse(); break; } diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 79b5019f604f5..33f966fb0341b 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -68,7 +68,7 @@ FreeJoint::~FreeJoint() { void FreeJoint::updateTransform() { // TODO(JS): This is workaround for Issue #122. - mT_Joint = math::expMap(get_q()); + mT_Joint = math::expMap(getConfigs()); mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse(); @@ -76,9 +76,9 @@ void FreeJoint::updateTransform() { } void FreeJoint::updateTransform_Issue122(double _timeStep) { - mT_Joint = mT_Joint * math::expMap(_timeStep * get_dq()); + mT_Joint = mT_Joint * math::expMap(_timeStep * getGenVels()); - set_q(math::logMap(mT_Joint)); + setConfigs(math::logMap(mT_Joint)); mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse(); @@ -169,13 +169,13 @@ void FreeJoint::updateJacobianTimeDeriv() { mdS.col(1) = math::AdT(mT_ChildBodyToJoint, dJ1); mdS.col(2) = math::AdT(mT_ChildBodyToJoint, dJ2); mdS.col(3) = - -math::ad(mS.leftCols<3>() * get_dq().head<3>(), + -math::ad(mS.leftCols<3>() * getGenVels().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J3)); mdS.col(4) = - -math::ad(mS.leftCols<3>() * get_dq().head<3>(), + -math::ad(mS.leftCols<3>() * getGenVels().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J4)); mdS.col(5) = - -math::ad(mS.leftCols<3>() * get_dq().head<3>(), + -math::ad(mS.leftCols<3>() * getGenVels().head<3>(), math::AdT(mT_ChildBodyToJoint * math::expAngular(-q), J5)); assert(!math::isNan(mdS)); diff --git a/dart/dynamics/GenCoordSystem.cpp b/dart/dynamics/GenCoordSystem.cpp index e2c0ca4742e89..a56c644a22acd 100644 --- a/dart/dynamics/GenCoordSystem.cpp +++ b/dart/dynamics/GenCoordSystem.cpp @@ -1,5 +1,5 @@ /* - * Copyright (c) 2013, Georgia Tech Research Corporation + * Copyright (c) 2013-2014, Georgia Tech Research Corporation * All rights reserved. * * Author(s): Jeongseok Lee @@ -41,270 +41,317 @@ namespace dart { namespace dynamics { -GenCoordSystem::GenCoordSystem() { +//============================================================================== +GenCoordSystem::GenCoordSystem() +{ } -GenCoordSystem::~GenCoordSystem() { +//============================================================================== +GenCoordSystem::~GenCoordSystem() +{ } -GenCoord* GenCoordSystem::getGenCoord(int _idx) const { +//============================================================================== +GenCoord* GenCoordSystem::getGenCoord(size_t _idx) const +{ assert(0 <= _idx && _idx < getNumGenCoords()); return mGenCoords[_idx]; } -GenCoord* GenCoordSystem::getGenCoord(const std::string& _name) const { - int size = getNumGenCoords(); +//============================================================================== +GenCoord* GenCoordSystem::getGenCoord(const std::string& _name) const +{ + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) + for (size_t i = 0; i < size; ++i) + { if (mGenCoords[i]->getName() == _name) return mGenCoords[i]; + } return NULL; } -void GenCoordSystem::set_q(const Eigen::VectorXd& _q) +//============================================================================== +void GenCoordSystem::setConfigs(const Eigen::VectorXd& _configs, + bool _updateCartesian) { - assert(_q.size() == getNumGenCoords()); + assert(_configs.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setConfig(_q[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setConfig(_configs[i]); } -void GenCoordSystem::set_dq(const Eigen::VectorXd& _dq) { - assert(_dq.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenVels(const Eigen::VectorXd& _vels) +{ + assert(_vels.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setVel(_dq[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setVel(_vels[i]); } -void GenCoordSystem::set_ddq(const Eigen::VectorXd& _ddq) { - assert(_ddq.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenAccs(const Eigen::VectorXd& _accs) +{ + assert(_accs.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setAcc(_ddq[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setAcc(_accs[i]); } -void GenCoordSystem::set_tau(const Eigen::VectorXd& _tau) { - assert(_tau.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenForces(const Eigen::VectorXd& _forces) +{ + assert(_forces.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setForce(_tau[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setForce(_forces[i]); } -void GenCoordSystem::setGenPosMin(const Eigen::VectorXd& _qMin) { - assert(_qMin.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setConfigsMin(const Eigen::VectorXd& _configsMin, bool _updateCartesian) +{ + assert(_configsMin.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setConfigMin(_qMin[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setConfigMin(_configsMin[i]); } -void GenCoordSystem::set_dqMin(const Eigen::VectorXd& _dqMin) { - assert(_dqMin.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenVelsMin(const Eigen::VectorXd& _velsMin) +{ + assert(_velsMin.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setVelMin(_dqMin[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setVelMin(_velsMin[i]); } -void GenCoordSystem::set_ddqMin(const Eigen::VectorXd& _ddqMin) { - assert(_ddqMin.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenAccsMin(const Eigen::VectorXd& _accsMin) +{ + assert(_accsMin.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setAccMin(_ddqMin[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setAccMin(_accsMin[i]); } -void GenCoordSystem::set_tauMin(const Eigen::VectorXd& _tauMin) { - assert(_tauMin.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenForcesMin(const Eigen::VectorXd& _forcesMin) +{ + assert(_forcesMin.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setForceMin(_tauMin[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setForceMin(_forcesMin[i]); } +//============================================================================== +void GenCoordSystem::setConfigsMax(const Eigen::VectorXd& _configsMax, bool _updateCartesian) +{ + assert(_configsMax.size() == getNumGenCoords()); -void GenCoordSystem::setGenPosMax(const Eigen::VectorXd& _qMax) { - assert(_qMax.size() == getNumGenCoords()); - - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setConfigMax(_qMax[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setConfigMax(_configsMax[i]); } -void GenCoordSystem::set_dqMax(const Eigen::VectorXd& _dqMax) { - assert(_dqMax.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenVelsMax(const Eigen::VectorXd& _velsMax) +{ + assert(_velsMax.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setVelMax(_dqMax[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setVelMax(_velsMax[i]); } -void GenCoordSystem::set_ddqMax(const Eigen::VectorXd& _ddqMax) { - assert(_ddqMax.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenAccsMax(const Eigen::VectorXd& _accsMax) +{ + assert(_accsMax.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setAccMax(_ddqMax[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setAccMax(_accsMax[i]); } -void GenCoordSystem::set_tauMax(const Eigen::VectorXd& _tauMax) { - assert(_tauMax.size() == getNumGenCoords()); +//============================================================================== +void GenCoordSystem::setGenForcesMax(const Eigen::VectorXd& _forcesMax) +{ + assert(_forcesMax.size() == getNumGenCoords()); - int size = getNumGenCoords(); + size_t size = getNumGenCoords(); - for (int i = 0; i < size; ++i) - mGenCoords[i]->setForceMax(_tauMax[i]); + for (size_t i = 0; i < size; ++i) + mGenCoords[i]->setForceMax(_forcesMax[i]); } -Eigen::VectorXd GenCoordSystem::get_q() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getConfigs() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd q = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) q(i) = mGenCoords[i]->getConfig(); - } return q; } -Eigen::VectorXd GenCoordSystem::get_dq() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenVels() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd dq = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) dq(i) = mGenCoords[i]->getVel(); - } return dq; } -Eigen::VectorXd GenCoordSystem::get_ddq() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenAccs() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd ddq = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) ddq(i) = mGenCoords[i]->getAcc(); - } return ddq; } -Eigen::VectorXd GenCoordSystem::get_tau() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenForces() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd tau = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) tau(i) = mGenCoords[i]->getForce(); - } return tau; } -Eigen::VectorXd GenCoordSystem::get_qMax() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getConfigsMax() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd q = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) q(i) = mGenCoords[i]->getConfigMax(); - } return q; } -Eigen::VectorXd GenCoordSystem::get_dqMax() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenVelsMax() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd dq = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) dq(i) = mGenCoords[i]->getVelMax(); - } return dq; } -Eigen::VectorXd GenCoordSystem::get_ddqMax() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenAccsMax() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd ddq = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) ddq(i) = mGenCoords[i]->getAccMax(); - } return ddq; } -Eigen::VectorXd GenCoordSystem::get_tauMax() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenForcesMax() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd tau = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) tau(i) = mGenCoords[i]->getForceMax(); - } return tau; } -Eigen::VectorXd GenCoordSystem::get_qMin() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getConfigsMin() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd q = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) q(i) = mGenCoords[i]->getConfigMin(); - } return q; } -Eigen::VectorXd GenCoordSystem::get_dqMin() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenVelsMin() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd dq = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) dq(i) = mGenCoords[i]->getVelMin(); - } return dq; } -Eigen::VectorXd GenCoordSystem::get_ddqMin() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenAccsMin() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd ddq = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) ddq(i) = mGenCoords[i]->getAccMin(); - } return ddq; } -Eigen::VectorXd GenCoordSystem::get_tauMin() const { - int size = getNumGenCoords(); +//============================================================================== +Eigen::VectorXd GenCoordSystem::getGenForcesMin() const +{ + size_t size = getNumGenCoords(); Eigen::VectorXd tau = Eigen::VectorXd::Zero(size); - for (int i = 0; i < size; ++i) { + for (size_t i = 0; i < size; ++i) tau(i) = mGenCoords[i]->getForceMin(); - } return tau; } -int GenCoordSystem::getNumGenCoords() const { +//============================================================================== +size_t GenCoordSystem::getNumGenCoords() const +{ return mGenCoords.size(); } diff --git a/dart/dynamics/GenCoordSystem.h b/dart/dynamics/GenCoordSystem.h index 4fa72b92c3280..36d76f9d0c50b 100644 --- a/dart/dynamics/GenCoordSystem.h +++ b/dart/dynamics/GenCoordSystem.h @@ -37,6 +37,7 @@ #ifndef DART_DYNAMICS_GENCOORDSYSTEM_H_ #define DART_DYNAMICS_GENCOORDSYSTEM_H_ +#include #include #include @@ -47,100 +48,110 @@ namespace dart { namespace dynamics { -/// \brief System is a base class for every classes that has Dofs. +/// \brief Base class for generalized coordinate systems +/// +/// class GenCoordSystem { public: - /// \brief Constructor - GenCoordSystem(); - - /// \brief Destructor - virtual ~GenCoordSystem(); - - /// \brief Get number of generalized coordinates in this system - int getNumGenCoords() const; + /// \brief Get number of generalized coordinates + size_t getNumGenCoords() const; /// \brief Get generalized coordinate whose index is _idx - GenCoord* getGenCoord(int _idx) const; + GenCoord* getGenCoord(size_t _idx) const; /// \brief Get gneralized coordinate whose name is _name GenCoord* getGenCoord(const std::string& _name) const; - /// \brief Set generalized coordinate vector - void set_q(const Eigen::VectorXd& _q); + //--------------------------- Configurations --------------------------------- + /// \brief Set configurations defined in terms of generalized coordinates + virtual void setConfigs(const Eigen::VectorXd& _configs, + bool _updateCartesian = true); - /// \brief Set generalized velocity vector - void set_dq(const Eigen::VectorXd& _dq); + /// \brief Get configurations defined in terms of generalized coordinates + virtual Eigen::VectorXd getConfigs() const; - /// \brief Set generalized acceleration vector - void set_ddq(const Eigen::VectorXd& _ddq); + /// \brief Set lower bounds for configurations + virtual void setConfigsMin(const Eigen::VectorXd& _configsMin, + bool _updateCartesian = true); - /// \brief Set generalized force vector (internal forces) - void set_tau(const Eigen::VectorXd& _tau); + /// \brief Get lower bounds for configurations + virtual Eigen::VectorXd getConfigsMin() const; - /// \brief - void setGenPosMin(const Eigen::VectorXd& _qMin); + /// \brief Set upper bounds for configurations + virtual void setConfigsMax(const Eigen::VectorXd& _configsMax, + bool _updateCartesian = true); - /// \brief - void set_dqMin(const Eigen::VectorXd& _dqMin); + /// \brief Get uppoer bounds for configurations + virtual Eigen::VectorXd getConfigsMax() const; - /// \brief - void set_ddqMin(const Eigen::VectorXd& _ddqMin); + //----------------------------- Velocities ----------------------------------- + /// \brief Set generalized velocities + virtual void setGenVels(const Eigen::VectorXd& _vels); - /// \brief - void set_tauMin(const Eigen::VectorXd& _tauMin); + /// \brief Get generalized velocities + virtual Eigen::VectorXd getGenVels() const; - /// \brief - void setGenPosMax(const Eigen::VectorXd& _qMax); + /// \brief Set lower bounds for generalized velocities + virtual void setGenVelsMin(const Eigen::VectorXd& _velsMin); - /// \brief - void set_dqMax(const Eigen::VectorXd& _dqMax); + /// \brief Get lower bounds for generalized velocities + virtual Eigen::VectorXd getGenVelsMin() const; - /// \brief - void set_ddqMax(const Eigen::VectorXd& _ddqMax); + /// \brief Set upper bounds for generalized velocities + virtual void setGenVelsMax(const Eigen::VectorXd& _velsMax); - /// \brief - void set_tauMax(const Eigen::VectorXd& _tauMax); + /// \brief Get uppoer bounds for generalized velocities + virtual Eigen::VectorXd getGenVelsMax() const; - /// \brief - Eigen::VectorXd get_q() const; + //---------------------------- Accelerations --------------------------------- + /// \brief Set generalized accelerations + virtual void setGenAccs(const Eigen::VectorXd& _accs); - /// \brief - Eigen::VectorXd get_dq() const; + /// \brief Get generalized accelerations + virtual Eigen::VectorXd getGenAccs() const; - /// \brief - Eigen::VectorXd get_ddq() const; + /// \brief Set lower bounds for generalized accelerations + virtual void setGenAccsMin(const Eigen::VectorXd& _accsMin); - /// \brief - Eigen::VectorXd get_tau() const; + /// \brief Get lower bounds for generalized accelerations + virtual Eigen::VectorXd getGenAccsMin() const; - /// \brief - Eigen::VectorXd get_qMin() const; + /// \brief Set upper bounds for generalized accelerations + virtual void setGenAccsMax(const Eigen::VectorXd& _accsMax); - /// \brief - Eigen::VectorXd get_dqMin() const; + /// \brief Get uppoer bounds for generalized accelerations + virtual Eigen::VectorXd getGenAccsMax() const; - /// \brief - Eigen::VectorXd get_ddqMin() const; + //------------------------------- Forces ------------------------------------- + /// \brief Set generalized forces + virtual void setGenForces(const Eigen::VectorXd& _forces); - /// \brief - Eigen::VectorXd get_tauMin() const; + /// \brief Get generalized forces + virtual Eigen::VectorXd getGenForces() const; - /// \brief - Eigen::VectorXd get_qMax() const; + /// \brief Set lower bounds for generalized forces + virtual void setGenForcesMin(const Eigen::VectorXd& _forcesMin); - /// \brief - Eigen::VectorXd get_dqMax() const; + /// \brief Get lower bounds for generalized forces + virtual Eigen::VectorXd getGenForcesMin() const; - /// \brief - Eigen::VectorXd get_ddqMax() const; + /// \brief Set upper bounds for generalized forces + virtual void setGenForcesMax(const Eigen::VectorXd& _forcesMax); - /// \brief - Eigen::VectorXd get_tauMax() const; + /// \brief Get uppoer bounds for generalized forces + virtual Eigen::VectorXd getGenForcesMax() const; protected: - /// \brief Pointers to Dofs. + /// \brief Array of pointers to generalized coordinates std::vector mGenCoords; + +protected: + /// \brief Constructor + GenCoordSystem(); + + /// \brief Destructor + virtual ~GenCoordSystem(); }; } // namespace dynamics diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 69021cd3fc806..fd5e80743a07e 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -206,7 +206,7 @@ double Joint::getPotentialEnergy() const { int dof = getNumGenCoords(); // Spring energy - Eigen::VectorXd q = get_q(); + Eigen::VectorXd q = getConfigs(); assert(q.size() == dof); for (int i = 0; i < dof; ++i) { PE += 0.5 * mSpringStiffness[i] diff --git a/dart/dynamics/PointMass.cpp b/dart/dynamics/PointMass.cpp index c355e1e5898ca..8d20de13a20bc 100644 --- a/dart/dynamics/PointMass.cpp +++ b/dart/dynamics/PointMass.cpp @@ -271,7 +271,7 @@ void PointMass::init() void PointMass::updateTransform() { // Local transpose - mX = get_q() + mX0; + mX = getConfigs() + mX0; assert(!math::isNan(mX)); // World transpose @@ -285,14 +285,14 @@ void PointMass::updateVelocity() // v = w(parent) x mX + v(parent) + dq mV = mParentSoftBodyNode->getBodyVelocity().head<3>().cross(mX) + mParentSoftBodyNode->getBodyVelocity().tail<3>() - + get_dq(); + + getGenVels(); assert(!math::isNan(mV)); } void PointMass::updateEta() { // eta = w(parent) x dq - Eigen::Vector3d dq = get_dq(); + Eigen::Vector3d dq = getGenVels(); mEta = mParentSoftBodyNode->getBodyVelocity().head<3>().cross(dq); assert(!math::isNan(mEta)); } @@ -302,7 +302,7 @@ void PointMass::updateAcceleration() // dv = dw(parent) x mX + dv(parent) + eata + ddq mdV = mParentSoftBodyNode->getBodyAcceleration().head<3>().cross(mX) + mParentSoftBodyNode->getBodyAcceleration().tail<3>() + - mEta + get_ddq(); + mEta + getGenAccs(); assert(!math::isNan(mdV)); } @@ -350,7 +350,7 @@ void PointMass::updateArticulatedInertia(double _dt) void PointMass::updateGeneralizedForce(bool _withDampingForces) { // tau = f - set_tau(mF); + setGenForces(mF); } void PointMass::updateBiasForce(double _dt, const Eigen::Vector3d& _gravity) @@ -375,15 +375,15 @@ void PointMass::updateBiasForce(double _dt, const Eigen::Vector3d& _gravity) double ke = mParentSoftBodyNode->getEdgeSpringStiffness(); double kd = mParentSoftBodyNode->getDampingCoefficient(); int nN = mConnectedPointMasses.size(); - mAlpha = get_tau() - - (kv + nN * ke) * get_q() - - (_dt * (kv + nN * ke) + kd) * get_dq() + mAlpha = getGenForces() + - (kv + nN * ke) * getConfigs() + - (_dt * (kv + nN * ke) + kd) * getGenVels() - mMass * mEta - mB; for (int i = 0; i < mConnectedPointMasses.size(); ++i) { - mAlpha += ke * (mConnectedPointMasses[i]->get_q() - + _dt * mConnectedPointMasses[i]->get_dq()); + mAlpha += ke * (mConnectedPointMasses[i]->getConfigs() + + _dt * mConnectedPointMasses[i]->getGenVels()); } assert(!math::isNan(mAlpha)); @@ -401,7 +401,7 @@ void PointMass::update_ddq() * (mAlpha - mMass * (mParentSoftBodyNode->getBodyAcceleration().head<3>().cross(mX) + mParentSoftBodyNode->getBodyAcceleration().tail<3>())); - set_ddq(ddq); + setGenAccs(ddq); assert(!math::isNan(ddq)); // Update dv @@ -418,7 +418,7 @@ void PointMass::update_F_fs() void PointMass::updateMassMatrix() { - mM_dV = get_ddq() + mM_dV = getGenAccs() + mParentSoftBodyNode->mM_dV.head<3>().cross(mX) + mParentSoftBodyNode->mM_dV.tail<3>(); assert(!math::isNan(mM_dV)); @@ -444,17 +444,17 @@ void PointMass::aggregateAugMassMatrix(Eigen::MatrixXd* _MCol, int _col, double d = mParentSoftBodyNode->getDampingCoefficient(); double kv = mParentSoftBodyNode->getVertexSpringStiffness(); _MCol->block<3, 1>(iStart, _col).noalias() - = mM_F + (_timeStep * _timeStep * kv + _timeStep * d) * get_ddq(); + = mM_F + (_timeStep * _timeStep * kv + _timeStep * d) * getGenAccs(); } void PointMass::updateInvMassMatrix() { - mInvM_beta = get_tau(); + mInvM_beta = getGenForces(); } void PointMass::updateInvAugMassMatrix() { - mInvM_beta = mMass * mImplicitPsi * get_tau(); + mInvM_beta = mMass * mImplicitPsi * getGenForces(); } void PointMass::aggregateInvMassMatrix(Eigen::MatrixXd* _MInvCol, int _col) @@ -463,7 +463,7 @@ void PointMass::aggregateInvMassMatrix(Eigen::MatrixXd* _MInvCol, int _col) // We assume that the three generalized coordinates are in a row. int iStart = getGenCoord(0)->getSkeletonIndex(); _MInvCol->block<3, 1>(iStart, _col) - = mPsi * get_tau() + = mPsi * getGenForces() - mParentSoftBodyNode->mInvM_U.head<3>().cross(mX) - mParentSoftBodyNode->mInvM_U.tail<3>(); } @@ -476,7 +476,7 @@ void PointMass::aggregateInvAugMassMatrix(Eigen::MatrixXd* _MInvCol, int _col, int iStart = getGenCoord(0)->getSkeletonIndex(); _MInvCol->block<3, 1>(iStart, _col) = mImplicitPsi - * (get_tau() + * (getGenForces() - mMass * (mParentSoftBodyNode->mInvM_U.head<3>().cross(mX) + mParentSoftBodyNode->mInvM_U.tail<3>())); } diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 8e530d2d794ba..db79c0c3ccce3 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -239,7 +239,7 @@ Marker* Skeleton::getMarker(const std::string& _name) const { return NULL; } -Eigen::VectorXd Skeleton::getConfig(const std::vector& _id) const { +Eigen::VectorXd Skeleton::getConfigs(const std::vector& _id) const { Eigen::VectorXd q(_id.size()); for (unsigned int i = 0; i < _id.size(); i++) @@ -248,11 +248,12 @@ Eigen::VectorXd Skeleton::getConfig(const std::vector& _id) const { return q; } -Eigen::VectorXd Skeleton::getConfig() const { - return get_q(); +Eigen::VectorXd Skeleton::getConfigs() const +{ + return GenCoordSystem::getConfigs(); } -void Skeleton::setConfig(const std::vector& _id, +void Skeleton::setConfigs(const std::vector& _id, const Eigen::VectorXd& _config) { for ( unsigned int i = 0; i < _id.size(); i++ ) mGenCoords[_id[i]]->setConfig(_config(i)); @@ -286,18 +287,22 @@ void Skeleton::setConfig(const std::vector& _id, } } -void Skeleton::setConfig(const Eigen::VectorXd& _config) { - set_q(_config); +void Skeleton::setConfigs(const Eigen::VectorXd& _configs, + bool _updateCartesian) +{ + GenCoordSystem::setConfigs(_configs, _updateCartesian); for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) { + it != mBodyNodes.end(); ++it) + { (*it)->updateTransform(); (*it)->updateVelocity(); (*it)->updateEta(); } for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); - it != mBodyNodes.rend(); ++it) { + it != mBodyNodes.rend(); ++it) + { (*it)->updateArticulatedInertia(mTimeStep); } @@ -312,7 +317,8 @@ void Skeleton::setConfig(const Eigen::VectorXd& _config) { // mIsDampingForceVectorDirty = true; for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) { + it != mBodyNodes.end(); ++it) + { (*it)->mIsBodyJacobianDirty = true; (*it)->mIsBodyJacobianTimeDerivDirty = true; } @@ -320,8 +326,8 @@ void Skeleton::setConfig(const Eigen::VectorXd& _config) { void Skeleton::setState(const Eigen::VectorXd& _state) { - set_q(_state.head(_state.size() / 2)); - set_dq(_state.tail(_state.size() / 2)); + GenCoordSystem::setConfigs(_state.head(_state.size() / 2)); + GenCoordSystem::setGenVels(_state.tail(_state.size() / 2)); for (std::vector::iterator it = mBodyNodes.begin(); it != mBodyNodes.end(); ++it) @@ -370,7 +376,7 @@ void Skeleton::setState(const Eigen::VectorXd& _state) Eigen::VectorXd Skeleton::getState() { Eigen::VectorXd state(2 * mGenCoords.size()); - state << get_q(), get_dq(); + state << getConfigs(), getGenVels(); return state; } @@ -423,7 +429,7 @@ const Eigen::VectorXd& Skeleton::getExternalForceVector() { } Eigen::VectorXd Skeleton::getInternalForceVector() const { - return get_tau(); + return getGenForces(); } const Eigen::VectorXd& Skeleton::getDampingForceVector() { @@ -455,13 +461,13 @@ void Skeleton::updateMassMatrix() { mM.setZero(); // Backup the origianl internal force - Eigen::VectorXd originalGenAcceleration = get_ddq(); + Eigen::VectorXd originalGenAcceleration = getGenAccs(); int dof = getNumGenCoords(); Eigen::VectorXd e = Eigen::VectorXd::Zero(dof); for (int j = 0; j < dof; ++j) { e[j] = 1.0; - set_ddq(e); + setGenAccs(e); // Prepare cache data for (std::vector::iterator it = mBodyNodes.begin(); @@ -488,7 +494,7 @@ void Skeleton::updateMassMatrix() { mM.triangularView() = mM.transpose(); // Restore the origianl internal force - set_ddq(originalGenAcceleration); + setGenAccs(originalGenAcceleration); mIsMassMatrixDirty = false; } @@ -500,13 +506,13 @@ void Skeleton::updateAugMassMatrix() { mAugM.setZero(); // Backup the origianl internal force - Eigen::VectorXd originalGenAcceleration = get_ddq(); + Eigen::VectorXd originalGenAcceleration = getGenAccs(); int dof = getNumGenCoords(); Eigen::VectorXd e = Eigen::VectorXd::Zero(dof); for (int j = 0; j < dof; ++j) { e[j] = 1.0; - set_ddq(e); + setGenAccs(e); // Prepare cache data for (std::vector::iterator it = mBodyNodes.begin(); @@ -533,7 +539,7 @@ void Skeleton::updateAugMassMatrix() { mAugM.triangularView() = mAugM.transpose(); // Restore the origianl internal force - set_ddq(originalGenAcceleration); + setGenAccs(originalGenAcceleration); mIsAugMassMatrixDirty = false; } @@ -547,13 +553,13 @@ void Skeleton::updateInvMassMatrix() { // mInvM.setZero(); // Backup the origianl internal force - Eigen::VectorXd originalInternalForce = get_tau(); + Eigen::VectorXd originalInternalForce = getGenForces(); int dof = getNumGenCoords(); Eigen::VectorXd e = Eigen::VectorXd::Zero(dof); for (int j = 0; j < dof; ++j) { e[j] = 1.0; - set_tau(e); + setGenForces(e); // Prepare cache data for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); @@ -580,7 +586,7 @@ void Skeleton::updateInvMassMatrix() { mInvM.triangularView() = mInvM.transpose(); // Restore the origianl internal force - set_tau(originalInternalForce); + setGenForces(originalInternalForce); mIsInvMassMatrixDirty = false; } @@ -594,13 +600,13 @@ void Skeleton::updateInvAugMassMatrix() { // mInvM.setZero(); // Backup the origianl internal force - Eigen::VectorXd originalInternalForce = get_tau(); + Eigen::VectorXd originalInternalForce = getGenForces(); int dof = getNumGenCoords(); Eigen::VectorXd e = Eigen::VectorXd::Zero(dof); for (int j = 0; j < dof; ++j) { e[j] = 1.0; - set_tau(e); + setGenForces(e); // Prepare cache data for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); @@ -627,7 +633,7 @@ void Skeleton::updateInvAugMassMatrix() { mInvAugM.triangularView() = mInvAugM.transpose(); // Restore the origianl internal force - set_tau(originalInternalForce); + setGenForces(originalInternalForce); mIsInvAugMassMatrixDirty = false; } @@ -767,27 +773,27 @@ void Skeleton::computeForwardDynamics() { } void Skeleton::setInternalForceVector(const Eigen::VectorXd& _forces) { - set_tau(_forces); + setGenForces(_forces); } void Skeleton::setMinInternalForceVector(const Eigen::VectorXd& _minForces) { - set_tauMin(_minForces); + setGenForcesMin(_minForces); } Eigen::VectorXd Skeleton::getMinInternalForces() const { - return get_tauMin(); + return getGenForcesMin(); } void Skeleton::setMaxInternalForceVector(const Eigen::VectorXd& _maxForces) { - set_tauMax(_maxForces); + setGenForcesMax(_maxForces); } Eigen::VectorXd Skeleton::getMaxInternalForceVector() const { - return get_tauMax(); + return getGenForcesMax(); } void Skeleton::clearInternalForces() { - set_tau(Eigen::VectorXd::Zero(getNumGenCoords())); + setGenForces(Eigen::VectorXd::Zero(getNumGenCoords())); } void Skeleton::setConstraintForceVector(const Eigen::VectorXd& _Fc) { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index eb65b81aa831b..58b905c683525 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -144,24 +144,24 @@ class Skeleton : public GenCoordSystem /// \brief Get marker whose name is _name Marker* getMarker(const std::string& _name) const; + //---------------- Virtual functions for GenCoordSystem ---------------------- + /// \copydoc GenCoordSystem::setConfigs() + virtual void setConfigs(const Eigen::VectorXd& _configs, + bool _updateCartesian = true); + + /// \copydoc GenCoordSystem::getConfigs() + virtual Eigen::VectorXd getConfigs() const; + //------------------ Properties updated by dynamics -------------------------- /// \brief Set the configuration of this skeleton described in generalized /// coordinates. The order of input configuration is determined by /// _id. - void setConfig(const std::vector& _id, const Eigen::VectorXd& _config); - - /// \brief Set the configuration of this skeleton described in generalized - /// coordinates. - void setConfig(const Eigen::VectorXd& _config); + void setConfigs(const std::vector& _id, const Eigen::VectorXd& _config); /// \brief Get the configuration of this skeleton described in generalized /// coordinates. The returned order of configuration is determined by /// _id. - Eigen::VectorXd getConfig(const std::vector& _id) const; - - /// \brief Get the configuration of this skeleton described in generalized - /// coordinates. - Eigen::VectorXd getConfig() const; + Eigen::VectorXd getConfigs(const std::vector& _id) const; /// \brief Set the state of this skeleton described in generalized /// coordinates. @@ -171,6 +171,7 @@ class Skeleton : public GenCoordSystem /// coordinates. Eigen::VectorXd getState(); + //---------------------------------------------------------------------------- /// \brief Get mass matrix of the skeleton. const Eigen::MatrixXd& getMassMatrix(); diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index fd0262406b39e..451edeeaa0095 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -419,7 +419,7 @@ void SoftBodyNode::updateBiasForce(double _timeStep, int dof = mParentJoint->getNumGenCoords(); if (dof > 0) { - mAlpha = mParentJoint->get_tau() + mAlpha = mParentJoint->getGenForces() + mParentJoint->getSpringForces(_timeStep) + mParentJoint->getDampingForces(); for (int i = 0; i < dof; i++) @@ -536,8 +536,8 @@ void SoftBodyNode::aggregateAugMassMatrix(Eigen::MatrixXd* _MCol, int _col, int iStart = mParentJoint->getGenCoord(0)->getSkeletonIndex(); _MCol->block(iStart, _col, dof, 1).noalias() = mParentJoint->getLocalJacobian().transpose() * mM_F - + D * (_timeStep * mParentJoint->get_ddq()) - + K * (_timeStep * _timeStep * mParentJoint->get_ddq()); + + D * (_timeStep * mParentJoint->getGenAccs()) + + K * (_timeStep * _timeStep * mParentJoint->getGenAccs()); } } @@ -567,7 +567,7 @@ void SoftBodyNode::updateInvMassMatrix() int dof = mParentJoint->getNumGenCoords(); if (dof > 0) { - mInvM_a = mParentJoint->get_tau(); + mInvM_a = mParentJoint->getGenForces(); mInvM_a.noalias() -= mParentJoint->getLocalJacobian().transpose() * mInvM_c; assert(!math::isNan(mInvM_a)); } @@ -608,7 +608,7 @@ void SoftBodyNode::updateInvAugMassMatrix() int dof = mParentJoint->getNumGenCoords(); if (dof > 0) { - mInvM_a = mParentJoint->get_tau(); + mInvM_a = mParentJoint->getGenForces(); mInvM_a.noalias() -= mParentJoint->getLocalJacobian().transpose() * mInvM_c; assert(!math::isNan(mInvM_a)); } diff --git a/dart/dynamics/SoftSkeleton.cpp b/dart/dynamics/SoftSkeleton.cpp index 86678b2662518..28374664f7fdd 100644 --- a/dart/dynamics/SoftSkeleton.cpp +++ b/dart/dynamics/SoftSkeleton.cpp @@ -126,14 +126,14 @@ void SoftSkeleton::updateExternalForceVector() int nN = pm->getNumConnectedPointMasses(); // Vertex restoring force - Eigen::Vector3d Fext = -(kv + nN * ke) * pm->get_q() - - (mTimeStep * (kv + nN*ke)) * pm->get_dq(); + Eigen::Vector3d Fext = -(kv + nN * ke) * pm->getConfigs() + - (mTimeStep * (kv + nN*ke)) * pm->getGenVels(); // Edge restoring force for (int j = 0; j < nN; ++j) { - Fext += ke * (pm->getConnectedPointMass(j)->get_q() - + mTimeStep * pm->getConnectedPointMass(j)->get_dq()); + Fext += ke * (pm->getConnectedPointMass(j)->getConfigs() + + mTimeStep * pm->getConnectedPointMass(j)->getGenVels()); } // Assign @@ -154,7 +154,7 @@ void SoftSkeleton::updateDampingForceVector() { PointMass* pm = (*it)->getPointMass(i); int iStart = pm->getGenCoord(0)->getSkeletonIndex(); - mFd.segment<3>(iStart) = -(*it)->getDampingCoefficient() * pm->get_dq(); + mFd.segment<3>(iStart) = -(*it)->getDampingCoefficient() * pm->getGenVels(); } } } diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index 064ad8d1f7232..88717410fba88 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -63,7 +63,7 @@ TranslationalJoint::~TranslationalJoint() { void TranslationalJoint::updateTransform() { mT = mT_ParentBodyToJoint - * Eigen::Translation3d(get_q()) + * Eigen::Translation3d(getConfigs()) * mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); } diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 9bd2f03855a46..72637004ad3ad 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -105,7 +105,7 @@ void SimWindow::draw() { for (unsigned int i = 0; i < nSkels; i++) { int start = mWorld->getIndex(i); int size = mWorld->getSkeleton(i)->getNumGenCoords(); - mWorld->getSkeleton(i)->setConfig( + mWorld->getSkeleton(i)->setConfigs( mBakedStates[mPlayFrame].segment(start, size)); } if (mShowMarkers) { @@ -223,7 +223,7 @@ void SimWindow::bake() { for (unsigned int i = 0; i < mWorld->getNumSkeletons(); i++) { state.segment(mWorld->getIndex(i), mWorld->getSkeleton(i)->getNumGenCoords()) = - mWorld->getSkeleton(i)->get_q(); + mWorld->getSkeleton(i)->getConfigs(); } for (int i = 0; i < nContacts; i++) { int begin = mWorld->getIndex(nSkeletons) + i * 6; diff --git a/dart/planning/PathPlanner.h b/dart/planning/PathPlanner.h index bfeed8664afd1..a80b71dfee745 100644 --- a/dart/planning/PathPlanner.h +++ b/dart/planning/PathPlanner.h @@ -88,7 +88,7 @@ bool PathPlanner::planPath(dynamics::Skeleton* robot, const std::vector const std::vector &start, const std::vector &goal, std::list &path) { - Eigen::VectorXd savedConfiguration = robot->getConfig(dofs); + Eigen::VectorXd savedConfiguration = robot->getConfigs(dofs); // ==================================================================== // Check for collisions in the start and goal configurations @@ -96,7 +96,7 @@ bool PathPlanner::planPath(dynamics::Skeleton* robot, const std::vector // Sift through the possible start configurations and eliminate those that are in collision std::vector feasibleStart; for(unsigned int i = 0; i < start.size(); i++) { - robot->setConfig(dofs, start[i]); + robot->setConfigs(dofs, start[i]); if(!world->checkCollision()) feasibleStart.push_back(start[i]); } @@ -109,7 +109,7 @@ bool PathPlanner::planPath(dynamics::Skeleton* robot, const std::vector // Sift through the possible goal configurations and eliminate those that are in collision std::vector feasibleGoal; for(unsigned int i = 0; i < goal.size(); i++) { - robot->setConfig(dofs, goal[i]); + robot->setConfigs(dofs, goal[i]); if(!world->checkCollision()) feasibleGoal.push_back(goal[i]); } @@ -132,7 +132,7 @@ bool PathPlanner::planPath(dynamics::Skeleton* robot, const std::vector } // Restore previous robot configuration - robot->setConfig(dofs, savedConfiguration); + robot->setConfigs(dofs, savedConfiguration); return result; } diff --git a/dart/planning/PathShortener.cpp b/dart/planning/PathShortener.cpp index 5da7d53d3515c..4211c4dddd760 100644 --- a/dart/planning/PathShortener.cpp +++ b/dart/planning/PathShortener.cpp @@ -33,7 +33,7 @@ void PathShortener::shortenPath(list &path) printf("--> Start Brute Force Shortener \n"); srand(time(NULL)); - VectorXd savedDofs = robot->getConfig(dofs); + VectorXd savedDofs = robot->getConfigs(dofs); const int numShortcuts = path.size() * 5; @@ -63,7 +63,7 @@ void PathShortener::shortenPath(list &path) path.splice(node2Iter, intermediatePoints); } } - robot->setConfig(dofs, savedDofs); + robot->setConfigs(dofs, savedDofs); printf("End Brute Force Shortener \n"); } @@ -90,7 +90,7 @@ bool PathShortener::segmentCollisionFree(list &intermediatePoints, con VectorXd midpoint = (double)n2 / (double)n * config1 + (double)n1 / (double)n * config2; list intermediatePoints1, intermediatePoints2; - robot->setConfig(dofs, midpoint); + robot->setConfigs(dofs, midpoint); if(!world->checkCollision() && segmentCollisionFree(intermediatePoints1, config1, midpoint) && segmentCollisionFree(intermediatePoints2, midpoint, config2)) { diff --git a/dart/planning/RRT.cpp b/dart/planning/RRT.cpp index bea35f118cde9..28e535b55ecd1 100644 --- a/dart/planning/RRT.cpp +++ b/dart/planning/RRT.cpp @@ -222,7 +222,7 @@ void RRT::tracePath(int node, std::list &path, bool reverse) { /* ********************************************************************************************* */ bool RRT::checkCollisions(const VectorXd &c) { - robot->setConfig(dofs, c); + robot->setConfigs(dofs, c); return world->checkCollision(); } diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 0e7a6c49fc43a..e9c4f69373648 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -81,8 +81,8 @@ Eigen::VectorXd World::getState() const { for (unsigned int i = 0; i < getNumSkeletons(); i++) { int start = mIndices[i] * 2; int size = getSkeleton(i)->getNumGenCoords(); - state.segment(start, size) = getSkeleton(i)->get_q(); - state.segment(start + size, size) = getSkeleton(i)->get_dq(); + state.segment(start, size) = getSkeleton(i)->getConfigs(); + state.segment(start + size, size) = getSkeleton(i)->getGenVels(); } return state; @@ -134,11 +134,11 @@ Eigen::VectorXd World::evalDeriv() { int size = getSkeleton(i)->getNumGenCoords(); // set velocities - deriv.segment(start, size) = getSkeleton(i)->get_dq() - + mTimeStep * getSkeleton(i)->get_ddq(); + deriv.segment(start, size) = getSkeleton(i)->getGenVels() + + mTimeStep * getSkeleton(i)->getGenAccs(); // set qddot (accelerations) - deriv.segment(start + size, size) = getSkeleton(i)->get_ddq(); + deriv.segment(start + size, size) = getSkeleton(i)->getGenAccs(); } return deriv; diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 6ba50d122b31a..7a15b7ca83ff0 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -586,7 +586,7 @@ dynamics::RevoluteJoint* SkelParser::readRevoluteJoint( double init_pos = getValueDouble(_jointElement, "init_pos"); Eigen::VectorXd ipos = Eigen::VectorXd(1); ipos << init_pos; - newRevoluteJoint->set_q(ipos); + newRevoluteJoint->setConfigs(ipos); } //-------------------------------------------------------------------------- @@ -595,7 +595,7 @@ dynamics::RevoluteJoint* SkelParser::readRevoluteJoint( double init_vel = getValueDouble(_jointElement, "init_vel"); Eigen::VectorXd ivel = Eigen::VectorXd(1); ivel << init_vel; - newRevoluteJoint->set_q(ivel); + newRevoluteJoint->setConfigs(ivel); } return newRevoluteJoint; @@ -650,7 +650,7 @@ dynamics::PrismaticJoint* SkelParser::readPrismaticJoint( double init_pos = getValueDouble(_jointElement, "init_pos"); Eigen::VectorXd ipos = Eigen::VectorXd(1); ipos << init_pos; - newPrismaticJoint->set_q(ipos); + newPrismaticJoint->setConfigs(ipos); } //-------------------------------------------------------------------------- @@ -659,7 +659,7 @@ dynamics::PrismaticJoint* SkelParser::readPrismaticJoint( double init_vel = getValueDouble(_jointElement, "init_vel"); Eigen::VectorXd ivel = Eigen::VectorXd(1); ivel << init_vel; - newPrismaticJoint->set_q(ivel); + newPrismaticJoint->setConfigs(ivel); } return newPrismaticJoint; @@ -720,7 +720,7 @@ dynamics::ScrewJoint* SkelParser::readScrewJoint( double init_pos = getValueDouble(_jointElement, "init_pos"); Eigen::VectorXd ipos = Eigen::VectorXd(1); ipos << init_pos; - newScrewJoint->set_q(ipos); + newScrewJoint->setConfigs(ipos); } //-------------------------------------------------------------------------- @@ -729,7 +729,7 @@ dynamics::ScrewJoint* SkelParser::readScrewJoint( double init_vel = getValueDouble(_jointElement, "init_vel"); Eigen::VectorXd ivel = Eigen::VectorXd(1); ivel << init_vel; - newScrewJoint->set_q(ivel); + newScrewJoint->setConfigs(ivel); } return newScrewJoint; @@ -819,14 +819,14 @@ dynamics::UniversalJoint* SkelParser::readUniversalJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector2d init_pos = getValueVector2d(_jointElement, "init_pos"); - newUniversalJoint->set_q(init_pos); + newUniversalJoint->setConfigs(init_pos); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector2d init_vel = getValueVector2d(_jointElement, "init_vel"); - newUniversalJoint->set_q(init_vel); + newUniversalJoint->setConfigs(init_vel); } return newUniversalJoint; @@ -842,14 +842,14 @@ dynamics::BallJoint* SkelParser::readBallJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos"); - newBallJoint->set_q(init_pos); + newBallJoint->setConfigs(init_pos); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel"); - newBallJoint->set_q(init_vel); + newBallJoint->setConfigs(init_vel); } return newBallJoint; @@ -970,14 +970,14 @@ dynamics::EulerJoint* SkelParser::readEulerJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos"); - newEulerJoint->set_q(init_pos); + newEulerJoint->setConfigs(init_pos); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel"); - newEulerJoint->set_q(init_vel); + newEulerJoint->setConfigs(init_vel); } return newEulerJoint; @@ -994,14 +994,14 @@ dynamics::TranslationalJoint* SkelParser::readTranslationalJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos"); - newTranslationalJoint->set_q(init_pos); + newTranslationalJoint->setConfigs(init_pos); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel"); - newTranslationalJoint->set_q(init_vel); + newTranslationalJoint->setConfigs(init_vel); } return newTranslationalJoint; @@ -1017,14 +1017,14 @@ dynamics::FreeJoint* SkelParser::readFreeJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector6d init_pos = getValueVector6d(_jointElement, "init_pos"); - newFreeJoint->set_q(init_pos); + newFreeJoint->setConfigs(init_pos); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector6d init_vel = getValueVector6d(_jointElement, "init_vel"); - newFreeJoint->set_q(init_vel); + newFreeJoint->setConfigs(init_vel); } return newFreeJoint; diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 5088cce44c3d3..4b1ffdb32c051 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -83,7 +83,7 @@ simulation::World* DartLoader::parseWorld(std::string _urdfFileName) { if(dynamic_cast(rootJoint)) { Eigen::Vector6d coordinates; coordinates << math::logMap(transform.linear()), transform.translation(); - rootJoint->set_q(coordinates); + rootJoint->setConfigs(coordinates); } else { rootJoint->setTransformFromParentBodyNode(transform); diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 08f0edf8861fb..5f0fcd177a105 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -250,7 +250,7 @@ void DynamicsTest::compareVelocities(const std::string& _fileName) VectorXd state = VectorXd::Zero(dof * 2); state << q, dq; skeleton->setState(state); - skeleton->set_ddq(ddq); + skeleton->setGenAccs(ddq); skeleton->computeInverseDynamicsLinear(true, true, false, false); // For each body node @@ -375,7 +375,7 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) VectorXd x = VectorXd::Zero(dof * 2); x << q, dq; skeleton->setState(x); - skeleton->set_ddq(ddq); + skeleton->setGenAccs(ddq); // Set x(k+1) = x(k) + dt * dx(k) VectorXd qNext = q + timeStep * dq; @@ -391,7 +391,7 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) // Calculation of velocities and Jacobian at k-th time step skeleton->setState(x); - skeleton->set_ddq(ddq); + skeleton->setGenAccs(ddq); skeleton->computeInverseDynamicsLinear(true, true, false, false); Vector6d vBody1 = bn->getBodyVelocity(); Vector6d vWorld1 = bn->getWorldVelocity(); @@ -407,7 +407,7 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) // Calculation of velocities and Jacobian at (k+1)-th time step skeleton->setState(xNext); - skeleton->set_ddq(ddq); + skeleton->setGenAccs(ddq); skeleton->computeInverseDynamicsLinear(true, true, false, false); Vector6d vBody2 = bn->getBodyVelocity(); Vector6d vWorld2 = bn->getWorldVelocity(); @@ -653,26 +653,26 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) // Get C2, Coriolis force vector using inverse dynamics algorithm Vector3d oldGravity = skel->getGravity(); VectorXd oldTau = skel->getInternalForceVector(); - VectorXd oldDdq = skel->get_ddq(); + VectorXd oldDdq = skel->getGenAccs(); // TODO(JS): Save external forces of body nodes skel->clearInternalForces(); skel->clearExternalForces(); - skel->set_ddq(VectorXd::Zero(dof)); + skel->setGenAccs(VectorXd::Zero(dof)); EXPECT_TRUE(skel->getInternalForceVector() == VectorXd::Zero(dof)); EXPECT_TRUE(skel->getExternalForceVector() == VectorXd::Zero(dof)); - EXPECT_TRUE(skel->get_ddq() == VectorXd::Zero(dof)); + EXPECT_TRUE(skel->getGenAccs() == VectorXd::Zero(dof)); skel->setGravity(Vector3d::Zero()); EXPECT_TRUE(skel->getGravity() == Vector3d::Zero()); skel->computeInverseDynamicsLinear(false, false, false, false); - VectorXd C2 = skel->get_tau(); + VectorXd C2 = skel->getGenForces(); skel->setGravity(oldGravity); EXPECT_TRUE(skel->getGravity() == oldGravity); skel->computeInverseDynamicsLinear(false, false, false, false); - VectorXd Cg2 = skel->get_tau(); + VectorXd Cg2 = skel->getGenForces(); EXPECT_TRUE(equals(C, C2, 1e-6)); if (!equals(C, C2, 1e-6)) @@ -688,8 +688,8 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) cout << "Cg2:" << Cg2.transpose() << endl; } - skel->set_tau(oldTau); - skel->set_ddq(oldDdq); + skel->setGenForces(oldTau); + skel->setGenAccs(oldDdq); // TODO(JS): Restore external forces of body nodes //------------------- Combined Force Vector Test ----------------------- @@ -783,16 +783,16 @@ void DynamicsTest::centerOfMass(const std::string& _fileName) x[k] = random(lb, ub); skel->setState(x); - VectorXd tau = skel->get_tau(); + VectorXd tau = skel->getGenForces(); for (int k = 0; k < tau.size(); ++k) tau[k] = random(lb, ub); - skel->set_tau(tau); + skel->setGenForces(tau); skel->computeForwardDynamics(); - VectorXd q = skel->get_q(); - VectorXd dq = skel->get_dq(); - VectorXd ddq = skel->get_ddq(); + VectorXd q = skel->getConfigs(); + VectorXd dq = skel->getGenVels(); + VectorXd ddq = skel->getGenAccs(); VectorXd com = skel->getWorldCOM(); VectorXd dcom = skel->getWorldCOMVelocity(); diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 8e185ac339974..52b48e2820937 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -68,7 +68,7 @@ TEST(FORWARD_KINEMATICS, YAW_ROLL) // position for (size_t i = 0; i < numTests; i++) { - robot->setConfig(twoLinkIndices, joints[i]); + robot->setConfigs(twoLinkIndices, joints[i]); Vector3d actual = robot->getBodyNode("ee")->getWorldTransform().translation(); bool equality = equals(actual, expectedPos[i], 1e-3); @@ -110,7 +110,7 @@ TEST(FORWARD_KINEMATICS, TWO_ROLLS) // position for (size_t i = 0; i < numTests; i++) { - robot->setConfig(twoLinkIndices, joints[i]); + robot->setConfigs(twoLinkIndices, joints[i]); Vector3d actual = robot->getBodyNode("ee")->getWorldTransform().translation(); bool equality = equals(actual, expectedPos[i], 1e-3); diff --git a/unittests/testInverseKinematics.cpp b/unittests/testInverseKinematics.cpp index 8d2b2f06a6cf4..33dfd9c691f59 100644 --- a/unittests/testInverseKinematics.cpp +++ b/unittests/testInverseKinematics.cpp @@ -107,7 +107,7 @@ TEST(InverseKinematics, FittingTransformation) Vector3d(0.3, 0.3, l2), DOF_ROLL); robot->init(); size_t dof = robot->getNumGenCoords(); - VectorXd oldConfig = robot->getConfig(); + VectorXd oldConfig = robot->getConfigs(); BodyNode* body1 = robot->getBodyNode(0); BodyNode* body2 = robot->getBodyNode(1); @@ -130,7 +130,7 @@ TEST(InverseKinematics, FittingTransformation) 0.0, TOLERANCE); // Set to initial configuration - robot->setConfig(oldConfig); + robot->setConfigs(oldConfig); } //----------------------- Revolute joint test --------------------------------- @@ -140,17 +140,17 @@ TEST(InverseKinematics, FittingTransformation) { // Store the original transformation and joint angle Isometry3d oldT2 = body2->getWorldTransform(); - VectorXd oldQ2 = joint2->get_q(); + VectorXd oldQ2 = joint2->getConfigs(); // Get desiredT2 by rotating the revolute joint with random angle - joint2->set_q(VectorXd::Random(1)); - robot->setConfig(robot->getConfig()); + joint2->setConfigs(VectorXd::Random(1)); + robot->setConfigs(robot->getConfigs()); Isometry3d desiredT2 = body2->getWorldTransform(); // Transform body2 to the original transofrmation and check if it is done // well - joint2->set_q(oldQ2); - robot->setConfig(robot->getConfig()); + joint2->setConfigs(oldQ2); + robot->setConfigs(robot->getConfigs()); EXPECT_NEAR( math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(), 0.0, TOLERANCE); @@ -173,18 +173,18 @@ TEST(InverseKinematics, FittingTransformation) // Store the original transformation and joint angle Isometry3d oldT2 = body2->getWorldTransform(); - VectorXd oldQ2 = joint2->get_q(); + VectorXd oldQ2 = joint2->getConfigs(); // Get desiredT2 by rotating the revolute joint with random angle out of // the joint limit range joint2->getGenCoord(0)->setConfig(math::random(DART_RADIAN * 15.5, DART_PI)); - robot->setConfig(robot->getConfig()); + robot->setConfigs(robot->getConfigs()); Isometry3d desiredT2 = body2->getWorldTransform(); // Transform body2 to the original transofrmation and check if it is done // well - joint2->set_q(oldQ2); - robot->setConfig(robot->getConfig()); + joint2->setConfigs(oldQ2); + robot->setConfigs(robot->getConfigs()); EXPECT_NEAR( math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(), 0.0, TOLERANCE); @@ -244,7 +244,7 @@ TEST(InverseKinematics, FittingVelocity) Vector3d diff = fittedLinVel - desiredVel; EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE); EXPECT_NEAR(fittedAngVel.dot(fittedAngVel), 0.0, TOLERANCE); - joint1->set_dq(Vector6d::Zero()); + joint1->setGenVels(Vector6d::Zero()); robot->setState(robot->getState()); // Test for angular velocity @@ -255,7 +255,7 @@ TEST(InverseKinematics, FittingVelocity) diff = fittedAngVel - desiredVel; EXPECT_NEAR(fittedLinVel.dot(fittedLinVel), 0.0, TOLERANCE); EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE); - joint1->set_dq(Vector6d::Zero()); + joint1->setGenVels(Vector6d::Zero()); robot->setState(robot->getState()); } } diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 26881a31aa4ae..6a664eff31129 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -126,15 +126,15 @@ void JOINTS::kinematicsTest(Joint* _joint) { // a Eigen::VectorXd q_a = q; - _joint->set_q(q_a); - skeleton.setConfig(q_a); + _joint->setConfigs(q_a); + skeleton.setConfigs(q_a); Eigen::Isometry3d T_a = _joint->getLocalTransform(); // b Eigen::VectorXd q_b = q; q_b(i) += q_delta; - _joint->set_q(q_b); - skeleton.setConfig(q_b); + _joint->setConfigs(q_b); + skeleton.setConfigs(q_b); Eigen::Isometry3d T_b = _joint->getLocalTransform(); // @@ -172,15 +172,15 @@ void JOINTS::kinematicsTest(Joint* _joint) { // a Eigen::VectorXd q_a = q; - _joint->set_q(q_a); - skeleton.setConfig(q_a); + _joint->setConfigs(q_a); + skeleton.setConfigs(q_a); Jacobian J_a = _joint->getLocalJacobian(); // b Eigen::VectorXd q_b = q; q_b(i) += q_delta; - _joint->set_q(q_b); - skeleton.setConfig(q_b); + _joint->setConfigs(q_b); + skeleton.setConfigs(q_b); Jacobian J_b = _joint->getLocalJacobian(); // @@ -204,7 +204,7 @@ void JOINTS::kinematicsTest(Joint* _joint) for (int i = 0; i < dof; ++i) q(i) = random(posMin, posMax); - skeleton.setConfig(q); + skeleton.setConfigs(q); if (_joint->getNumGenCoords() == 0) return; diff --git a/unittests/testSoftDynamics.cpp b/unittests/testSoftDynamics.cpp index 570b9f50572d7..48fdf53b4651a 100644 --- a/unittests/testSoftDynamics.cpp +++ b/unittests/testSoftDynamics.cpp @@ -429,7 +429,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) // Get C2, Coriolis force vector using inverse dynamics algorithm Vector3d oldGravity = skel->getGravity(); VectorXd oldTau = skel->getInternalForceVector(); - VectorXd oldDdq = skel->get_ddq(); + VectorXd oldDdq = skel->getGenAccs(); // TODO(JS): Save external forces of body nodes vector oldKv(nSoftBodyNodes, 0.0); vector oldKe(nSoftBodyNodes, 0.0); @@ -445,7 +445,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) skel->clearInternalForces(); skel->clearExternalForces(); - skel->set_ddq(VectorXd::Zero(dof)); + skel->setGenAccs(VectorXd::Zero(dof)); for (int k = 0; k < nSoftBodyNodes; ++k) { assert(softSkel != NULL); @@ -457,17 +457,17 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) EXPECT_TRUE(skel->getInternalForceVector() == VectorXd::Zero(dof)); EXPECT_TRUE(skel->getExternalForceVector() == VectorXd::Zero(dof)); - EXPECT_TRUE(skel->get_ddq() == VectorXd::Zero(dof)); + EXPECT_TRUE(skel->getGenAccs() == VectorXd::Zero(dof)); skel->setGravity(Vector3d::Zero()); EXPECT_TRUE(skel->getGravity() == Vector3d::Zero()); skel->computeInverseDynamicsLinear(false, false, false, false); - VectorXd C2 = skel->get_tau(); + VectorXd C2 = skel->getGenForces(); skel->setGravity(oldGravity); EXPECT_TRUE(skel->getGravity() == oldGravity); skel->computeInverseDynamicsLinear(false, false, false, false); - VectorXd Cg2 = skel->get_tau(); + VectorXd Cg2 = skel->getGenForces(); EXPECT_TRUE(equals(C, C2, 1e-6)); if (!equals(C, C2, 1e-6)) @@ -483,8 +483,8 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) cout << "Cg2:" << Cg2.transpose() << endl; } - skel->set_tau(oldTau); - skel->set_ddq(oldDdq); + skel->setGenForces(oldTau); + skel->setGenAccs(oldDdq); // TODO(JS): Restore external forces of body nodes } } From 119148aab639f38ffdce3bcc6bdcd70b3075d9e4 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 21 Mar 2014 02:55:34 -0400 Subject: [PATCH 05/11] Clean up interface of Skeleton for generalized coordinate (still need more clean up) --- apps/balance/Main.cpp | 2 +- apps/harnessTest/Main.cpp | 2 +- dart/dynamics/GenCoordSystem.cpp | 7 +- dart/dynamics/GenCoordSystem.h | 9 +- dart/dynamics/Skeleton.cpp | 232 +++++++++++++++++++--------- dart/dynamics/Skeleton.h | 155 ++++++++++++------- dart/dynamics/SoftSkeleton.h | 4 +- dart/planning/PathShortener.cpp | 6 +- dart/planning/RRT.cpp | 2 +- dart/simulation/World.cpp | 2 +- unittests/testDynamics.cpp | 18 +-- unittests/testForwardKinematics.cpp | 4 +- unittests/testSoftDynamics.cpp | 13 +- 13 files changed, 290 insertions(+), 166 deletions(-) diff --git a/apps/balance/Main.cpp b/apps/balance/Main.cpp index 4fee8fd4ef1d4..567880e6eefc5 100644 --- a/apps/balance/Main.cpp +++ b/apps/balance/Main.cpp @@ -64,7 +64,7 @@ int main(int argc, char* argv[]) { genCoordIds.push_back(13); // lower back Eigen::VectorXd initConfig(8); initConfig << -0.1, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1; - myWorld->getSkeleton(1)->setConfigs(genCoordIds, initConfig); + myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig); // create controller Controller* myController = new Controller(myWorld->getSkeleton(1), diff --git a/apps/harnessTest/Main.cpp b/apps/harnessTest/Main.cpp index 5fe7e181a1f63..b2fd943f55fb0 100644 --- a/apps/harnessTest/Main.cpp +++ b/apps/harnessTest/Main.cpp @@ -68,7 +68,7 @@ int main(int argc, char* argv[]) genCoordIds.push_back(13); // lower back Eigen::VectorXd initConfig(9); initConfig << -0.1, 0.2, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1; - myWorld->getSkeleton(1)->setConfigs(genCoordIds, initConfig); + myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig); // create controller Controller* myController = new Controller(myWorld->getSkeleton(1), diff --git a/dart/dynamics/GenCoordSystem.cpp b/dart/dynamics/GenCoordSystem.cpp index a56c644a22acd..799528d8885c1 100644 --- a/dart/dynamics/GenCoordSystem.cpp +++ b/dart/dynamics/GenCoordSystem.cpp @@ -73,8 +73,7 @@ GenCoord* GenCoordSystem::getGenCoord(const std::string& _name) const } //============================================================================== -void GenCoordSystem::setConfigs(const Eigen::VectorXd& _configs, - bool _updateCartesian) +void GenCoordSystem::setConfigs(const Eigen::VectorXd& _configs) { assert(_configs.size() == getNumGenCoords()); @@ -118,7 +117,7 @@ void GenCoordSystem::setGenForces(const Eigen::VectorXd& _forces) } //============================================================================== -void GenCoordSystem::setConfigsMin(const Eigen::VectorXd& _configsMin, bool _updateCartesian) +void GenCoordSystem::setConfigsMin(const Eigen::VectorXd& _configsMin) { assert(_configsMin.size() == getNumGenCoords()); @@ -162,7 +161,7 @@ void GenCoordSystem::setGenForcesMin(const Eigen::VectorXd& _forcesMin) } //============================================================================== -void GenCoordSystem::setConfigsMax(const Eigen::VectorXd& _configsMax, bool _updateCartesian) +void GenCoordSystem::setConfigsMax(const Eigen::VectorXd& _configsMax) { assert(_configsMax.size() == getNumGenCoords()); diff --git a/dart/dynamics/GenCoordSystem.h b/dart/dynamics/GenCoordSystem.h index 36d76f9d0c50b..c04267ff5b3f9 100644 --- a/dart/dynamics/GenCoordSystem.h +++ b/dart/dynamics/GenCoordSystem.h @@ -65,22 +65,19 @@ class GenCoordSystem //--------------------------- Configurations --------------------------------- /// \brief Set configurations defined in terms of generalized coordinates - virtual void setConfigs(const Eigen::VectorXd& _configs, - bool _updateCartesian = true); + virtual void setConfigs(const Eigen::VectorXd& _configs); /// \brief Get configurations defined in terms of generalized coordinates virtual Eigen::VectorXd getConfigs() const; /// \brief Set lower bounds for configurations - virtual void setConfigsMin(const Eigen::VectorXd& _configsMin, - bool _updateCartesian = true); + virtual void setConfigsMin(const Eigen::VectorXd& _configsMin); /// \brief Get lower bounds for configurations virtual Eigen::VectorXd getConfigsMin() const; /// \brief Set upper bounds for configurations - virtual void setConfigsMax(const Eigen::VectorXd& _configsMax, - bool _updateCartesian = true); + virtual void setConfigsMax(const Eigen::VectorXd& _configsMax); /// \brief Get uppoer bounds for configurations virtual Eigen::VectorXd getConfigsMax() const; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index db79c0c3ccce3..bbfa5089aaf91 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -239,7 +239,9 @@ Marker* Skeleton::getMarker(const std::string& _name) const { return NULL; } -Eigen::VectorXd Skeleton::getConfigs(const std::vector& _id) const { +//============================================================================== +Eigen::VectorXd Skeleton::getConfigSegs(const std::vector& _id) const +{ Eigen::VectorXd q(_id.size()); for (unsigned int i = 0; i < _id.size(); i++) @@ -248,64 +250,112 @@ Eigen::VectorXd Skeleton::getConfigs(const std::vector& _id) const { return q; } -Eigen::VectorXd Skeleton::getConfigs() const +//============================================================================== +void Skeleton::setConfigSegs(const std::vector& _id, + const Eigen::VectorXd& _configs, + bool _updateTransforms, + bool _updateVels, + bool _updateAccs) { - return GenCoordSystem::getConfigs(); + for ( unsigned int i = 0; i < _id.size(); i++ ) + mGenCoords[_id[i]]->setConfig(_configs(i)); + + computeForwardKinematics(_updateTransforms, _updateVels, _updateAccs); } -void Skeleton::setConfigs(const std::vector& _id, - const Eigen::VectorXd& _config) { - for ( unsigned int i = 0; i < _id.size(); i++ ) - mGenCoords[_id[i]]->setConfig(_config(i)); +//============================================================================== +void Skeleton::setConfigs(const Eigen::VectorXd& _configs, + bool _updateTransforms, + bool _updateVels, + bool _updateAccs) +{ + GenCoordSystem::setConfigs(_configs); - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) { - (*it)->updateTransform(); - (*it)->updateVelocity(); - (*it)->updateEta(); - } + computeForwardKinematics(_updateTransforms, _updateVels, _updateAccs); +} - for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); - it != mBodyNodes.rend(); ++it) { - (*it)->updateArticulatedInertia(mTimeStep); - } +//============================================================================== +void Skeleton::setGenVels(const Eigen::VectorXd& _genVels, + bool _updateVels, + bool _updateAccs) +{ + GenCoordSystem::setGenVels(_genVels); - mIsMassMatrixDirty = true; - mIsAugMassMatrixDirty = true; - mIsInvMassMatrixDirty = true; - mIsInvAugMassMatrixDirty = true; - mIsCoriolisVectorDirty = true; - mIsGravityForceVectorDirty = true; - mIsCombinedVectorDirty = true; - mIsExternalForceVectorDirty = true; - // mIsDampingForceVectorDirty = true; + computeForwardKinematics(false, _updateVels, _updateAccs); +} - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) { - (*it)->mIsBodyJacobianDirty = true; - (*it)->mIsBodyJacobianTimeDerivDirty = true; - } +//============================================================================== +void Skeleton::setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs) +{ + GenCoordSystem::setGenAccs(_genAccs); + + computeForwardKinematics(false, false, _updateAccs); } -void Skeleton::setConfigs(const Eigen::VectorXd& _configs, - bool _updateCartesian) +//============================================================================== +void Skeleton::setState(const Eigen::VectorXd& _state, + bool _updateTransforms, + bool _updateVels, + bool _updateAccs) { - GenCoordSystem::setConfigs(_configs, _updateCartesian); + assert(_state.size() % 2 == 0); + GenCoordSystem::setConfigs(_state.head(_state.size() / 2)); + GenCoordSystem::setGenVels(_state.tail(_state.size() / 2)); - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) + // computeForwardKinematics(_updateTransforms, _updateVels, _updateAccs); + computeForwardKinematicsIssue122(_updateTransforms, _updateVels, _updateAccs); +} + +//============================================================================== +Eigen::VectorXd Skeleton::getState() const +{ + Eigen::VectorXd state(2 * mGenCoords.size()); + state << getConfigs(), getGenVels(); + return state; +} + +//============================================================================== +void Skeleton::computeForwardKinematics(bool _updateTransforms, + bool _updateVels, + bool _updateAccs) +{ + if (_updateTransforms) { - (*it)->updateTransform(); - (*it)->updateVelocity(); - (*it)->updateEta(); + for (std::vector::iterator it = mBodyNodes.begin(); + it != mBodyNodes.end(); ++it) + { + (*it)->updateTransform(); + } } + if (_updateVels) + { + for (std::vector::iterator it = mBodyNodes.begin(); + it != mBodyNodes.end(); ++it) + { + (*it)->updateVelocity(); + (*it)->updateEta(); + } + } + + if (_updateAccs) + { + for (std::vector::iterator it = mBodyNodes.begin(); + it != mBodyNodes.end(); ++it) + { + (*it)->updateAcceleration(); + } + } + + // TODO(JS): This will be moved to forward dynamics and hybrid dynamics + // routine for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); it != mBodyNodes.rend(); ++it) { (*it)->updateArticulatedInertia(mTimeStep); } + // TODO(JS): mIsMassMatrixDirty = true; mIsAugMassMatrixDirty = true; mIsInvMassMatrixDirty = true; @@ -314,8 +364,9 @@ void Skeleton::setConfigs(const Eigen::VectorXd& _configs, mIsGravityForceVectorDirty = true; mIsCombinedVectorDirty = true; mIsExternalForceVectorDirty = true; - // mIsDampingForceVectorDirty = true; +// mIsDampingForceVectorDirty = true; + // TODO(JS): for (std::vector::iterator it = mBodyNodes.begin(); it != mBodyNodes.end(); ++it) { @@ -324,38 +375,71 @@ void Skeleton::setConfigs(const Eigen::VectorXd& _configs, } } -void Skeleton::setState(const Eigen::VectorXd& _state) +//============================================================================== +void Skeleton::computeForwardKinematicsIssue122(bool _updateTransforms, + bool _updateVels, + bool _updateAccs) { - GenCoordSystem::setConfigs(_state.head(_state.size() / 2)); - GenCoordSystem::setGenVels(_state.tail(_state.size() / 2)); + if (_updateTransforms) + { + for (std::vector::iterator it = mBodyNodes.begin(); + it != mBodyNodes.end(); ++it) + { + // TODO(JS): This is workaround for Issue #122. + // TODO(JS): If possible we recomment not to use dynamic_cast. Fix for this + // issue should not to use dynamic_cast. + if (dynamic_cast((*it)->getParentJoint()) + || dynamic_cast((*it)->getParentJoint())) + { + (*it)->updateTransform_Issue122(mTimeStep); + } + else + { + (*it)->updateTransform(); + } + } + } - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) + if (_updateVels) { - // TODO(JS): This is workaround for Issue #122. - // TODO(JS): If possible we recomment not to use dynamic_cast. Fix for this - // issue should not to use dynamic_cast. - if (dynamic_cast((*it)->getParentJoint()) - || dynamic_cast((*it)->getParentJoint())) + for (std::vector::iterator it = mBodyNodes.begin(); + it != mBodyNodes.end(); ++it) { - (*it)->updateTransform_Issue122(mTimeStep); (*it)->updateVelocity(); - (*it)->updateEta_Issue122(); + + // TODO(JS): This is workaround for Issue #122. + // TODO(JS): If possible we recomment not to use dynamic_cast. Fix for this + // issue should not to use dynamic_cast. + if (dynamic_cast((*it)->getParentJoint()) + || dynamic_cast((*it)->getParentJoint())) + { + (*it)->updateEta_Issue122(); + } + else + { + (*it)->updateEta(); + } } - else + } + + if (_updateAccs) + { + for (std::vector::iterator it = mBodyNodes.begin(); + it != mBodyNodes.end(); ++it) { - (*it)->updateTransform(); - (*it)->updateVelocity(); - (*it)->updateEta(); + (*it)->updateAcceleration(); } } + // TODO(JS): This will be moved to forward dynamics and hybrid dynamics + // routine for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); it != mBodyNodes.rend(); ++it) { (*it)->updateArticulatedInertia(mTimeStep); } + // TODO(JS): mIsMassMatrixDirty = true; mIsAugMassMatrixDirty = true; mIsInvMassMatrixDirty = true; @@ -366,6 +450,7 @@ void Skeleton::setState(const Eigen::VectorXd& _state) mIsExternalForceVectorDirty = true; mIsDampingForceVectorDirty = true; + // TODO(JS): for (std::vector::iterator it = mBodyNodes.begin(); it != mBodyNodes.end(); ++it) { @@ -374,12 +459,6 @@ void Skeleton::setState(const Eigen::VectorXd& _state) } } -Eigen::VectorXd Skeleton::getState() { - Eigen::VectorXd state(2 * mGenCoords.size()); - state << getConfigs(), getGenVels(); - return state; -} - const Eigen::MatrixXd& Skeleton::getMassMatrix() { if (mIsMassMatrixDirty) updateMassMatrix(); @@ -467,7 +546,7 @@ void Skeleton::updateMassMatrix() { Eigen::VectorXd e = Eigen::VectorXd::Zero(dof); for (int j = 0; j < dof; ++j) { e[j] = 1.0; - setGenAccs(e); + GenCoordSystem::setGenAccs(e); // Prepare cache data for (std::vector::iterator it = mBodyNodes.begin(); @@ -512,7 +591,7 @@ void Skeleton::updateAugMassMatrix() { Eigen::VectorXd e = Eigen::VectorXd::Zero(dof); for (int j = 0; j < dof; ++j) { e[j] = 1.0; - setGenAccs(e); + GenCoordSystem::setGenAccs(e); // Prepare cache data for (std::vector::iterator it = mBodyNodes.begin(); @@ -539,7 +618,7 @@ void Skeleton::updateAugMassMatrix() { mAugM.triangularView() = mAugM.transpose(); // Restore the origianl internal force - setGenAccs(originalGenAcceleration); + GenCoordSystem::setGenAccs(originalGenAcceleration); mIsAugMassMatrixDirty = false; } @@ -717,28 +796,29 @@ void Skeleton::updateDampingForceVector() { } } -void Skeleton::computeInverseDynamicsLinear(bool _computeJacobian, - bool _computeJacobianDeriv, - bool _withExternalForces, - bool _withDampingForces) { +//============================================================================== +void Skeleton::computeInverseDynamics(bool _withExternalForces, + bool _withDampingForces) +{ // Skip immobile or 0-dof skeleton if (getNumGenCoords() == 0) return; - // Forward recursion - for (std::vector::iterator it = mBodyNodes.begin(); - it != mBodyNodes.end(); ++it) { - (*it)->updateAcceleration(); - } - // Backward recursion for (std::vector::reverse_iterator it = mBodyNodes.rbegin(); - it != mBodyNodes.rend(); ++it) { + it != mBodyNodes.rend(); ++it) + { (*it)->updateBodyForce(mGravity, _withExternalForces); (*it)->updateGeneralizedForce(_withDampingForces); } } +//============================================================================== +void Skeleton::computeHybridDynamics() +{ + dterr << "Not implemented yet.\n"; +} + void Skeleton::clearExternalForces() { for (std::vector::iterator it = mBodyNodes.begin(); it != mBodyNodes.end(); ++it) { diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 58b905c683525..a8116f0cfa58e 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -70,7 +70,7 @@ class Skeleton : public GenCoordSystem /// \brief Destructor virtual ~Skeleton(); - //-------------------------------------------------------------------------- + //------------------------------ Properties ---------------------------------- /// \brief Set name. void setName(const std::string& _name); @@ -78,23 +78,18 @@ class Skeleton : public GenCoordSystem const std::string& getName() const; /// \brief Set whether this skeleton allows self collisions between body - /// nodes in this skeleton. + /// nodes in this skeleton. /// \param[in] _isSelfCollidable True if the skeleton is checked for self - /// collision. + /// collision. void setSelfCollidable(bool _isSelfCollidable); /// \brief Get whether this skeleton allows self collisions between body - /// nodes in this skeleton. + /// nodes in this skeleton. /// \return True if the skeleton is checked for self collision. bool isSelfCollidable() const; /// \brief Set whether this skeleton will be updated by forward dynamics. /// \param[in] _isMobile True if this skeleton is mobile. - /// \warning This function should be called before this skeleton is added to - /// the world. If not, the constraint dynamics algorithm will not - /// work. If the user want to change the immobile state after this - /// skeleton is added to the world, the user should remove this - /// skeleton from the world and add it to the world again. void setMobile(bool _isMobile); /// \brief Get whether this skeleton will be updated by forward dynamics. @@ -102,21 +97,21 @@ class Skeleton : public GenCoordSystem bool isMobile() const; /// \brief Set time step. This timestep is used for implicit joint damping - /// force. + /// force. void setTimeStep(double _timeStep); /// \brief Get time step. double getTimeStep() const; /// \brief Set 3-dim gravitational acceleration. The gravity is used for - /// calculating gravity force vector of the skeleton. + /// calculating gravity force vector of the skeleton. void setGravity(const Eigen::Vector3d& _gravity); /// \brief Get 3-dim gravitational acceleration. const Eigen::Vector3d& getGravity() const; /// \brief Get total mass of the skeleton. The total mass is calculated at - /// init(). + /// init(). double getMass() const; //----------------------- Structueral Properties ----------------------------- @@ -144,42 +139,95 @@ class Skeleton : public GenCoordSystem /// \brief Get marker whose name is _name Marker* getMarker(const std::string& _name) const; - //---------------- Virtual functions for GenCoordSystem ---------------------- - /// \copydoc GenCoordSystem::setConfigs() - virtual void setConfigs(const Eigen::VectorXd& _configs, - bool _updateCartesian = true); + //--------------------------- Initialization --------------------------------- + /// \brief Initialize this skeleton for kinematics and dynamics + void init(double _timeStep = 0.001, + const Eigen::Vector3d& _gravity = Eigen::Vector3d(0.0, 0.0, -9.81)); - /// \copydoc GenCoordSystem::getConfigs() - virtual Eigen::VectorXd getConfigs() const; + //--------------- Interfaces for generalized coordinates --------------------- + /// \brief Set configurations defined in terms of generalized coordinates + /// and update Cartesian terms of body nodes using following parameters. + /// \param[in] _updateTransforms True to update transformations of body nodes + /// \param[in] _updateVels True to update spacial velocities of body nodes + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setConfigs(const Eigen::VectorXd& _configs, + bool _updateTransforms = true, + bool _updateVels = true, + bool _updateAccs = true); + + /// \brief Set generalized velocities + /// \param[in] _updateVels True to update spacial velocities of body nodes + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setGenVels(const Eigen::VectorXd& _genVels, + bool _updateVels = true, + bool _updateAccs = true); + + /// \brief Set generalized accelerations + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setGenAccs(const Eigen::VectorXd& _genAccs, + bool _updateAccs = true); - //------------------ Properties updated by dynamics -------------------------- /// \brief Set the configuration of this skeleton described in generalized - /// coordinates. The order of input configuration is determined by - /// _id. - void setConfigs(const std::vector& _id, const Eigen::VectorXd& _config); + /// coordinates. The order of input configuration is determined by _id. + /// \param[in] _id + /// \param[in] _configs + /// \param[in] _updateTransforms True to update transformations of body nodes + /// \param[in] _updateVels True to update spacial velocities of body nodes + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + void setConfigSegs(const std::vector& _id, + const Eigen::VectorXd& _configs, + bool _updateTransforms = true, + bool _updateVels = true, + bool _updateAccs = true); /// \brief Get the configuration of this skeleton described in generalized - /// coordinates. The returned order of configuration is determined by - /// _id. - Eigen::VectorXd getConfigs(const std::vector& _id) const; + /// coordinates. The returned order of configuration is determined by _id. + Eigen::VectorXd getConfigSegs(const std::vector& _id) const; + + /// \brief Set the state of this skeleton described in generalized coordinates + /// \param[in] _state + /// \param[in] _updateTransforms True to update transformations of body nodes + /// \param[in] _updateVels True to update spacial velocities of body nodes + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + void setState(const Eigen::VectorXd& _state, + bool _updateTransforms = true, + bool _updateVels = true, + bool _updateAccs = true); + + /// \brief Get the state of this skeleton described in generalized coordinates + Eigen::VectorXd getState() const; + + //----------------------- Kinematics algorithms ------------------------------ + /// \brief Compute forward kinematics + void computeForwardKinematics(bool _updateTransforms = true, + bool _updateVels = true, + bool _updateAccs = true); + + /// \brief Compute forward kinematics + /// \warning This is called by Skeleton::setState() as workaround for #122 + void computeForwardKinematicsIssue122(bool _updateTransforms = true, + bool _updateVels = true, + bool _updateAccs = true); + + //------------------------ Dynamics algorithms ------------------------------- + /// \brief Compute forward dynamics + void computeForwardDynamics(); - /// \brief Set the state of this skeleton described in generalized - /// coordinates. - void setState(const Eigen::VectorXd& _state); + /// \brief Compute inverse dynamics + void computeInverseDynamics(bool _withExternalForces = false, + bool _withDampingForces = false); - /// \brief Get the state of this skeleton described in generalized - /// coordinates. - Eigen::VectorXd getState(); + /// \brief Compute hybrid dynamics + void computeHybridDynamics(); - //---------------------------------------------------------------------------- + //------------------------- Equations of Motion ------------------------------ /// \brief Get mass matrix of the skeleton. const Eigen::MatrixXd& getMassMatrix(); /// \brief Get augmented mass matrix of the skeleton. This matrix is used - /// in ConstraintDynamics to compute constraint forces. The matrix is - /// M + h*D + h*h*K where D is diagonal joint damping coefficient - /// matrix, K is diagonal joint stiffness matrix, and h is simulation - /// time step. + /// in ConstraintDynamics to compute constraint forces. The matrix is + /// M + h*D + h*h*K where D is diagonal joint damping coefficient matrix, K is + /// diagonal joint stiffness matrix, and h is simulation time step. const Eigen::MatrixXd& getAugMassMatrix(); /// \brief Get inverse of mass matrix of the skeleton. @@ -229,7 +277,7 @@ class Skeleton : public GenCoordSystem void clearInternalForces(); /// \brief Clear external forces, which are manually added to the body nodes - /// by the user. + /// by the user. void clearExternalForces(); /// \brief Clear contact forces of the body nodes in this skeleton. @@ -262,20 +310,6 @@ class Skeleton : public GenCoordSystem /// \brief Get potential energy of this skeleton. virtual double getPotentialEnergy() const; - //------------------- Recursive dynamics algorithms -------------------------- - /// \brief Initialize this skeleton for kinematics and dynamics - void init(double _timeStep = 0.001, - const Eigen::Vector3d& _gravity = Eigen::Vector3d(0.0, 0.0, -9.81)); - - /// \brief (q, dq, ddq) --> (tau) - void computeInverseDynamicsLinear(bool _computeJacobian = true, - bool _computeJacobianDeriv = true, - bool _withExternalForces = false, - bool _withDampingForces = false); - - /// \brief (q, dq, tau) --> (ddq) - void computeForwardDynamics(); - //--------------------------- Rendering -------------------------------------- /// \brief Draw this skeleton void draw(renderer::RenderInterface* _ri = NULL, @@ -292,16 +326,15 @@ class Skeleton : public GenCoordSystem std::string mName; /// \brief Flags for selfcollision test. True if the collision detector checks - /// selfcollision. + /// selfcollision. bool mIsSelfCollidable; /// \brief List of body nodes in the skeleton. std::vector mBodyNodes; /// \brief If the skeleton is not mobile, its dynamic effect is equivalent - /// to having infinite mass. If the configuration of an immobile - /// skeleton are manually changed, the collision results might not be - /// correct. + /// to having infinite mass. If the configuration of an immobile skeleton are + /// manually changed, the collision results might not be correct. bool mIsMobile; /// \brief Time step for implicit joint damping force. @@ -313,6 +346,15 @@ class Skeleton : public GenCoordSystem /// \brief Total mass. double mTotalMass; + /// \brief True when configurations are modified + bool mIsConfigsModified; + + /// \brief True when generalized velocities are modified + bool mIsGenVelsModified; + + /// \brief True when generalized accelerations are modified + bool mIsGenAccsModified; + /// \brief Mass matrix for the skeleton. Eigen::MatrixXd mM; @@ -344,7 +386,7 @@ class Skeleton : public GenCoordSystem bool mIsCoriolisVectorDirty; /// \brief Gravity vector for the skeleton; computed in nonrecursive - /// dynamics only. + /// dynamics only. Eigen::VectorXd mG; /// \brief Dirty flag for the gravity force vector. @@ -371,6 +413,7 @@ class Skeleton : public GenCoordSystem /// \brief Dirty flag for the damping force vector. bool mIsDampingForceVectorDirty; +protected: /// \brief Update mass matrix of the skeleton. virtual void updateMassMatrix(); diff --git a/dart/dynamics/SoftSkeleton.h b/dart/dynamics/SoftSkeleton.h index ba36773903b29..7e2e96d12682e 100644 --- a/dart/dynamics/SoftSkeleton.h +++ b/dart/dynamics/SoftSkeleton.h @@ -75,8 +75,8 @@ class SoftSkeleton : public Skeleton int getNumRigidBodyNodes() const; /// \brief - void init(double _timeStep = 0.001, const Eigen::Vector3d& _gravity = - Eigen::Vector3d(0.0, 0.0, -9.81)); + void init(double _timeStep = 0.001, + const Eigen::Vector3d& _gravity = Eigen::Vector3d(0.0, 0.0, -9.81)); protected: // Documentation inherited. diff --git a/dart/planning/PathShortener.cpp b/dart/planning/PathShortener.cpp index 4211c4dddd760..323ed097b2565 100644 --- a/dart/planning/PathShortener.cpp +++ b/dart/planning/PathShortener.cpp @@ -33,7 +33,7 @@ void PathShortener::shortenPath(list &path) printf("--> Start Brute Force Shortener \n"); srand(time(NULL)); - VectorXd savedDofs = robot->getConfigs(dofs); + VectorXd savedDofs = robot->getConfigSegs(dofs); const int numShortcuts = path.size() * 5; @@ -63,7 +63,7 @@ void PathShortener::shortenPath(list &path) path.splice(node2Iter, intermediatePoints); } } - robot->setConfigs(dofs, savedDofs); + robot->setConfigSegs(dofs, savedDofs); printf("End Brute Force Shortener \n"); } @@ -90,7 +90,7 @@ bool PathShortener::segmentCollisionFree(list &intermediatePoints, con VectorXd midpoint = (double)n2 / (double)n * config1 + (double)n1 / (double)n * config2; list intermediatePoints1, intermediatePoints2; - robot->setConfigs(dofs, midpoint); + robot->setConfigSegs(dofs, midpoint); if(!world->checkCollision() && segmentCollisionFree(intermediatePoints1, config1, midpoint) && segmentCollisionFree(intermediatePoints2, midpoint, config2)) { diff --git a/dart/planning/RRT.cpp b/dart/planning/RRT.cpp index 28e535b55ecd1..3e6e1f340353e 100644 --- a/dart/planning/RRT.cpp +++ b/dart/planning/RRT.cpp @@ -222,7 +222,7 @@ void RRT::tracePath(int node, std::list &path, bool reverse) { /* ********************************************************************************************* */ bool RRT::checkCollisions(const VectorXd &c) { - robot->setConfigs(dofs, c); + robot->setConfigSegs(dofs, c); return world->checkCollision(); } diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index e9c4f69373648..9e28ccc98db90 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -92,7 +92,7 @@ void World::setState(const Eigen::VectorXd& _newState) { for (int i = 0; i < getNumSkeletons(); i++) { int start = 2 * mIndices[i]; int size = 2 * getSkeleton(i)->getNumGenCoords(); - getSkeleton(i)->setState(_newState.segment(start, size)); + getSkeleton(i)->setState(_newState.segment(start, size), true, true, false); } } diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 5f0fcd177a105..1ad6854a5b1e8 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -251,7 +251,7 @@ void DynamicsTest::compareVelocities(const std::string& _fileName) state << q, dq; skeleton->setState(state); skeleton->setGenAccs(ddq); - skeleton->computeInverseDynamicsLinear(true, true, false, false); + skeleton->computeInverseDynamics(false, false); // For each body node for (int k = 0; k < skeleton->getNumBodyNodes(); ++k) @@ -392,7 +392,7 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) // Calculation of velocities and Jacobian at k-th time step skeleton->setState(x); skeleton->setGenAccs(ddq); - skeleton->computeInverseDynamicsLinear(true, true, false, false); + skeleton->computeInverseDynamics(false, false); Vector6d vBody1 = bn->getBodyVelocity(); Vector6d vWorld1 = bn->getWorldVelocity(); MatrixXd JBody1 = bn->getBodyJacobian(); @@ -408,7 +408,7 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) // Calculation of velocities and Jacobian at (k+1)-th time step skeleton->setState(xNext); skeleton->setGenAccs(ddq); - skeleton->computeInverseDynamicsLinear(true, true, false, false); + skeleton->computeInverseDynamics(false, false); Vector6d vBody2 = bn->getBodyVelocity(); Vector6d vWorld2 = bn->getWorldVelocity(); MatrixXd JBody2 = bn->getBodyJacobian(); @@ -658,7 +658,7 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) skel->clearInternalForces(); skel->clearExternalForces(); - skel->setGenAccs(VectorXd::Zero(dof)); + skel->setGenAccs(VectorXd::Zero(dof), false); EXPECT_TRUE(skel->getInternalForceVector() == VectorXd::Zero(dof)); EXPECT_TRUE(skel->getExternalForceVector() == VectorXd::Zero(dof)); @@ -666,12 +666,12 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) skel->setGravity(Vector3d::Zero()); EXPECT_TRUE(skel->getGravity() == Vector3d::Zero()); - skel->computeInverseDynamicsLinear(false, false, false, false); + skel->computeInverseDynamics(false, false); VectorXd C2 = skel->getGenForces(); skel->setGravity(oldGravity); EXPECT_TRUE(skel->getGravity() == oldGravity); - skel->computeInverseDynamicsLinear(false, false, false, false); + skel->computeInverseDynamics(false, false); VectorXd Cg2 = skel->getGenForces(); EXPECT_TRUE(equals(C, C2, 1e-6)); @@ -689,7 +689,7 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) } skel->setGenForces(oldTau); - skel->setGenAccs(oldDdq); + skel->setGenAccs(oldDdq, false); // TODO(JS): Restore external forces of body nodes //------------------- Combined Force Vector Test ----------------------- @@ -720,9 +720,9 @@ void DynamicsTest::centerOfMass(const std::string& _fileName) //---------------------------- Settings -------------------------------------- // Number of random state tests for each skeletons #ifndef NDEBUG // Debug mode - int nRandomItr = 5; + int nRandomItr = 10; #else - int nRandomItr = 1; + int nRandomItr = 100; #endif // Lower and upper bound of configuration for system diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index 52b48e2820937..fa9d4256afb06 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -68,7 +68,7 @@ TEST(FORWARD_KINEMATICS, YAW_ROLL) // position for (size_t i = 0; i < numTests; i++) { - robot->setConfigs(twoLinkIndices, joints[i]); + robot->setConfigSegs(twoLinkIndices, joints[i]); Vector3d actual = robot->getBodyNode("ee")->getWorldTransform().translation(); bool equality = equals(actual, expectedPos[i], 1e-3); @@ -110,7 +110,7 @@ TEST(FORWARD_KINEMATICS, TWO_ROLLS) // position for (size_t i = 0; i < numTests; i++) { - robot->setConfigs(twoLinkIndices, joints[i]); + robot->setConfigSegs(twoLinkIndices, joints[i]); Vector3d actual = robot->getBodyNode("ee")->getWorldTransform().translation(); bool equality = equals(actual, expectedPos[i], 1e-3); diff --git a/unittests/testSoftDynamics.cpp b/unittests/testSoftDynamics.cpp index 48fdf53b4651a..54a9e4d063eaa 100644 --- a/unittests/testSoftDynamics.cpp +++ b/unittests/testSoftDynamics.cpp @@ -358,6 +358,11 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) x[k] = random(lb, ub); skel->setState(x); + ////////////////////////////////////////////////////////////////////////// + /// TODO: Workaround code for Issue #122 + skel->computeForwardKinematics(true, true, true); + ////////////////////////////////////////////////////////////////////////// + //------------------------ Mass Matrix Test ---------------------------- // Get matrices MatrixXd M = skel->getMassMatrix(); @@ -445,7 +450,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) skel->clearInternalForces(); skel->clearExternalForces(); - skel->setGenAccs(VectorXd::Zero(dof)); + skel->setGenAccs(VectorXd::Zero(dof), false); for (int k = 0; k < nSoftBodyNodes; ++k) { assert(softSkel != NULL); @@ -461,12 +466,12 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) skel->setGravity(Vector3d::Zero()); EXPECT_TRUE(skel->getGravity() == Vector3d::Zero()); - skel->computeInverseDynamicsLinear(false, false, false, false); + skel->computeInverseDynamics(false, false); VectorXd C2 = skel->getGenForces(); skel->setGravity(oldGravity); EXPECT_TRUE(skel->getGravity() == oldGravity); - skel->computeInverseDynamicsLinear(false, false, false, false); + skel->computeInverseDynamics(false, false); VectorXd Cg2 = skel->getGenForces(); EXPECT_TRUE(equals(C, C2, 1e-6)); @@ -484,7 +489,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) } skel->setGenForces(oldTau); - skel->setGenAccs(oldDdq); + skel->setGenAccs(oldDdq, false); // TODO(JS): Restore external forces of body nodes } } From 63ea3ecf18214d57304283a59273e4a8c45f4f81 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 21 Mar 2014 05:55:44 -0400 Subject: [PATCH 06/11] Add kinematics interface for joints --- Changelog.md | 2 +- apps/atlasRobot/Controller.cpp | 4 +- apps/atlasRobot/Main.cpp | 2 +- apps/balance/Main.cpp | 3 +- apps/ballJointConstraintTest/Main.cpp | 2 +- apps/closedLoop/Main.cpp | 2 +- apps/forwardSim/Main.cpp | 2 +- apps/hardcodedDesign/MyWindow.cpp | 2 +- apps/harnessTest/Main.cpp | 3 +- apps/softArticulatedBodiesTest/Main.cpp | 2 +- apps/softOpenChain/Main.cpp | 2 +- dart/dynamics/BallJoint.cpp | 5 +- dart/dynamics/BodyNode.cpp | 30 ++---- dart/dynamics/FreeJoint.cpp | 5 +- dart/dynamics/Joint.cpp | 95 +++++++++++++++++++ dart/dynamics/Joint.h | 63 ++++++++++++- dart/dynamics/Skeleton.cpp | 4 +- dart/gui/SimWindow.cpp | 2 +- dart/planning/PathShortener.cpp | 6 +- dart/planning/RRT.cpp | 3 +- dart/utils/SkelParser.cpp | 32 +++---- dart/utils/urdf/DartLoader.cpp | 2 +- unittests/testDynamics.cpp | 120 ++++++++++++------------ unittests/testForwardKinematics.cpp | 4 +- unittests/testInverseKinematics.cpp | 23 ++--- unittests/testJoints.cpp | 16 ++-- unittests/testSoftDynamics.cpp | 2 +- 27 files changed, 286 insertions(+), 152 deletions(-) diff --git a/Changelog.md b/Changelog.md index 5cbda9c1c6e93..6473cbf8ce935 100644 --- a/Changelog.md +++ b/Changelog.md @@ -10,7 +10,7 @@ 1. Added planar joint 1. Added initial implementation of soft body dynamics 1. Added useful kinematical properties pertain to COM of skeleton -1. Renewed optimizer interface and added nlopt solver +1. Improved optimizer interface and added nlopt solver * [Pull request #152](https://github.com/dartsim/dart/pull/152) 1. Added initial implementation of inverse kinematics functions * [Pull request #154](https://github.com/dartsim/dart/pull/154) diff --git a/apps/atlasRobot/Controller.cpp b/apps/atlasRobot/Controller.cpp index 254db63cdfdc9..72ad3414fadae 100644 --- a/apps/atlasRobot/Controller.cpp +++ b/apps/atlasRobot/Controller.cpp @@ -347,8 +347,8 @@ void Controller::unharnessRightFoot() void Controller::resetRobot() { int dof = mAtlasRobot->getNumGenCoords(); - mAtlasRobot->setConfigs(mInitialState.head(dof)); // See #122 - mAtlasRobot->setState(mInitialState); + mAtlasRobot->setConfigs(mInitialState.head(dof), true, true, false); // See #122 + mAtlasRobot->setState(mInitialState, true, true, false); dtmsg << "Robot is reset." << std::endl; } diff --git a/apps/atlasRobot/Main.cpp b/apps/atlasRobot/Main.cpp index 0d34e7fd9c02b..00cb3b177f8b8 100644 --- a/apps/atlasRobot/Main.cpp +++ b/apps/atlasRobot/Main.cpp @@ -76,7 +76,7 @@ int main(int argc, char* argv[]) // Set initial configuration for Atlas robot VectorXd q = atlas->getConfigs(); q[0] = -0.5 * DART_PI; - atlas->setConfigs(q); + atlas->setConfigs(q, true, true, false); // Set gravity of the world myWorld->setGravity(Vector3d(0.0, -9.81, 0.0)); diff --git a/apps/balance/Main.cpp b/apps/balance/Main.cpp index 567880e6eefc5..41cdef3d39fff 100644 --- a/apps/balance/Main.cpp +++ b/apps/balance/Main.cpp @@ -64,7 +64,8 @@ int main(int argc, char* argv[]) { genCoordIds.push_back(13); // lower back Eigen::VectorXd initConfig(8); initConfig << -0.1, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1; - myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig); + myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig, + true, true, false); // create controller Controller* myController = new Controller(myWorld->getSkeleton(1), diff --git a/apps/ballJointConstraintTest/Main.cpp b/apps/ballJointConstraintTest/Main.cpp index 71b163730bade..fca6ff5362d29 100644 --- a/apps/ballJointConstraintTest/Main.cpp +++ b/apps/ballJointConstraintTest/Main.cpp @@ -69,7 +69,7 @@ int main(int argc, char* argv[]) Eigen::VectorXd initPose(dof); for (int i = 0; i < dof; i++) initPose[i] = random(-0.5, 0.5); - myWorld->getSkeleton(0)->setConfigs(initPose); + myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false); // Add damping to every joint for (int i = 0; i < myWorld->getSkeleton(0)->getNumBodyNodes(); i++) { diff --git a/apps/closedLoop/Main.cpp b/apps/closedLoop/Main.cpp index 147314e99a790..ee78126b0b9d9 100644 --- a/apps/closedLoop/Main.cpp +++ b/apps/closedLoop/Main.cpp @@ -73,7 +73,7 @@ int main(int argc, char* argv[]) initPose[23] = 3.14159 * 0.4; initPose[26] = 3.14159 * 0.4; initPose[29] = 3.14159 * 0.4; - myWorld->getSkeleton(0)->setConfigs(initPose); + myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false); // create a ball joint constraint BodyNode *bd1 = myWorld->getSkeleton(0)->getBodyNode("link 6"); diff --git a/apps/forwardSim/Main.cpp b/apps/forwardSim/Main.cpp index fddeb0cd15dba..79afc8174b8b4 100644 --- a/apps/forwardSim/Main.cpp +++ b/apps/forwardSim/Main.cpp @@ -57,7 +57,7 @@ int main(int argc, char* argv[]) { Eigen::VectorXd initPose(dof); for (int i = 0; i < dof; i++) initPose[i] = dart::math::random(-0.5, 0.5); - myWorld->getSkeleton(0)->setConfigs(initPose); + myWorld->getSkeleton(0)->setConfigs(initPose, true, true, false); // create a window and link it to the world MyWindow window; diff --git a/apps/hardcodedDesign/MyWindow.cpp b/apps/hardcodedDesign/MyWindow.cpp index de71585d34bfe..d18efa92b932b 100644 --- a/apps/hardcodedDesign/MyWindow.cpp +++ b/apps/hardcodedDesign/MyWindow.cpp @@ -63,7 +63,7 @@ void MyWindow::keyboard(unsigned char _key, int _x, int _y) { size_t dofIdx = _key - 49; Eigen::VectorXd pose = skel->getConfigs(); pose(dofIdx) = pose(dofIdx) + (inverse ? -dDOF : dDOF); - skel->setConfigs(pose); + skel->setConfigs(pose, true, true, false); std::cout << "Updated pose DOF " << dofIdx << ": " << pose.transpose() << std::endl; glutPostRedisplay(); diff --git a/apps/harnessTest/Main.cpp b/apps/harnessTest/Main.cpp index b2fd943f55fb0..20774d44cdf33 100644 --- a/apps/harnessTest/Main.cpp +++ b/apps/harnessTest/Main.cpp @@ -68,7 +68,8 @@ int main(int argc, char* argv[]) genCoordIds.push_back(13); // lower back Eigen::VectorXd initConfig(9); initConfig << -0.1, 0.2, 0.2, -0.5, 0.3, 0.2, -0.5, 0.3, -0.1; - myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig); + myWorld->getSkeleton(1)->setConfigSegs(genCoordIds, initConfig, + true, true, false); // create controller Controller* myController = new Controller(myWorld->getSkeleton(1), diff --git a/apps/softArticulatedBodiesTest/Main.cpp b/apps/softArticulatedBodiesTest/Main.cpp index bbbcf4e2e070a..bb17b436b46e7 100644 --- a/apps/softArticulatedBodiesTest/Main.cpp +++ b/apps/softArticulatedBodiesTest/Main.cpp @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) Eigen::VectorXd initPose = Eigen::VectorXd::Zero(dof); for (int i = 0; i < 3; i++) initPose[i] = dart::math::random(-0.5, 0.5); - myWorld->getSkeleton(1)->setConfigs(initPose); + myWorld->getSkeleton(1)->setConfigs(initPose, true, true, false); // create a window and link it to the world MyWindow window; diff --git a/apps/softOpenChain/Main.cpp b/apps/softOpenChain/Main.cpp index fc70c4d346eb3..1d44f63f8d5bd 100644 --- a/apps/softOpenChain/Main.cpp +++ b/apps/softOpenChain/Main.cpp @@ -63,7 +63,7 @@ int main(int argc, char* argv[]) Eigen::VectorXd initPose = Eigen::VectorXd::Zero(dof); for (int i = 0; i < 3; i++) initPose[i] = dart::math::random(-0.5, 0.5); - myWorld->getSkeleton("skeleton 1")->setConfigs(initPose); + myWorld->getSkeleton("skeleton 1")->setConfigs(initPose, true, true, false); // create a window and link it to the world MyWindow window; diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index 0bbb4389880ff..3feb0966dfe35 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -79,7 +79,7 @@ inline void BallJoint::updateTransform() { void BallJoint::updateTransform_Issue122(double _timeStep) { mT_Joint = mT_Joint * math::expAngular(_timeStep * getGenVels()); - setConfigs(math::logMap(mT_Joint).head<3>()); + GenCoordSystem::setConfigs(math::logMap(mT_Joint).head<3>()); mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse(); @@ -151,7 +151,8 @@ inline void BallJoint::updateJacobianTimeDeriv() { void BallJoint::updateJacobianTimeDeriv_Issue122() { // mdS == 0 - assert(mdS == Eigen::MatrixXd::Zero(6, 3)); + mdS.setZero(); +// assert(mdS == Eigen::MatrixXd::Zero(6, 3)); } } // namespace dynamics diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 023537575ad39..3f0a267bc1fba 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -235,11 +235,7 @@ void BodyNode::fitWorldLinearVel(const Eigen::Vector3d& _targetLinVel, // Set optimal configuration of the parent joint Eigen::VectorXd jointDQ = prob.getOptimalSolution(); - parentJoint->setGenVels(jointDQ); - - // Update forward kinematics information - // TODO(JS): Need more efficient api for this - mSkeleton->setState(mSkeleton->getState()); + parentJoint->setGenVels(jointDQ, true, true); } void BodyNode::fitWorldAngularVel(const Eigen::Vector3d& _targetAngVel, @@ -276,11 +272,7 @@ void BodyNode::fitWorldAngularVel(const Eigen::Vector3d& _targetAngVel, // Set optimal configuration of the parent joint Eigen::VectorXd jointDQ = prob.getOptimalSolution(); - parentJoint->setGenVels(jointDQ); - - // Update forward kinematics information - // TODO(JS): Need more efficient api for this - mSkeleton->setState(mSkeleton->getState()); + parentJoint->setGenVels(jointDQ, true, true); } const Eigen::Isometry3d& BodyNode::getWorldTransform() const { @@ -928,7 +920,7 @@ void BodyNode::update_ddq() { ddq.noalias() = mImplicitPsi * mAlpha; } - mParentJoint->setGenAccs(ddq); + mParentJoint->GenCoordSystem::setGenAccs(ddq); assert(!math::isNan(ddq)); updateAcceleration(); @@ -1352,11 +1344,7 @@ void BodyNode::fitWorldTransformParentJointImpl( // Set optimal configuration of the parent joint Eigen::VectorXd jointQ = prob.getOptimalSolution(); - parentJoint->setConfigs(jointQ); - - // Update forward kinematics information - // TODO(JS): Need more efficient api for this - mSkeleton->setConfigs(mSkeleton->getConfigs()); + parentJoint->setConfigs(jointQ, true, true, true); } void BodyNode::fitWorldTransformAncestorJointsImpl( @@ -1386,9 +1374,8 @@ double BodyNode::TransformObjFunc::eval(Eigen::Map& _x) assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size()); // Update forward kinematics information with _x - // TODO(JS): Need more efficient api for this - mBodyNode->getParentJoint()->setConfigs(_x); - mSkeleton->setConfigs(mSkeleton->getConfigs()); + // We are just insterested in transformation of mBodyNode + mBodyNode->getParentJoint()->setConfigs(_x, true, false, false); // Compute and return the geometric distance between body node transformation // and target transformation @@ -1427,9 +1414,8 @@ double BodyNode::VelocityObjFunc::eval(Eigen::Map& _x) assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size()); // Update forward kinematics information with _x - // TODO(JS): Need more efficient api for this - mBodyNode->getParentJoint()->setGenVels(_x); - mSkeleton->setState(mSkeleton->getState()); + // We are just insterested in spacial velocity of mBodyNode + mBodyNode->getParentJoint()->setGenVels(_x, true, false); // Compute and return the geometric distance between body node transformation // and target transformation diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 33f966fb0341b..8a77af9c24386 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -78,7 +78,7 @@ void FreeJoint::updateTransform() { void FreeJoint::updateTransform_Issue122(double _timeStep) { mT_Joint = mT_Joint * math::expMap(_timeStep * getGenVels()); - setConfigs(math::logMap(mT_Joint)); + GenCoordSystem::setConfigs(math::logMap(mT_Joint)); mT = mT_ParentBodyToJoint * mT_Joint * mT_ChildBodyToJoint.inverse(); @@ -183,7 +183,8 @@ void FreeJoint::updateJacobianTimeDeriv() { void FreeJoint::updateJacobianTimeDeriv_Issue122() { // mdS == 0 - assert(mdS == Eigen::MatrixXd::Zero(6, 6)); +// assert(mdS == Eigen::MatrixXd::Zero(6, 6)); + mdS.setZero(); } void FreeJoint::clampRotation() { diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index fd5e80743a07e..83b6c16e5828f 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -42,12 +42,15 @@ #include "dart/common/Console.h" #include "dart/renderer/RenderInterface.h" #include "dart/dynamics/BodyNode.h" +#include "dart/dynamics/Skeleton.h" namespace dart { namespace dynamics { +//============================================================================== Joint::Joint(const std::string& _name) : mName(_name), + mSkeleton(NULL), mSkelIndex(-1), mIsPositionLimited(true), mT_ParentBodyToJoint(Eigen::Isometry3d::Identity()), @@ -147,6 +150,98 @@ double Joint::getDampingCoefficient(int _idx) const { return mDampingCoefficient[_idx]; } +//============================================================================== +void Joint::setConfig(size_t _idx, + double _config, + bool _updateTransforms, + bool _updateVels, + bool _updateAccs) +{ + assert(_idx < mGenCoords.size()); + + mGenCoords[_idx]->setConfig(_config); + + if (mSkeleton) + { + // TODO(JS): It would be good if we know whether the skeleton is initialzed. + mSkeleton->computeForwardKinematics(_updateTransforms, _updateVels, + _updateAccs); + } +} + +//============================================================================== +void Joint::setConfigs(const Eigen::VectorXd& _configs, + bool _updateTransforms, + bool _updateVels, + bool _updateAccs) +{ + GenCoordSystem::setConfigs(_configs); + + if (mSkeleton) + { + // TODO(JS): It would be good if we know whether the skeleton is initialzed. + mSkeleton->computeForwardKinematics(_updateTransforms, _updateVels, + _updateAccs); + } +} + +//============================================================================== +void Joint::setGenVel(size_t _idx, + double _genVel, + bool _updateVels, + bool _updateAccs) +{ + assert(_idx < mGenCoords.size()); + + mGenCoords[_idx]->setVel(_genVel); + + if (mSkeleton) + { + // TODO(JS): It would be good if we know whether the skeleton is initialzed. + mSkeleton->computeForwardKinematics(false, _updateVels, _updateAccs); + } +} + +//============================================================================== +void Joint::setGenVels(const Eigen::VectorXd& _genVels, + bool _updateVels, + bool _updateAccs) +{ + GenCoordSystem::setGenVels(_genVels); + + if (mSkeleton) + { + // TODO(JS): It would be good if we know whether the skeleton is initialzed. + mSkeleton->computeForwardKinematics(false, _updateVels, _updateAccs); + } +} + +//============================================================================== +void Joint::setGenAcc(size_t _idx, double _genAcc, bool _updateAccs) +{ + assert(_idx < mGenCoords.size()); + + mGenCoords[_idx]->setAcc(_genAcc); + + if (mSkeleton) + { + // TODO(JS): It would be good if we know whether the skeleton is initialzed. + mSkeleton->computeForwardKinematics(false, false, _updateAccs); + } +} + +//============================================================================== +void Joint::setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs) +{ + GenCoordSystem::setGenAccs(_genAccs); + + if (mSkeleton) + { + // TODO(JS): It would be good if we know whether the skeleton is initialzed. + mSkeleton->computeForwardKinematics(false, false, _updateAccs); + } +} + Eigen::VectorXd Joint::getDampingForces() const { int numDofs = getNumGenCoords(); Eigen::VectorXd dampingForce(numDofs); diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 80f21b2c3eed9..2d2c8d042a06e 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -56,17 +56,16 @@ namespace dynamics { class BodyNode; class Skeleton; -/// \class joint +/// \brief Joint class Joint : public GenCoordSystem { public: - /// \brief Set BodyNode as friend class + /// \brief Friend class declaration friend class BodyNode; public: - //---------------------- Constructor and Destructor -------------------------- /// \brief Constructor - Joint(const std::string& _name = "Noname Joint"); + explicit Joint(const std::string& _name = "Noname Joint"); /// \brief Destructor virtual ~Joint(); @@ -78,7 +77,8 @@ class Joint : public GenCoordSystem /// \brief Get joint name const std::string& getName() const; - /// \brief Get skeleton that this joint belongs to + /// \brief Get skeleton that this joint belongs to. The skeleton set by + /// init(). Skeleton* getSkeleton() const; /// \brief Get index of this joint in the skeleton that this joint belongs to @@ -130,6 +130,59 @@ class Joint : public GenCoordSystem /// \param[in] _idx Index of joint axis. double getDampingCoefficient(int _idx) const; + //----------------- Interface for generalized coordinates -------------------- + /// \brief Set single configuration in terms of generalized coordinates + /// \param[in] _idx + /// \param[in] _config + /// \param[in] _updateTransforms True to update transformations of body nodes + /// \param[in] _updateVels True to update spacial velocities of body nodes + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setConfig(size_t _idx, double _config, + bool _updateTransforms = true, + bool _updateVels = true, + bool _updateAccs = true); + + /// \brief Set configurations in terms of generalized coordinates + /// \param[in] _configs + /// \param[in] _updateTransforms True to update transformations of body nodes + /// \param[in] _updateVels True to update spacial velocities of body nodes + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setConfigs(const Eigen::VectorXd& _configs, + bool _updateTransforms = true, + bool _updateVels = true, + bool _updateAccs = true); + + /// \brief Set single generalized velocity + /// \param[in] _idx + /// \param[in] _genVel + /// \param[in] _updateVels True to update spacial velocities of body nodes + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setGenVel(size_t _idx, double _genVel, + bool _updateVels = true, + bool _updateAccs = true); + + /// \brief Set generalized velocities + /// \param[in] _genVels + /// \param[in] _updateVels True to update spacial velocities of body nodes + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setGenVels(const Eigen::VectorXd& _genVels, + bool _updateVels = true, + bool _updateAccs = true); + + /// \brief Set single generalized acceleration + /// \param[in] _idx + /// \param[in] _genAcc + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setGenAcc(size_t _idx, double _genAcc, + bool _updateAccs = true); + + /// \brief Set generalized accelerations + /// \param[in] _genAccs + /// \param[in] _updateAccs True to update spacial accelerations of body nodes + virtual void setGenAccs(const Eigen::VectorXd& _genAccs, + bool _updateAccs = true); + + //---------------------------------------------------------------------------- /// \brief Get potential energy. double getPotentialEnergy() const; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index bbfa5089aaf91..9333169e19e08 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -572,8 +572,8 @@ void Skeleton::updateMassMatrix() { } mM.triangularView() = mM.transpose(); - // Restore the origianl internal force - setGenAccs(originalGenAcceleration); + // Restore the origianl generalized accelerations + setGenAccs(originalGenAcceleration, false); mIsMassMatrixDirty = false; } diff --git a/dart/gui/SimWindow.cpp b/dart/gui/SimWindow.cpp index 72637004ad3ad..892caabb38092 100644 --- a/dart/gui/SimWindow.cpp +++ b/dart/gui/SimWindow.cpp @@ -106,7 +106,7 @@ void SimWindow::draw() { int start = mWorld->getIndex(i); int size = mWorld->getSkeleton(i)->getNumGenCoords(); mWorld->getSkeleton(i)->setConfigs( - mBakedStates[mPlayFrame].segment(start, size)); + mBakedStates[mPlayFrame].segment(start, size), true, false, false); } if (mShowMarkers) { int sumDofs = mWorld->getIndex(nSkels); diff --git a/dart/planning/PathShortener.cpp b/dart/planning/PathShortener.cpp index 323ed097b2565..a414eb76c41ae 100644 --- a/dart/planning/PathShortener.cpp +++ b/dart/planning/PathShortener.cpp @@ -63,7 +63,8 @@ void PathShortener::shortenPath(list &path) path.splice(node2Iter, intermediatePoints); } } - robot->setConfigSegs(dofs, savedDofs); + // TODO(JS): What kinematic values should be updated here? + robot->setConfigSegs(dofs, savedDofs, true, true, true); printf("End Brute Force Shortener \n"); } @@ -90,7 +91,8 @@ bool PathShortener::segmentCollisionFree(list &intermediatePoints, con VectorXd midpoint = (double)n2 / (double)n * config1 + (double)n1 / (double)n * config2; list intermediatePoints1, intermediatePoints2; - robot->setConfigSegs(dofs, midpoint); + // TODO(JS): What kinematic values should be updated here? + robot->setConfigSegs(dofs, midpoint, true, true, true); if(!world->checkCollision() && segmentCollisionFree(intermediatePoints1, config1, midpoint) && segmentCollisionFree(intermediatePoints2, midpoint, config2)) { diff --git a/dart/planning/RRT.cpp b/dart/planning/RRT.cpp index 3e6e1f340353e..64f6e46c44cb3 100644 --- a/dart/planning/RRT.cpp +++ b/dart/planning/RRT.cpp @@ -222,7 +222,8 @@ void RRT::tracePath(int node, std::list &path, bool reverse) { /* ********************************************************************************************* */ bool RRT::checkCollisions(const VectorXd &c) { - robot->setConfigSegs(dofs, c); + // TODO(JS): What kinematic values should be updated here? + robot->setConfigSegs(dofs, c, true, true, true); return world->checkCollision(); } diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp index 7a15b7ca83ff0..b5eab4f9b3859 100644 --- a/dart/utils/SkelParser.cpp +++ b/dart/utils/SkelParser.cpp @@ -586,7 +586,7 @@ dynamics::RevoluteJoint* SkelParser::readRevoluteJoint( double init_pos = getValueDouble(_jointElement, "init_pos"); Eigen::VectorXd ipos = Eigen::VectorXd(1); ipos << init_pos; - newRevoluteJoint->setConfigs(ipos); + newRevoluteJoint->setConfigs(ipos, false, false, false); } //-------------------------------------------------------------------------- @@ -595,7 +595,7 @@ dynamics::RevoluteJoint* SkelParser::readRevoluteJoint( double init_vel = getValueDouble(_jointElement, "init_vel"); Eigen::VectorXd ivel = Eigen::VectorXd(1); ivel << init_vel; - newRevoluteJoint->setConfigs(ivel); + newRevoluteJoint->setGenVels(ivel, false, false); } return newRevoluteJoint; @@ -650,7 +650,7 @@ dynamics::PrismaticJoint* SkelParser::readPrismaticJoint( double init_pos = getValueDouble(_jointElement, "init_pos"); Eigen::VectorXd ipos = Eigen::VectorXd(1); ipos << init_pos; - newPrismaticJoint->setConfigs(ipos); + newPrismaticJoint->setConfigs(ipos, false, false, false); } //-------------------------------------------------------------------------- @@ -659,7 +659,7 @@ dynamics::PrismaticJoint* SkelParser::readPrismaticJoint( double init_vel = getValueDouble(_jointElement, "init_vel"); Eigen::VectorXd ivel = Eigen::VectorXd(1); ivel << init_vel; - newPrismaticJoint->setConfigs(ivel); + newPrismaticJoint->setGenVels(ivel, false, false); } return newPrismaticJoint; @@ -720,7 +720,7 @@ dynamics::ScrewJoint* SkelParser::readScrewJoint( double init_pos = getValueDouble(_jointElement, "init_pos"); Eigen::VectorXd ipos = Eigen::VectorXd(1); ipos << init_pos; - newScrewJoint->setConfigs(ipos); + newScrewJoint->setConfigs(ipos, false, false, false); } //-------------------------------------------------------------------------- @@ -729,7 +729,7 @@ dynamics::ScrewJoint* SkelParser::readScrewJoint( double init_vel = getValueDouble(_jointElement, "init_vel"); Eigen::VectorXd ivel = Eigen::VectorXd(1); ivel << init_vel; - newScrewJoint->setConfigs(ivel); + newScrewJoint->setGenVels(ivel, false, false); } return newScrewJoint; @@ -819,14 +819,14 @@ dynamics::UniversalJoint* SkelParser::readUniversalJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector2d init_pos = getValueVector2d(_jointElement, "init_pos"); - newUniversalJoint->setConfigs(init_pos); + newUniversalJoint->setConfigs(init_pos, false, false, false); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector2d init_vel = getValueVector2d(_jointElement, "init_vel"); - newUniversalJoint->setConfigs(init_vel); + newUniversalJoint->setGenVels(init_vel, false, false); } return newUniversalJoint; @@ -842,14 +842,14 @@ dynamics::BallJoint* SkelParser::readBallJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos"); - newBallJoint->setConfigs(init_pos); + newBallJoint->setConfigs(init_pos, false, false, false); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel"); - newBallJoint->setConfigs(init_vel); + newBallJoint->setGenVels(init_vel, false, false); } return newBallJoint; @@ -970,14 +970,14 @@ dynamics::EulerJoint* SkelParser::readEulerJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos"); - newEulerJoint->setConfigs(init_pos); + newEulerJoint->setConfigs(init_pos, false, false, false); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel"); - newEulerJoint->setConfigs(init_vel); + newEulerJoint->setGenVels(init_vel, false, false); } return newEulerJoint; @@ -994,14 +994,14 @@ dynamics::TranslationalJoint* SkelParser::readTranslationalJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector3d init_pos = getValueVector3d(_jointElement, "init_pos"); - newTranslationalJoint->setConfigs(init_pos); + newTranslationalJoint->setConfigs(init_pos, false, false, false); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector3d init_vel = getValueVector3d(_jointElement, "init_vel"); - newTranslationalJoint->setConfigs(init_vel); + newTranslationalJoint->setGenVels(init_vel, false, false); } return newTranslationalJoint; @@ -1017,14 +1017,14 @@ dynamics::FreeJoint* SkelParser::readFreeJoint( // init_pos if (hasElement(_jointElement, "init_pos")) { Eigen::Vector6d init_pos = getValueVector6d(_jointElement, "init_pos"); - newFreeJoint->setConfigs(init_pos); + newFreeJoint->setConfigs(init_pos, false, false, false); } //-------------------------------------------------------------------------- // init_vel if (hasElement(_jointElement, "init_vel")) { Eigen::Vector6d init_vel = getValueVector6d(_jointElement, "init_vel"); - newFreeJoint->setConfigs(init_vel); + newFreeJoint->setConfigs(init_vel, false, false, false); } return newFreeJoint; diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 4b1ffdb32c051..77b39c8d45d89 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -83,7 +83,7 @@ simulation::World* DartLoader::parseWorld(std::string _urdfFileName) { if(dynamic_cast(rootJoint)) { Eigen::Vector6d coordinates; coordinates << math::logMap(transform.linear()), transform.translation(); - rootJoint->setConfigs(coordinates); + rootJoint->setConfigs(coordinates, false, false, false); } else { rootJoint->setTransformFromParentBodyNode(transform); diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index 1ad6854a5b1e8..f3caeaebae050 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -249,8 +249,8 @@ void DynamicsTest::compareVelocities(const std::string& _fileName) } VectorXd state = VectorXd::Zero(dof * 2); state << q, dq; - skeleton->setState(state); - skeleton->setGenAccs(ddq); + skeleton->setState(state, true, true, true); + skeleton->setGenAccs(ddq, true); skeleton->computeInverseDynamics(false, false); // For each body node @@ -374,8 +374,8 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) } VectorXd x = VectorXd::Zero(dof * 2); x << q, dq; - skeleton->setState(x); - skeleton->setGenAccs(ddq); + skeleton->setState(x, true, true, true); + skeleton->setGenAccs(ddq, true); // Set x(k+1) = x(k) + dt * dx(k) VectorXd qNext = q + timeStep * dq; @@ -390,8 +390,8 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) int nDepGenCoord = bn->getNumDependentGenCoords(); // Calculation of velocities and Jacobian at k-th time step - skeleton->setState(x); - skeleton->setGenAccs(ddq); + skeleton->setState(x, true, true, true); + skeleton->setGenAccs(ddq, true); skeleton->computeInverseDynamics(false, false); Vector6d vBody1 = bn->getBodyVelocity(); Vector6d vWorld1 = bn->getWorldVelocity(); @@ -406,8 +406,8 @@ void DynamicsTest::compareAccelerations(const std::string& _fileName) MatrixXd dJWorld1 = bn->getWorldJacobianTimeDeriv(); // Calculation of velocities and Jacobian at (k+1)-th time step - skeleton->setState(xNext); - skeleton->setGenAccs(ddq); + skeleton->setState(xNext, true, true, true); + skeleton->setGenAccs(ddq, true); skeleton->computeInverseDynamics(false, false); Vector6d vBody2 = bn->getBodyVelocity(); Vector6d vWorld2 = bn->getWorldVelocity(); @@ -582,7 +582,7 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) VectorXd x = skel->getState(); for (int k = 0; k < x.size(); ++k) x[k] = random(lb, ub); - skel->setState(x); + skel->setState(x, true, true, false); //------------------------ Mass Matrix Test ---------------------------- // Get matrices @@ -658,7 +658,7 @@ void DynamicsTest::compareEquationsOfMotion(const std::string& _fileName) skel->clearInternalForces(); skel->clearExternalForces(); - skel->setGenAccs(VectorXd::Zero(dof), false); + skel->setGenAccs(VectorXd::Zero(dof), true); EXPECT_TRUE(skel->getInternalForceVector() == VectorXd::Zero(dof)); EXPECT_TRUE(skel->getExternalForceVector() == VectorXd::Zero(dof)); @@ -781,7 +781,7 @@ void DynamicsTest::centerOfMass(const std::string& _fileName) VectorXd x = skel->getState(); for (int k = 0; k < x.size(); ++k) x[k] = random(lb, ub); - skel->setState(x); + skel->setState(x, true, true, false); VectorXd tau = skel->getGenForces(); for (int k = 0; k < tau.size(); ++k) @@ -823,55 +823,55 @@ void DynamicsTest::centerOfMass(const std::string& _fileName) delete myWorld; } -////============================================================================== -//TEST_F(DynamicsTest, compareVelocities) -//{ -// for (int i = 0; i < getList().size(); ++i) -// { -//#ifndef NDEBUG -// dtdbg << getList()[i] << std::endl; -//#endif -// compareVelocities(getList()[i]); -// } -//} - -////============================================================================== -//TEST_F(DynamicsTest, compareAccelerations) -//{ -// for (int i = 0; i < getList().size(); ++i) -// { -//#ifndef NDEBUG -// dtdbg << getList()[i] << std::endl; -//#endif -// compareAccelerations(getList()[i]); -// } -//} - -////============================================================================== -//TEST_F(DynamicsTest, compareEquationsOfMotion) -//{ -// for (int i = 0; i < getList().size(); ++i) -// { -// //////////////////////////////////////////////////////////////////////////// -// // TODO(JS): Following five skel files, which contain euler joints couldn't -// // pass EQUATIONS_OF_MOTION, are disabled. -// std::string skelFileName = getList()[i]; -// if (skelFileName == DART_DATA_PATH"skel/test/chainwhipa.skel" -// || skelFileName == DART_DATA_PATH"skel/test/serial_chain_eulerxyz_joint.skel" -// || skelFileName == DART_DATA_PATH"skel/test/simple_tree_structure_euler_joint.skel" -// || skelFileName == DART_DATA_PATH"skel/test/tree_structure_euler_joint.skel" -// || skelFileName == DART_DATA_PATH"skel/fullbody1.skel") -// { -// continue; -// } -// //////////////////////////////////////////////////////////////////////////// - -//#ifndef NDEBUG -// dtdbg << getList()[i] << std::endl; -//#endif -// compareEquationsOfMotion(getList()[i]); -// } -//} +//============================================================================== +TEST_F(DynamicsTest, compareVelocities) +{ + for (int i = 0; i < getList().size(); ++i) + { +#ifndef NDEBUG + dtdbg << getList()[i] << std::endl; +#endif + compareVelocities(getList()[i]); + } +} + +//============================================================================== +TEST_F(DynamicsTest, compareAccelerations) +{ + for (int i = 0; i < getList().size(); ++i) + { +#ifndef NDEBUG + dtdbg << getList()[i] << std::endl; +#endif + compareAccelerations(getList()[i]); + } +} + +//============================================================================== +TEST_F(DynamicsTest, compareEquationsOfMotion) +{ + for (int i = 0; i < getList().size(); ++i) + { + //////////////////////////////////////////////////////////////////////////// + // TODO(JS): Following five skel files, which contain euler joints couldn't + // pass EQUATIONS_OF_MOTION, are disabled. + std::string skelFileName = getList()[i]; + if (skelFileName == DART_DATA_PATH"skel/test/chainwhipa.skel" + || skelFileName == DART_DATA_PATH"skel/test/serial_chain_eulerxyz_joint.skel" + || skelFileName == DART_DATA_PATH"skel/test/simple_tree_structure_euler_joint.skel" + || skelFileName == DART_DATA_PATH"skel/test/tree_structure_euler_joint.skel" + || skelFileName == DART_DATA_PATH"skel/fullbody1.skel") + { + continue; + } + //////////////////////////////////////////////////////////////////////////// + +#ifndef NDEBUG + dtdbg << getList()[i] << std::endl; +#endif + compareEquationsOfMotion(getList()[i]); + } +} //============================================================================== TEST_F(DynamicsTest, testCenterOfMass) diff --git a/unittests/testForwardKinematics.cpp b/unittests/testForwardKinematics.cpp index fa9d4256afb06..aa1c8cf4bdcca 100644 --- a/unittests/testForwardKinematics.cpp +++ b/unittests/testForwardKinematics.cpp @@ -68,7 +68,7 @@ TEST(FORWARD_KINEMATICS, YAW_ROLL) // position for (size_t i = 0; i < numTests; i++) { - robot->setConfigSegs(twoLinkIndices, joints[i]); + robot->setConfigSegs(twoLinkIndices, joints[i], true, false, false); Vector3d actual = robot->getBodyNode("ee")->getWorldTransform().translation(); bool equality = equals(actual, expectedPos[i], 1e-3); @@ -110,7 +110,7 @@ TEST(FORWARD_KINEMATICS, TWO_ROLLS) // position for (size_t i = 0; i < numTests; i++) { - robot->setConfigSegs(twoLinkIndices, joints[i]); + robot->setConfigSegs(twoLinkIndices, joints[i], true, false, false); Vector3d actual = robot->getBodyNode("ee")->getWorldTransform().translation(); bool equality = equals(actual, expectedPos[i], 1e-3); diff --git a/unittests/testInverseKinematics.cpp b/unittests/testInverseKinematics.cpp index 33dfd9c691f59..dea5a94e68c87 100644 --- a/unittests/testInverseKinematics.cpp +++ b/unittests/testInverseKinematics.cpp @@ -130,7 +130,7 @@ TEST(InverseKinematics, FittingTransformation) 0.0, TOLERANCE); // Set to initial configuration - robot->setConfigs(oldConfig); + robot->setConfigs(oldConfig, true, false, false); } //----------------------- Revolute joint test --------------------------------- @@ -143,14 +143,12 @@ TEST(InverseKinematics, FittingTransformation) VectorXd oldQ2 = joint2->getConfigs(); // Get desiredT2 by rotating the revolute joint with random angle - joint2->setConfigs(VectorXd::Random(1)); - robot->setConfigs(robot->getConfigs()); + joint2->setConfigs(VectorXd::Random(1), true, false, false); Isometry3d desiredT2 = body2->getWorldTransform(); // Transform body2 to the original transofrmation and check if it is done // well - joint2->setConfigs(oldQ2); - robot->setConfigs(robot->getConfigs()); + joint2->setConfigs(oldQ2, true, false, false); EXPECT_NEAR( math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(), 0.0, TOLERANCE); @@ -178,13 +176,12 @@ TEST(InverseKinematics, FittingTransformation) // Get desiredT2 by rotating the revolute joint with random angle out of // the joint limit range joint2->getGenCoord(0)->setConfig(math::random(DART_RADIAN * 15.5, DART_PI)); - robot->setConfigs(robot->getConfigs()); + robot->setConfigs(robot->getConfigs(), true, false, false); Isometry3d desiredT2 = body2->getWorldTransform(); // Transform body2 to the original transofrmation and check if it is done // well - joint2->setConfigs(oldQ2); - robot->setConfigs(robot->getConfigs()); + joint2->setConfigs(oldQ2, true, false, false); EXPECT_NEAR( math::logMap(oldT2.inverse() * body2->getWorldTransform()).norm(), 0.0, TOLERANCE); @@ -226,7 +223,7 @@ TEST(InverseKinematics, FittingVelocity) robot->init(); BodyNode* body1 = robot->getBodyNode(0); - BodyNode* body2 = robot->getBodyNode(1); +// BodyNode* body2 = robot->getBodyNode(1); Joint* joint1 = body1->getParentJoint(); // Joint* joint2 = body2->getParentJoint(); @@ -244,8 +241,8 @@ TEST(InverseKinematics, FittingVelocity) Vector3d diff = fittedLinVel - desiredVel; EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE); EXPECT_NEAR(fittedAngVel.dot(fittedAngVel), 0.0, TOLERANCE); - joint1->setGenVels(Vector6d::Zero()); - robot->setState(robot->getState()); + joint1->setGenVels(Vector6d::Zero(), true, true); + robot->setState(robot->getState(), true, true, false); // Test for angular velocity desiredVel = Vector3d::Random(); @@ -255,8 +252,8 @@ TEST(InverseKinematics, FittingVelocity) diff = fittedAngVel - desiredVel; EXPECT_NEAR(fittedLinVel.dot(fittedLinVel), 0.0, TOLERANCE); EXPECT_NEAR(diff.dot(diff), 0.0, TOLERANCE); - joint1->setGenVels(Vector6d::Zero()); - robot->setState(robot->getState()); + joint1->setGenVels(Vector6d::Zero(), true, true); + robot->setState(robot->getState(), true, true, false); } } diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index 6a664eff31129..e779132f06e12 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -103,7 +103,7 @@ void JOINTS::kinematicsTest(Joint* _joint) Eigen::VectorXd state = Eigen::VectorXd::Zero(2*dof); state.head(dof) = q; state.tail(dof) = dq; - skeleton.setState(state); + skeleton.setState(state, true, true, false); if (_joint->getNumGenCoords() == 0) return; @@ -126,15 +126,13 @@ void JOINTS::kinematicsTest(Joint* _joint) { // a Eigen::VectorXd q_a = q; - _joint->setConfigs(q_a); - skeleton.setConfigs(q_a); + _joint->setConfigs(q_a, true, false, false); Eigen::Isometry3d T_a = _joint->getLocalTransform(); // b Eigen::VectorXd q_b = q; q_b(i) += q_delta; - _joint->setConfigs(q_b); - skeleton.setConfigs(q_b); + _joint->setConfigs(q_b, true, false, false); Eigen::Isometry3d T_b = _joint->getLocalTransform(); // @@ -172,15 +170,13 @@ void JOINTS::kinematicsTest(Joint* _joint) { // a Eigen::VectorXd q_a = q; - _joint->setConfigs(q_a); - skeleton.setConfigs(q_a); + _joint->setConfigs(q_a, true, false, false); Jacobian J_a = _joint->getLocalJacobian(); // b Eigen::VectorXd q_b = q; q_b(i) += q_delta; - _joint->setConfigs(q_b); - skeleton.setConfigs(q_b); + _joint->setConfigs(q_b, true, false, false); Jacobian J_b = _joint->getLocalJacobian(); // @@ -204,7 +200,7 @@ void JOINTS::kinematicsTest(Joint* _joint) for (int i = 0; i < dof; ++i) q(i) = random(posMin, posMax); - skeleton.setConfigs(q); + skeleton.setConfigs(q, true, false, false); if (_joint->getNumGenCoords() == 0) return; diff --git a/unittests/testSoftDynamics.cpp b/unittests/testSoftDynamics.cpp index 54a9e4d063eaa..98d929d0c3076 100644 --- a/unittests/testSoftDynamics.cpp +++ b/unittests/testSoftDynamics.cpp @@ -356,7 +356,7 @@ void SoftDynamicsTest::compareEquationsOfMotion(const std::string& _fileName) VectorXd x = skel->getState(); for (int k = 0; k < x.size(); ++k) x[k] = random(lb, ub); - skel->setState(x); + skel->setState(x, true, true, true); ////////////////////////////////////////////////////////////////////////// /// TODO: Workaround code for Issue #122 From eef55ba086ede981a699c9b80ffdcdecd7e06a38 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 21 Mar 2014 06:44:26 -0400 Subject: [PATCH 07/11] Remove unsued variables --- dart/dynamics/Skeleton.h | 9 --------- 1 file changed, 9 deletions(-) diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index a8116f0cfa58e..3b47afcaa0f4a 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -346,15 +346,6 @@ class Skeleton : public GenCoordSystem /// \brief Total mass. double mTotalMass; - /// \brief True when configurations are modified - bool mIsConfigsModified; - - /// \brief True when generalized velocities are modified - bool mIsGenVelsModified; - - /// \brief True when generalized accelerations are modified - bool mIsGenAccsModified; - /// \brief Mass matrix for the skeleton. Eigen::MatrixXd mM; From 7ef78edb01b44f65a5e0dd1f637acef565f51129 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 21 Mar 2014 06:44:37 -0400 Subject: [PATCH 08/11] Update Migration.md --- Migration.md | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/Migration.md b/Migration.md index f6691421e940b..4d4bed9483a8c 100644 --- a/Migration.md +++ b/Migration.md @@ -13,11 +13,48 @@ + void fitWorldTransformAncestorJointsImpl(BodyNode* _body, const Eigen::Isometry3d& _target, bool _jointLimit = true) + void fitWorldTransformAllJointsImpl(BodyNode* _body, const Eigen::Isometry3d& _target, bool _jointLimit = true) +1. **dart/dynamics/Joint.h** + + virtual void setConfig(size_t _idx, double _config, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true) + + virtual void setConfigs(const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true) + + virtual void setGenVel(size_t _idx, double _genVel, bool _updateVels = true, bool _updateAccs = true) + + virtual void setGenVels(const Eigen::VectorXd& _genVels, bool _updateVels = true, bool _updateAccs = true) + + virtual void setGenAcc(size_t _idx, double _genAcc, bool _updateAccs = true) + + virtual void setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs = true) + +1. **dart/dynamics/Skeleton.h** + + virtual void setGenVels(const Eigen::VectorXd& _genVels, bool _updateVels = true, bool _updateAccs = true) + + virtual void setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs = true) + + void computeForwardKinematics(bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true) + ### Deletions + +1. **dart/dynamics/Skeleton.h** + + Eigen::VectorXd getConfig() const + ### Modifications +1. **dart/dynamics/Skeleton.h** + + + ***From:*** void setConfig(const std::vector& _id, const Eigen::VectorXd& _config) + + ***To:*** void setConfigSegs(const std::vector& _id, const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true) + + + ***From:*** Eigen::VectorXd getConfig(const std::vector& _id) const + + ***To:*** Eigen::VectorXd getConfigSegs(const std::vector& _id) const + + + ***From:*** void setConfig(const Eigen::VectorXd& _config) + + ***To:*** virtual void setConfigs(const Eigen::VectorXd& _configs, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true) + + + ***From:*** void setState(const Eigen::VectorXd& _state) + + ***To:*** void setState(const Eigen::VectorXd& _state, bool _updateTransforms = true, bool _updateVels = true, bool _updateAccs = true) + + + ***From:*** Eigen::VectorXd getState() + + ***To:*** Eigen::VectorXd getState() const + 1. **dart/dynamics/BodyNode.h** + + ***From:*** int getNumContactForces() const + ***To:*** int getNumContacts() const ### New Deprecations + + From 67fc6bf170433aee2edb174be8f4353e14268473 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Fri, 21 Mar 2014 06:47:25 -0400 Subject: [PATCH 09/11] Update Changelog.md --- Changelog.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/Changelog.md b/Changelog.md index 6473cbf8ce935..7c4aff674176c 100644 --- a/Changelog.md +++ b/Changelog.md @@ -16,6 +16,8 @@ * [Pull request #154](https://github.com/dartsim/dart/pull/154) 1. Added optional collision detector: BulletCollision. * [Pull request #156](https://github.com/dartsim/dart/pull/156) +1. Improved kinematics API for skeleton and joint + * [Pull request #161](https://github.com/dartsim/dart/pull/161) ### Version 3.0 (2013-11-04) From 59a240b95dfe52d92ead6e4ca0f3a890a41095c6 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 24 Mar 2014 14:19:51 -0400 Subject: [PATCH 10/11] Change default parameter value of setters for generalized coordinates --- dart/dynamics/Joint.h | 12 ++++++------ dart/dynamics/Skeleton.h | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 2d2c8d042a06e..745bf9d04fad7 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -139,8 +139,8 @@ class Joint : public GenCoordSystem /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setConfig(size_t _idx, double _config, bool _updateTransforms = true, - bool _updateVels = true, - bool _updateAccs = true); + bool _updateVels = false, + bool _updateAccs = false); /// \brief Set configurations in terms of generalized coordinates /// \param[in] _configs @@ -149,8 +149,8 @@ class Joint : public GenCoordSystem /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setConfigs(const Eigen::VectorXd& _configs, bool _updateTransforms = true, - bool _updateVels = true, - bool _updateAccs = true); + bool _updateVels = false, + bool _updateAccs = false); /// \brief Set single generalized velocity /// \param[in] _idx @@ -159,7 +159,7 @@ class Joint : public GenCoordSystem /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setGenVel(size_t _idx, double _genVel, bool _updateVels = true, - bool _updateAccs = true); + bool _updateAccs = false); /// \brief Set generalized velocities /// \param[in] _genVels @@ -167,7 +167,7 @@ class Joint : public GenCoordSystem /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setGenVels(const Eigen::VectorXd& _genVels, bool _updateVels = true, - bool _updateAccs = true); + bool _updateAccs = false); /// \brief Set single generalized acceleration /// \param[in] _idx diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 3b47afcaa0f4a..5b46c8dce37e6 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -152,15 +152,15 @@ class Skeleton : public GenCoordSystem /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setConfigs(const Eigen::VectorXd& _configs, bool _updateTransforms = true, - bool _updateVels = true, - bool _updateAccs = true); + bool _updateVels = false, + bool _updateAccs = false); /// \brief Set generalized velocities /// \param[in] _updateVels True to update spacial velocities of body nodes /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setGenVels(const Eigen::VectorXd& _genVels, bool _updateVels = true, - bool _updateAccs = true); + bool _updateAccs = false); /// \brief Set generalized accelerations /// \param[in] _updateAccs True to update spacial accelerations of body nodes @@ -177,8 +177,8 @@ class Skeleton : public GenCoordSystem void setConfigSegs(const std::vector& _id, const Eigen::VectorXd& _configs, bool _updateTransforms = true, - bool _updateVels = true, - bool _updateAccs = true); + bool _updateVels = false, + bool _updateAccs = false); /// \brief Get the configuration of this skeleton described in generalized /// coordinates. The returned order of configuration is determined by _id. @@ -192,7 +192,7 @@ class Skeleton : public GenCoordSystem void setState(const Eigen::VectorXd& _state, bool _updateTransforms = true, bool _updateVels = true, - bool _updateAccs = true); + bool _updateAccs = false); /// \brief Get the state of this skeleton described in generalized coordinates Eigen::VectorXd getState() const; From 2690703dffd76001b1ac7c1f873ec07c4f4b0e1d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 24 Mar 2014 14:27:24 -0400 Subject: [PATCH 11/11] Update comment --- dart/dynamics/Joint.h | 9 --------- dart/dynamics/Skeleton.h | 3 --- 2 files changed, 12 deletions(-) diff --git a/dart/dynamics/Joint.h b/dart/dynamics/Joint.h index 745bf9d04fad7..6667bb8d5fc69 100644 --- a/dart/dynamics/Joint.h +++ b/dart/dynamics/Joint.h @@ -132,8 +132,6 @@ class Joint : public GenCoordSystem //----------------- Interface for generalized coordinates -------------------- /// \brief Set single configuration in terms of generalized coordinates - /// \param[in] _idx - /// \param[in] _config /// \param[in] _updateTransforms True to update transformations of body nodes /// \param[in] _updateVels True to update spacial velocities of body nodes /// \param[in] _updateAccs True to update spacial accelerations of body nodes @@ -143,7 +141,6 @@ class Joint : public GenCoordSystem bool _updateAccs = false); /// \brief Set configurations in terms of generalized coordinates - /// \param[in] _configs /// \param[in] _updateTransforms True to update transformations of body nodes /// \param[in] _updateVels True to update spacial velocities of body nodes /// \param[in] _updateAccs True to update spacial accelerations of body nodes @@ -153,8 +150,6 @@ class Joint : public GenCoordSystem bool _updateAccs = false); /// \brief Set single generalized velocity - /// \param[in] _idx - /// \param[in] _genVel /// \param[in] _updateVels True to update spacial velocities of body nodes /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setGenVel(size_t _idx, double _genVel, @@ -162,7 +157,6 @@ class Joint : public GenCoordSystem bool _updateAccs = false); /// \brief Set generalized velocities - /// \param[in] _genVels /// \param[in] _updateVels True to update spacial velocities of body nodes /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setGenVels(const Eigen::VectorXd& _genVels, @@ -170,14 +164,11 @@ class Joint : public GenCoordSystem bool _updateAccs = false); /// \brief Set single generalized acceleration - /// \param[in] _idx - /// \param[in] _genAcc /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setGenAcc(size_t _idx, double _genAcc, bool _updateAccs = true); /// \brief Set generalized accelerations - /// \param[in] _genAccs /// \param[in] _updateAccs True to update spacial accelerations of body nodes virtual void setGenAccs(const Eigen::VectorXd& _genAccs, bool _updateAccs = true); diff --git a/dart/dynamics/Skeleton.h b/dart/dynamics/Skeleton.h index 5b46c8dce37e6..403798e751bd5 100644 --- a/dart/dynamics/Skeleton.h +++ b/dart/dynamics/Skeleton.h @@ -169,8 +169,6 @@ class Skeleton : public GenCoordSystem /// \brief Set the configuration of this skeleton described in generalized /// coordinates. The order of input configuration is determined by _id. - /// \param[in] _id - /// \param[in] _configs /// \param[in] _updateTransforms True to update transformations of body nodes /// \param[in] _updateVels True to update spacial velocities of body nodes /// \param[in] _updateAccs True to update spacial accelerations of body nodes @@ -185,7 +183,6 @@ class Skeleton : public GenCoordSystem Eigen::VectorXd getConfigSegs(const std::vector& _id) const; /// \brief Set the state of this skeleton described in generalized coordinates - /// \param[in] _state /// \param[in] _updateTransforms True to update transformations of body nodes /// \param[in] _updateVels True to update spacial velocities of body nodes /// \param[in] _updateAccs True to update spacial accelerations of body nodes