diff --git a/CHANGELOG.md b/CHANGELOG.md index 9c36cb77ebbdb..083e25dc514c1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,11 @@ ## DART 6 +### [DART 6.16.1 (2025-06-23)](https://github.com/dartsim/dart/milestone/77?closed=1) + +* Build + * Add a Kinematic Joint: [#1953] (https://github.com/dartsim/dart/pull/1953) + ### [DART 6.15.0 (2024-11-15)](https://github.com/dartsim/dart/milestone/77?closed=1) * Tested Platforms diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp new file mode 100644 index 0000000000000..352b5486b528e --- /dev/null +++ b/dart/dynamics/KinematicJoint.cpp @@ -0,0 +1,416 @@ +/* + * Copyright (c) 2011-2024, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/dynamics/KinematicJoint.hpp" + +#include "dart/dynamics/DegreeOfFreedom.hpp" +#include "dart/math/Geometry.hpp" +#include "dart/math/Helpers.hpp" + +#include + +namespace dart { +namespace dynamics { + +//============================================================================== +KinematicJoint::Properties::Properties(const Base::Properties& properties) + : Base::Properties(properties) +{ + // Do nothing +} + +//============================================================================== +KinematicJoint::~KinematicJoint() +{ + // Do nothing +} + +//============================================================================== +KinematicJoint::Properties KinematicJoint::getKinematicJointProperties() const +{ + return getGenericJointProperties(); +} + +//============================================================================== +Eigen::Vector6d KinematicJoint::convertToPositions(const Eigen::Isometry3d& _tf) +{ + Eigen::Vector6d x; + x.head<3>() = math::logMap(_tf.linear()); + x.tail<3>() = _tf.translation(); + return x; +} + +//============================================================================== +Eigen::Isometry3d KinematicJoint::convertToTransform( + const Eigen::Vector6d& _positions) +{ + Eigen::Isometry3d tf(Eigen::Isometry3d::Identity()); + tf.linear() = math::expMapRot(_positions.head<3>()); + tf.translation() = _positions.tail<3>(); + return tf; +} + +//============================================================================== +void KinematicJoint::setTransformOf( + Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) +{ + if (nullptr == joint) + return; + + KinematicJoint* kinematicJoint = dynamic_cast(joint); + + if (nullptr == kinematicJoint) { + dtwarn + << "[KinematicJoint::setTransform] Invalid joint type. Setting " + "transform " + << "is only allowed to KinematicJoint. The joint type of given joint [" + << joint->getName() << "] is [" << joint->getType() << "].\n"; + return; + } + + kinematicJoint->setTransform(tf, withRespectTo); +} + +//============================================================================== +void KinematicJoint::setTransformOf( + BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) +{ + if (nullptr == bodyNode) + return; + + setTransformOf(bodyNode->getParentJoint(), tf, withRespectTo); +} + +//============================================================================== +void KinematicJoint::setTransformOf( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo, + bool applyToAllRootBodies) +{ + if (!skeleton) + return; + + const auto numTrees = skeleton->getNumTrees(); + if (numTrees == 0) + return; + + if (applyToAllRootBodies) { + for (std::size_t i = 0; i < numTrees; ++i) + setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); + } else { + setTransformOf(skeleton->getRootBodyNode(), tf, withRespectTo); + } +} + +//============================================================================== +void KinematicJoint::setSpatialMotion( + const Eigen::Isometry3d* newTransform, + const Frame* withRespectTo, + const Eigen::Vector6d* newSpatialVelocity, + const Frame* velRelativeTo, + const Frame* velInCoordinatesOf) +{ + if (newTransform) + setTransform(*newTransform, withRespectTo); + + if (newSpatialVelocity) + setSpatialVelocity(*newSpatialVelocity, velRelativeTo, velInCoordinatesOf); +} + +//============================================================================== +void KinematicJoint::setRelativeTransform(const Eigen::Isometry3d& newTransform) +{ + setPositionsStatic(convertToPositions( + Joint::mAspectProperties.mT_ParentBodyToJoint.inverse() * newTransform + * Joint::mAspectProperties.mT_ChildBodyToJoint)); +} + +//============================================================================== +void KinematicJoint::setTransform( + const Eigen::Isometry3d& newTransform, const Frame* withRespectTo) +{ + assert(nullptr != withRespectTo); + + setRelativeTransform( + withRespectTo->getTransform(getChildBodyNode()->getParentFrame()) + * newTransform); +} + +//============================================================================== +void KinematicJoint::setRelativeSpatialVelocity( + const Eigen::Vector6d& newSpatialVelocity) +{ + setVelocitiesStatic(newSpatialVelocity); +} + +//============================================================================== +void KinematicJoint::setRelativeSpatialVelocity( + const Eigen::Vector6d& newSpatialVelocity, const Frame* inCoordinatesOf) +{ + assert(nullptr != inCoordinatesOf); + + if (getChildBodyNode() == inCoordinatesOf) { + setRelativeSpatialVelocity(newSpatialVelocity); + } else { + setRelativeSpatialVelocity( + math::AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), + newSpatialVelocity)); + } +} + +//============================================================================== +void KinematicJoint::setSpatialVelocity( + const Eigen::Vector6d& newSpatialVelocity, + const Frame* relativeTo, + const Frame* inCoordinatesOf) +{ + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + if (getChildBodyNode() == relativeTo) { + dtwarn << "[KinematicJoint::setSpatialVelocity] Invalid reference frame " + "for newSpatialVelocity. It shouldn't be the child BodyNode.\n"; + return; + } + + // Transform newSpatialVelocity into the child body node frame if needed + Eigen::Vector6d targetRelSpatialVel = newSpatialVelocity; + if (getChildBodyNode() != inCoordinatesOf) { + targetRelSpatialVel = math::AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); + } + + // Adjust for parent frame velocity if relativeTo is not the parent frame + if (getChildBodyNode()->getParentFrame() != relativeTo) { + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + + if (relativeTo->isWorld()) { + targetRelSpatialVel -= parentVelocity; + } else { + const Eigen::Vector6d relVelocity = math::AdT( + relativeTo->getTransform(getChildBodyNode()), + relativeTo->getSpatialVelocity()); + targetRelSpatialVel += relVelocity - parentVelocity; + } + } + + setRelativeSpatialVelocity(targetRelSpatialVel); +} + +//============================================================================== +void KinematicJoint::setLinearVelocity( + const Eigen::Vector3d& newLinearVelocity, const Frame* relativeTo) +{ + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + Eigen::Vector6d targetSpatialVelocity; + + if (Frame::World() == relativeTo) { + targetSpatialVelocity.head<3>() + = getChildBodyNode()->getSpatialVelocity().head<3>(); + } else { + targetSpatialVelocity.head<3>() + = getChildBodyNode() + ->getSpatialVelocity(relativeTo, getChildBodyNode()) + .head<3>(); + } + + targetSpatialVelocity.tail<3>() = newLinearVelocity; + setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); +} + +//============================================================================== +void KinematicJoint::setAngularVelocity( + const Eigen::Vector3d& newAngularVelocity, + const Frame* relativeTo, + const Frame* inCoordinatesOf) +{ + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + Eigen::Vector6d targetSpatialVelocity = Eigen::Vector6d::Zero(); + + // Gather the angular velocity in the child body node frame + targetSpatialVelocity.head<3>() + = getChildBodyNode()->getWorldTransform().linear().transpose() + * inCoordinatesOf->getWorldTransform().linear() * newAngularVelocity; + + // Translate Velocities if a non world coordinate frame is used + if (Frame::World() == relativeTo) { + targetSpatialVelocity.tail<3>() + = getChildBodyNode()->getSpatialVelocity().tail<3>(); + } else { + targetSpatialVelocity.tail<3>() + = getChildBodyNode() + ->getSpatialVelocity(relativeTo, getChildBodyNode()) + .tail<3>(); + } + + // Set the spatial velocity of the child body node + setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); +} + +//============================================================================== +Eigen::Matrix6d KinematicJoint::getRelativeJacobianStatic( + const Eigen::Vector6d& /*positions*/) const +{ + return mJacobian; +} + +//============================================================================== +Eigen::Vector6d KinematicJoint::getPositionDifferencesStatic( + const Eigen::Vector6d& _q2, const Eigen::Vector6d& _q1) const +{ + const Eigen::Isometry3d T1 = convertToTransform(_q1); + const Eigen::Isometry3d T2 = convertToTransform(_q2); + + return convertToPositions(T1.inverse() * T2); +} + +//============================================================================== +KinematicJoint::KinematicJoint(const Properties& properties) + : Base(properties), mQ(Eigen::Isometry3d::Identity()) +{ + mJacobianDeriv = Eigen::Matrix6d::Zero(); + + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createGenericJointAspect(properties); + createJointAspect(properties); +} + +//============================================================================== +Joint* KinematicJoint::clone() const +{ + return new KinematicJoint(getKinematicJointProperties()); +} + +//============================================================================== +const std::string& KinematicJoint::getType() const +{ + return getStaticType(); +} + +//============================================================================== +const std::string& KinematicJoint::getStaticType() +{ + static const std::string name = "KinematicJoint"; + return name; +} + +//============================================================================== +bool KinematicJoint::isCyclic(std::size_t _index) const +{ + return _index < 3 && !hasPositionLimit(0) && !hasPositionLimit(1) + && !hasPositionLimit(2); +} + +//============================================================================== +void KinematicJoint::integratePositions(double _dt) +{ + const Eigen::Isometry3d Qdiff + = convertToTransform(getVelocitiesStatic() * _dt); + const Eigen::Isometry3d Qnext = getQ() * Qdiff; + const Eigen::Isometry3d QdiffInv = Qdiff.inverse(); + + setVelocitiesStatic(math::AdR(QdiffInv, getVelocitiesStatic())); + setPositionsStatic(convertToPositions(Qnext)); +} + +//============================================================================== +void KinematicJoint::integrateVelocities(double /*_dt*/) +{ + setVelocitiesStatic(getVelocitiesStatic()); +} + +//============================================================================== +void KinematicJoint::updateConstrainedTerms(double /*timeStep*/) +{ + const Eigen::Vector6d& velBefore = getVelocitiesStatic(); + setVelocitiesStatic(velBefore); +} + +//==============================================ss================================ +void KinematicJoint::updateDegreeOfFreedomNames() +{ + static const char* suffixes[6] + = {"_rot_x", "_rot_y", "_rot_z", "_pos_x", "_pos_y", "_pos_z"}; + + for (std::size_t i = 0; i < 6; ++i) { + if (!mDofs[i]->isNamePreserved()) + mDofs[i]->setName(Joint::mAspectProperties.mName + suffixes[i], false); + } +} + +//============================================================================== +void KinematicJoint::updateRelativeTransform() const +{ + mQ = convertToTransform(getPositionsStatic()); + + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * mQ + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); + + assert(math::verifyTransform(mT)); +} + +//============================================================================== +void KinematicJoint::updateRelativeJacobian(bool _mandatory) const +{ + if (_mandatory) + mJacobian + = math::getAdTMatrix(Joint::mAspectProperties.mT_ChildBodyToJoint); +} + +//============================================================================== +void KinematicJoint::updateRelativeJacobianTimeDeriv() const +{ + assert(Eigen::Matrix6d::Zero() == mJacobianDeriv); +} + +//============================================================================== +const Eigen::Isometry3d& KinematicJoint::getQ() const +{ + if (mNeedTransformUpdate) { + updateRelativeTransform(); + mNeedTransformUpdate = false; + } + + return mQ; +} + +} // namespace dynamics +} // namespace dart diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp new file mode 100644 index 0000000000000..7d231081a3f53 --- /dev/null +++ b/dart/dynamics/KinematicJoint.hpp @@ -0,0 +1,271 @@ +/* + * Copyright (c) 2011-2024, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_DYNAMICS_KINEMATICJOINT_HPP_ +#define DART_DYNAMICS_KINEMATICJOINT_HPP_ + +#include + +#include + +#include + +#include + +namespace dart { +namespace dynamics { + +/// This class represents a joint that allows for kinematic control of the +/// child BodyNode's transform and spatial velocity. It does not enforce any +/// dynamics, meaning it does not compute forces or torques based on the +/// kinematic state. Instead, it provides methods to set the transform and +/// spatial velocity of the child BodyNode directly, allowing for precise +/// control over its position and orientation in space. + +class KinematicJoint : public GenericJoint +{ +public: + friend class Skeleton; + + using Base = GenericJoint; + + struct Properties : Base::Properties + { + DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(Properties) + + Properties(const Base::Properties& properties = Base::Properties()); + + virtual ~Properties() = default; + }; + + KinematicJoint(const KinematicJoint&) = delete; + + /// Destructor + virtual ~KinematicJoint(); + + /// Get the Properties of this KinematicJoint + Properties getKinematicJointProperties() const; + + // Documentation inherited + const std::string& getType() const override; + + /// Get joint type for this class + static const std::string& getStaticType(); + + // Documentation inherited + bool isCyclic(std::size_t _index) const override; + + /// Convert a transform into a 6D vector that can be used to set the positions + /// of a KinematicJoint. The positions returned by this function will result + /// in a relative transform of getTransformFromParentBodyNode() * _tf * + /// getTransformFromChildBodyNode().inverse() between the parent BodyNode and + /// the child BodyNode frames when applied to a KinematicJoint. + static Eigen::Vector6d convertToPositions(const Eigen::Isometry3d& _tf); + + /// Convert a KinematicJoint-style 6D vector into a transform + static Eigen::Isometry3d convertToTransform( + const Eigen::Vector6d& _positions); + + /// If the given joint is a KinematicJoint, then set the transform of the + /// given Joint's child BodyNode so that its transform with respect to + /// "withRespecTo" is equal to "tf". + static void setTransformOf( + Joint* joint, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World()); + + /// If the parent Joint of the given BodyNode is a KinematicJoint, then set + /// the transform of the given BodyNode so that its transform with respect to + /// "withRespecTo" is equal to "tf". + static void setTransformOf( + BodyNode* bodyNode, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World()); + + /// Apply setTransform(bodyNode, tf, withRespecTo) for all the root BodyNodes + /// of the given Skeleton. If false is passed in "applyToAllRootBodies", then + /// it will be applied to only the default root BodyNode that will be obtained + /// by Skeleton::getRootBodyNode(). + static void setTransformOf( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World(), + bool applyToAllRootBodies = true); + + /// Set the transform, spatial velocity, and spatial acceleration of the child + /// BodyNode relative to an arbitrary Frame. The reference frame can be + /// arbitrarily specified. + /// + /// If you want to set more than one kind of Cartetian coordinates (e.g., + /// transform and spatial velocity) at the same time, you should call + /// corresponding setters in a certain order (transform -> velocity -> + /// acceleration), If you don't velocity or acceleration can be corrupted by + /// transform or velocity. This function calls the corresponding setters in + /// the right order so that all the desired Cartetian coordinates are properly + /// set. + /// + /// Pass nullptr for "newTransform", "newSpatialVelocity", + /// if you don't want to set them. + /// + /// \param[in] newTransform Desired transform of the child BodyNode. + /// \param[in] withRespectTo The relative Frame of "newTransform". + /// \param[in] newSpatialVelocity Desired spatial velocity of the child + /// BodyNode. + /// \param[in] velRelativeTo The relative frame of "newSpatialVelocity". + /// \param[in] velInCoordinatesOf The reference frame of "newSpatialVelocity". + void setSpatialMotion( + const Eigen::Isometry3d* newTransform, + const Frame* withRespectTo, + const Eigen::Vector6d* newSpatialVelocity, + const Frame* velRelativeTo, + const Frame* velInCoordinatesOf); + + /// Set the transform of the child BodyNode relative to the parent BodyNode + /// \param[in] newTransform Desired transform of the child BodyNode. + void setRelativeTransform(const Eigen::Isometry3d& newTransform); + + /// Set the transform of the child BodyNode relative to an arbitrary Frame. + /// \param[in] newTransform Desired transform of the child BodyNode. + /// \param[in] withRespectTo The relative Frame of "newTransform". + void setTransform( + const Eigen::Isometry3d& newTransform, + const Frame* withRespectTo = Frame::World()); + + /// Set the spatial velocity of the child BodyNode relative to the parent + /// BodyNode. + /// \param[in] newSpatialVelocity Desired spatial velocity of the child + /// BodyNode. The reference frame of "newSpatialVelocity" is the child + /// BodyNode. + void setRelativeSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity); + + /// Set the spatial velocity of tfinhe child BodyNode relative to the parent + /// BodyNode. + /// \param[in] newSpatialVelocity Desired spatial velocity of the child + /// BodyNode. + /// \param[in] inCoordinatesOf The reference frame of "newSpatialVelocity". + void setRelativeSpatialVelocity( + const Eigen::Vector6d& newSpatialVelocity, const Frame* inCoordinatesOf); + + /// Set the spatial velocity of the child BodyNode relative to an arbitrary + /// Frame. + /// \param[in] newSpatialVelocity Desired spatial velocity of the child + /// BodyNode. + /// \param[in] relativeTo The relative frame of "newSpatialVelocity". + /// \param[in] inCoordinatesOf The reference frame of "newSpatialVelocity". + void setSpatialVelocity( + const Eigen::Vector6d& newSpatialVelocity, + const Frame* relativeTo, + const Frame* inCoordinatesOf); + + /// Set the linear portion of classical velocity of the child BodyNode + /// relative to an arbitrary Frame. + /// + /// Note that the angular portion of claasical velocity of the child + /// BodyNode will not be changed after this function called. + /// + /// \param[in] newLinearVelocity + /// \param[in] relativeTo The relative frame of "newLinearVelocity". + /// \param[in] inCoordinatesOf The reference frame of "newLinearVelocity". + void setLinearVelocity( + const Eigen::Vector3d& newLinearVelocity, + const Frame* relativeTo = Frame::World()); + + /// Set the angular portion of classical velocity of the child BodyNode + /// relative to an arbitrary Frame. + /// + /// Note that the linear portion of claasical velocity of the child + /// BodyNode will not be changed after this function called. + /// + /// \param[in] newAngularVelocity + /// \param[in] relativeTo The relative frame of "newAngularVelocity". + /// \param[in] inCoordinatesOf The reference frame of "newAngularVelocity". + void setAngularVelocity( + const Eigen::Vector3d& newAngularVelocity, + const Frame* relativeTo = Frame::World(), + const Frame* inCoordinatesOf = Frame::World()); + + // Documentation inherited + Eigen::Matrix6d getRelativeJacobianStatic( + const Eigen::Vector6d& _positions) const override; + + // Documentation inherited + Eigen::Vector6d getPositionDifferencesStatic( + const Eigen::Vector6d& _q2, const Eigen::Vector6d& _q1) const override; + +protected: + /// Constructor called by Skeleton class + KinematicJoint(const Properties& properties); + + // Documentation inherited + Joint* clone() const override; + + using Base::getRelativeJacobianStatic; + + // Documentation inherited + void integratePositions(double _dt) override; + + // Documentation inherited + void integrateVelocities(double _dt) override; + + // Documentation inherited + void updateConstrainedTerms(double timeStep) override; + + // Documentation inherited + void updateDegreeOfFreedomNames() override; + + // Documentation inherited + void updateRelativeTransform() const override; + + // Documentation inherited + void updateRelativeJacobian(bool _mandatory = true) const override; + + // Documentation inherited + void updateRelativeJacobianTimeDeriv() const override; + +protected: + /// Access mQ, which is an auto-updating variable + const Eigen::Isometry3d& getQ() const; + + /// Transformation matrix dependent on generalized coordinates + /// + /// Do not use directly! Use getQ() to access this + mutable Eigen::Isometry3d mQ; + +public: + // To get byte-aligned Eigen vectors + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_KinematicJoint_HPP_ diff --git a/package.xml b/package.xml index e3112c2b5ec81..c11fafe6380b1 100644 --- a/package.xml +++ b/package.xml @@ -4,7 +4,7 @@ a Catkin workspace. Catkin is not required to build DART. For more information, see: http://ros.org/reps/rep-0136.html --> dartsim - 6.16.0 + 6.16.1 DART (Dynamic Animation and Robotics Toolkit) is a collaborative, cross-platform, open source library created by the Georgia Tech Graphics