From 6209661b1f9ebd39bcc17afc9267f968bce2ec11 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 6 May 2025 16:45:56 +0000 Subject: [PATCH 01/24] first commit KinematicsJoint --- dart/dynamics/KinematicJoint.cpp | 677 +++++++++++++++++++++++++++++++ dart/dynamics/KinematicJoint.hpp | 370 +++++++++++++++++ 2 files changed, 1047 insertions(+) create mode 100644 dart/dynamics/KinematicJoint.cpp create mode 100644 dart/dynamics/KinematicJoint.hpp diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp new file mode 100644 index 0000000000000..9dab92338b4e7 --- /dev/null +++ b/dart/dynamics/KinematicJoint.cpp @@ -0,0 +1,677 @@ +/* + * 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::setTransform( + Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) + { + return setTransformOf(joint, tf, withRespectTo); + } + + //============================================================================== + 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::setTransform( + BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) + { + setTransformOf(bodyNode, 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::setTransform( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo, + bool applyToAllRootBodies) + { + setTransformOf(skeleton, tf, withRespectTo, applyToAllRootBodies); + } + + //============================================================================== + void KinematicJoint::setTransformOf( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo, + bool applyToAllRootBodies) + { + if (nullptr == skeleton) + return; + + const std::size_t numTrees = skeleton->getNumTrees(); + + if (0 == numTrees) + return; + + if (!applyToAllRootBodies) { + setTransformOf(skeleton->getRootBodyNode(), tf, withRespectTo); + return; + } + + for (std::size_t i = 0; i < numTrees; ++i) + setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); + } + + //============================================================================== + void KinematicJoint::setSpatialMotion( + const Eigen::Isometry3d* newTransform, + const Frame* withRespectTo, + const Eigen::Vector6d* newSpatialVelocity, + const Frame* velRelativeTo, + const Frame* velInCoordinatesOf, + const Eigen::Vector6d* newSpatialAcceleration, + const Frame* accRelativeTo, + const Frame* accInCoordinatesOf) + { + if (newTransform) + setTransform(*newTransform, withRespectTo); + + if (newSpatialVelocity) + setSpatialVelocity(*newSpatialVelocity, velRelativeTo, velInCoordinatesOf); + + if (newSpatialAcceleration) { + setSpatialAcceleration( + *newSpatialAcceleration, accRelativeTo, accInCoordinatesOf); + } + } + + //============================================================================== + 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) + { + dterr << "[KinematicJoint::setRelativeSpatialVelocity] This function is " + << "deprecated. Use setRelativeSpatialVelocity() instead.\n"; + 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; + } + + // Change the reference frame of "newSpatialVelocity" to the child body node + // frame. + Eigen::Vector6d targetRelSpatialVel = newSpatialVelocity; + if (getChildBodyNode() != inCoordinatesOf) { + targetRelSpatialVel = math::AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); + } + + // Compute the target relative spatial velocity from the parent body node to + // the child body node. + if (getChildBodyNode()->getParentFrame() != relativeTo) { + if (relativeTo->isWorld()) { + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + + targetRelSpatialVel -= parentVelocity; + } else { + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + const Eigen::Vector6d arbitraryVelocity = math::AdT( + relativeTo->getTransform(getChildBodyNode()), + relativeTo->getSpatialVelocity()); + + targetRelSpatialVel += -parentVelocity + arbitraryVelocity; + } + } + + setRelativeSpatialVelocity(targetRelSpatialVel); + } + + //============================================================================== + void KinematicJoint::setLinearVelocity( + const Eigen::Vector3d& newLinearVelocity, + const Frame* relativeTo, + const Frame* inCoordinatesOf) + { + dterr << "[KinematicJoint::setLinearVelocity] " << newLinearVelocity << "\n"; + 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>(); + } + + dterr <getWorldTransform().linear() << "\n"; + + targetSpatialVelocity.tail<3>() = newLinearVelocity; + //= getChildBodyNode()->getWorldTransform().linear().transpose() + // * inCoordinatesOf->getWorldTransform().linear() * newLinearVelocity; + // Above code is equivalent to: + // targetSpatialVelocity.tail<3>() + // = getChildBodyNode()->getTransform( + // inCoordinatesOf).linear().transpose() + // * newLinearVelocity; + // but faster. + + 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; + + targetSpatialVelocity.head<3>() + = getChildBodyNode()->getWorldTransform().linear().transpose() + * inCoordinatesOf->getWorldTransform().linear() * newAngularVelocity; + // Above code is equivalent to: + // targetSpatialVelocity.head<3>() + // = getChildBodyNode()->getTransform( + // inCoordinatesOf).linear().transpose() + // * newAngularVelocity; + // but faster. + + if (Frame::World() == relativeTo) { + targetSpatialVelocity.tail<3>() + = getChildBodyNode()->getSpatialVelocity().tail<3>(); + } else { + targetSpatialVelocity.tail<3>() + = getChildBodyNode() + ->getSpatialVelocity(relativeTo, getChildBodyNode()) + .tail<3>(); + } + + setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); + } + + //============================================================================== + void KinematicJoint::setRelativeSpatialAcceleration( + const Eigen::Vector6d& newSpatialAcceleration) + { + const Eigen::Matrix6d& J = getRelativeJacobianStatic(); + const Eigen::Matrix6d& dJ = getRelativeJacobianTimeDerivStatic(); + + setAccelerationsStatic( + J.inverse() * (newSpatialAcceleration - dJ * getVelocitiesStatic())); + } + + //============================================================================== + void KinematicJoint::setRelativeSpatialAcceleration( + const Eigen::Vector6d& newSpatialAcceleration, const Frame* inCoordinatesOf) + { + assert(nullptr != inCoordinatesOf); + + if (getChildBodyNode() == inCoordinatesOf) { + setRelativeSpatialAcceleration(newSpatialAcceleration); + } else { + setRelativeSpatialAcceleration(math::AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), + newSpatialAcceleration)); + } + } + + //============================================================================== + void KinematicJoint::setSpatialAcceleration( + const Eigen::Vector6d& newSpatialAcceleration, + const Frame* relativeTo, + const Frame* inCoordinatesOf) + { + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + if (getChildBodyNode() == relativeTo) { + dtwarn << "[KinematicJoint::setSpatialAcceleration] Invalid reference " + << "frame for newSpatialAcceleration. It shouldn't be the child " + << "BodyNode.\n"; + return; + } + + // Change the reference frame of "newSpatialAcceleration" to the child body + // node frame. + Eigen::Vector6d targetRelSpatialAcc = newSpatialAcceleration; + if (getChildBodyNode() != inCoordinatesOf) { + targetRelSpatialAcc = math::AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), + newSpatialAcceleration); + } + + // Compute the target relative spatial acceleration from the parent body node + // to the child body node. + if (getChildBodyNode()->getParentFrame() != relativeTo) { + if (relativeTo->isWorld()) { + const Eigen::Vector6d parentAcceleration + = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialAcceleration()) + + math::ad( + getChildBodyNode()->getSpatialVelocity(), + getRelativeJacobianStatic() * getVelocitiesStatic()); + + targetRelSpatialAcc -= parentAcceleration; + } else { + const Eigen::Vector6d parentAcceleration + = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialAcceleration()) + + math::ad( + getChildBodyNode()->getSpatialVelocity(), + getRelativeJacobianStatic() * getVelocitiesStatic()); + const Eigen::Vector6d arbitraryAcceleration + = math::AdT( + relativeTo->getTransform(getChildBodyNode()), + relativeTo->getSpatialAcceleration()) + - math::ad( + getChildBodyNode()->getSpatialVelocity(), + math::AdT( + relativeTo->getTransform(getChildBodyNode()), + relativeTo->getSpatialVelocity())); + + targetRelSpatialAcc += -parentAcceleration + arbitraryAcceleration; + } + } + + setRelativeSpatialAcceleration(targetRelSpatialAcc); + } + + //============================================================================== + void KinematicJoint::setLinearAcceleration( + const Eigen::Vector3d& newLinearAcceleration, + const Frame* relativeTo, + const Frame* inCoordinatesOf) + { + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + Eigen::Vector6d targetSpatialAcceleration; + + if (Frame::World() == relativeTo) { + targetSpatialAcceleration.head<3>() + = getChildBodyNode()->getSpatialAcceleration().head<3>(); + } else { + targetSpatialAcceleration.head<3>() + = getChildBodyNode() + ->getSpatialAcceleration(relativeTo, getChildBodyNode()) + .head<3>(); + } + + const Eigen::Vector6d& V + = getChildBodyNode()->getSpatialVelocity(relativeTo, inCoordinatesOf); + targetSpatialAcceleration.tail<3>() + = getChildBodyNode()->getWorldTransform().linear().transpose() + * inCoordinatesOf->getWorldTransform().linear() + * (newLinearAcceleration - V.head<3>().cross(V.tail<3>())); + // Above code is equivalent to: + // targetSpatialAcceleration.tail<3>() + // = getChildBodyNode()->getTransform( + // inCoordinatesOf).linear().transpose() + // * (newLinearAcceleration - V.head<3>().cross(V.tail<3>())); + // but faster. + + setSpatialAcceleration( + targetSpatialAcceleration, relativeTo, getChildBodyNode()); + } + + //============================================================================== + void KinematicJoint::setAngularAcceleration( + const Eigen::Vector3d& newAngularAcceleration, + const Frame* relativeTo, + const Frame* inCoordinatesOf) + { + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + Eigen::Vector6d targetSpatialAcceleration; + + targetSpatialAcceleration.head<3>() + = getChildBodyNode()->getWorldTransform().linear().transpose() + * inCoordinatesOf->getWorldTransform().linear() + * newAngularAcceleration; + // Above code is equivalent to: + // targetSpatialAcceleration.head<3>() + // = getChildBodyNode()->getTransform( + // inCoordinatesOf).linear().transpose() + // * newAngularAcceleration; + // but faster. + + if (Frame::World() == relativeTo) { + targetSpatialAcceleration.tail<3>() + = getChildBodyNode()->getSpatialAcceleration().tail<3>(); + } else { + targetSpatialAcceleration.tail<3>() + = getChildBodyNode() + ->getSpatialAcceleration(relativeTo, getChildBodyNode()) + .tail<3>(); + } + + setSpatialAcceleration( + targetSpatialAcceleration, 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())); + setAccelerationsStatic(math::AdR(QdiffInv, getAccelerationsStatic())); + setPositionsStatic(convertToPositions(Qnext)); + } + + //============================================================================== + void KinematicJoint::integrateVelocities(double _dt) + { + dterr << "[KinematicJoint::integrateVelocities] My function.\n"; + // Integrating the acceleration gives us the new velocity of child body frame. + // But if there is any linear acceleration, the frame will be displaced. If we + // apply euler integration direcly on the spatial acceleration, it will + // produce the velocity of a point that is instantaneously coincident with the + // previous location of the child body frame. However, we want to compute the + // spatial velocity at the current location of the child body frame. To + // accomplish this, we first convert the linear portion of the spatial + // acceleration into classical linear acceleration and apply the integration. + Eigen::Vector6d accel = getAccelerationsStatic(); + //const Eigen::Vector6d& velBefore = getVelocitiesStatic(); + //accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>()); + dtwarn << "accel: " << accel.transpose() << " dt " << _dt << "\n"; + setVelocitiesStatic(getVelocitiesStatic()); + + // Since the velocity has been updated, we use the new velocity to recompute + // the spatial acceleration. This is needed to ensure that functions like + // BodyNode::getLinearAcceleration work properly. + //const Eigen::Vector6d& velAfter = getVelocitiesStatic(); + accel.tail<3>() = Eigen::Vector3d::Zero();// velAfter.head<3>().cross(velAfter.tail<3>()); + setAccelerationsStatic(accel); + } + + //============================================================================== + void KinematicJoint::updateConstrainedTerms(double timeStep) + { + dtwarn << "[KinematicJoint::updateConstrainedTerms] My other function.\n"; + //const double invTimeStep = 1.0 / timeStep; + + //const Eigen::Vector6d& velBefore = getVelocitiesStatic(); + Eigen::Vector6d accel = getAccelerationsStatic(); + dtwarn << "accel: " << accel.transpose() << timeStep << "\n"; + dtwarn << "vel: " << getVelocitiesStatic().transpose() << "\n"; + dtwarn << "vel: " << mVelocityChanges << "\n"; + + //accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>()); + + setVelocitiesStatic(getVelocitiesStatic()); + + //const Eigen::Vector6d& velAfter = getVelocitiesStatic(); + //accel.tail<3>() -= velAfter.head<3>().cross(velAfter.tail<3>()); + setAccelerationsStatic(accel); + //this->mAspectState.mForces.noalias() += mImpulses * invTimeStep; + // Note: As long as this is only called from BodyNode::updateConstrainedTerms + } + + //============================================================================== + void KinematicJoint::updateDegreeOfFreedomNames() + { + if (!mDofs[0]->isNamePreserved()) + mDofs[0]->setName(Joint::mAspectProperties.mName + "_rot_x", false); + if (!mDofs[1]->isNamePreserved()) + mDofs[1]->setName(Joint::mAspectProperties.mName + "_rot_y", false); + if (!mDofs[2]->isNamePreserved()) + mDofs[2]->setName(Joint::mAspectProperties.mName + "_rot_z", false); + if (!mDofs[3]->isNamePreserved()) + mDofs[3]->setName(Joint::mAspectProperties.mName + "_pos_x", false); + if (!mDofs[4]->isNamePreserved()) + mDofs[4]->setName(Joint::mAspectProperties.mName + "_pos_y", false); + if (!mDofs[5]->isNamePreserved()) + mDofs[5]->setName(Joint::mAspectProperties.mName + "_pos_z", 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 + \ No newline at end of file diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp new file mode 100644 index 0000000000000..f79aa2c0dbbf0 --- /dev/null +++ b/dart/dynamics/KinematicJoint.hpp @@ -0,0 +1,370 @@ +/* + * 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 { + + /// class KinematicJoint + 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". + /// + /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead + DART_DEPRECATED(6.9) + static void setTransform( + Joint* joint, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World()); + + /// 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". + /// + /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead + DART_DEPRECATED(6.9) + static void setTransform( + BodyNode* bodyNode, + 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(). + /// + /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead + DART_DEPRECATED(6.9) + static void setTransform( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World(), + bool applyToAllRootBodies = true); + + /// 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", or + /// "newSpatialAcceleration" 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". + /// \param[in] newSpatialAcceleration Desired spatial acceleration of the + /// child BodyNode. + /// \param[in] accRelativeTo The relative frame of "newSpatialAcceleration". + /// \param[in] accInCoordinatesOf The reference frame of + /// "newSpatialAcceleration". + void setSpatialMotion( + const Eigen::Isometry3d* newTransform, + const Frame* withRespectTo, + const Eigen::Vector6d* newSpatialVelocity, + const Frame* velRelativeTo, + const Frame* velInCoordinatesOf, + const Eigen::Vector6d* newSpatialAcceleration, + const Frame* accRelativeTo, + const Frame* accInCoordinatesOf); + + /// 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 the 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(), + const Frame* inCoordinatesOf = 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()); + + /// Set the spatial acceleration of the child BodyNode relative to the parent + /// BodyNode. + /// \param[in] newSpatialAcceleration Desired spatial acceleration of the + /// child BodyNode. The reference frame of "newSpatialAcceleration" is the + /// child BodyNode. + void setRelativeSpatialAcceleration( + const Eigen::Vector6d& newSpatialAcceleration); + + /// Set the spatial acceleration of the child BodyNode relative to the parent + /// BodyNode. + /// \param[in] newSpatialAcceleration Desired spatial acceleration of the + /// child BodyNode. + /// \param[in] inCoordinatesOf The reference frame of + /// "newSpatialAcceleration". + void setRelativeSpatialAcceleration( + const Eigen::Vector6d& newSpatialAcceleration, + const Frame* inCoordinatesOf); + + /// Set the spatial acceleration of the child BodyNode relative to an + /// arbitrary Frame. + /// \param[in] newSpatialAcceleration Desired spatial acceleration of the + /// child BodyNode. + /// \param[in] relativeTo The relative frame of "newSpatialAcceleration". + /// \param[in] inCoordinatesOf The reference frame of + /// "newSpatialAcceleration". + void setSpatialAcceleration( + const Eigen::Vector6d& newSpatialAcceleration, + const Frame* relativeTo, + const Frame* inCoordinatesOf); + + /// Set the linear portion of classical acceleration of the child BodyNode + /// relative to an arbitrary Frame. + /// + /// Note that the angular portion of claasical acceleration of the child + /// BodyNode will not be changed after this function called. + /// + /// \param[in] newLinearAcceleration + /// \param[in] relativeTo The relative frame of "newLinearAcceleration". + /// \param[in] inCoordinatesOf The reference frame of "newLinearAcceleration". + void setLinearAcceleration( + const Eigen::Vector3d& newLinearAcceleration, + const Frame* relativeTo = Frame::World(), + const Frame* inCoordinatesOf = Frame::World()); + + /// Set the angular portion of classical acceleration of the child BodyNode + /// relative to an arbitrary Frame. + /// + /// Note that the linear portion of claasical acceleration of the child + /// BodyNode will not be changed after this function called. + /// + /// \param[in] newAngularAcceleration + /// \param[in] relativeTo The relative frame of "newAngularAcceleration". + /// \param[in] inCoordinatesOf The reference frame of + /// "newAngularAcceleration". + void setAngularAcceleration( + const Eigen::Vector3d& newAngularAcceleration, + 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_ + \ No newline at end of file From ea6464182a5dcfb8c6ecf3162aa069eb8fd5ae9e Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 9 May 2025 16:46:44 +0000 Subject: [PATCH 02/24] cleanup --- dart/dynamics/KinematicJoint.cpp | 226 +++---------------------------- dart/dynamics/KinematicJoint.hpp | 60 +------- 2 files changed, 25 insertions(+), 261 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 9dab92338b4e7..5488dd8bd9aee 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -162,10 +162,7 @@ const Frame* withRespectTo, const Eigen::Vector6d* newSpatialVelocity, const Frame* velRelativeTo, - const Frame* velInCoordinatesOf, - const Eigen::Vector6d* newSpatialAcceleration, - const Frame* accRelativeTo, - const Frame* accInCoordinatesOf) + const Frame* velInCoordinatesOf) { if (newTransform) setTransform(*newTransform, withRespectTo); @@ -173,10 +170,6 @@ if (newSpatialVelocity) setSpatialVelocity(*newSpatialVelocity, velRelativeTo, velInCoordinatesOf); - if (newSpatialAcceleration) { - setSpatialAcceleration( - *newSpatialAcceleration, accRelativeTo, accInCoordinatesOf); - } } //============================================================================== @@ -202,8 +195,6 @@ void KinematicJoint::setRelativeSpatialVelocity( const Eigen::Vector6d& newSpatialVelocity) { - dterr << "[KinematicJoint::setRelativeSpatialVelocity] This function is " - << "deprecated. Use setRelativeSpatialVelocity() instead.\n"; setVelocitiesStatic(newSpatialVelocity); } @@ -271,10 +262,8 @@ //============================================================================== void KinematicJoint::setLinearVelocity( const Eigen::Vector3d& newLinearVelocity, - const Frame* relativeTo, - const Frame* inCoordinatesOf) + const Frame* relativeTo) { - dterr << "[KinematicJoint::setLinearVelocity] " << newLinearVelocity << "\n"; assert(nullptr != relativeTo); assert(nullptr != inCoordinatesOf); @@ -289,19 +278,8 @@ ->getSpatialVelocity(relativeTo, getChildBodyNode()) .head<3>(); } - - dterr <getWorldTransform().linear() << "\n"; - - targetSpatialVelocity.tail<3>() = newLinearVelocity; - //= getChildBodyNode()->getWorldTransform().linear().transpose() - // * inCoordinatesOf->getWorldTransform().linear() * newLinearVelocity; - // Above code is equivalent to: - // targetSpatialVelocity.tail<3>() - // = getChildBodyNode()->getTransform( - // inCoordinatesOf).linear().transpose() - // * newLinearVelocity; - // but faster. + targetSpatialVelocity.tail<3>() = newLinearVelocity; setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); } @@ -315,17 +293,13 @@ assert(nullptr != inCoordinatesOf); Eigen::Vector6d targetSpatialVelocity; - + + // Hean Angular Velocities targetSpatialVelocity.head<3>() = getChildBodyNode()->getWorldTransform().linear().transpose() * inCoordinatesOf->getWorldTransform().linear() * newAngularVelocity; - // Above code is equivalent to: - // targetSpatialVelocity.head<3>() - // = getChildBodyNode()->getTransform( - // inCoordinatesOf).linear().transpose() - // * newAngularVelocity; - // but faster. - + + // Translate Velocities if a non world coordiinate frame is used if (Frame::World() == relativeTo) { targetSpatialVelocity.tail<3>() = getChildBodyNode()->getSpatialVelocity().tail<3>(); @@ -338,96 +312,7 @@ setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); } - - //============================================================================== - void KinematicJoint::setRelativeSpatialAcceleration( - const Eigen::Vector6d& newSpatialAcceleration) - { - const Eigen::Matrix6d& J = getRelativeJacobianStatic(); - const Eigen::Matrix6d& dJ = getRelativeJacobianTimeDerivStatic(); - - setAccelerationsStatic( - J.inverse() * (newSpatialAcceleration - dJ * getVelocitiesStatic())); - } - - //============================================================================== - void KinematicJoint::setRelativeSpatialAcceleration( - const Eigen::Vector6d& newSpatialAcceleration, const Frame* inCoordinatesOf) - { - assert(nullptr != inCoordinatesOf); - - if (getChildBodyNode() == inCoordinatesOf) { - setRelativeSpatialAcceleration(newSpatialAcceleration); - } else { - setRelativeSpatialAcceleration(math::AdR( - inCoordinatesOf->getTransform(getChildBodyNode()), - newSpatialAcceleration)); - } - } - - //============================================================================== - void KinematicJoint::setSpatialAcceleration( - const Eigen::Vector6d& newSpatialAcceleration, - const Frame* relativeTo, - const Frame* inCoordinatesOf) - { - assert(nullptr != relativeTo); - assert(nullptr != inCoordinatesOf); - - if (getChildBodyNode() == relativeTo) { - dtwarn << "[KinematicJoint::setSpatialAcceleration] Invalid reference " - << "frame for newSpatialAcceleration. It shouldn't be the child " - << "BodyNode.\n"; - return; - } - - // Change the reference frame of "newSpatialAcceleration" to the child body - // node frame. - Eigen::Vector6d targetRelSpatialAcc = newSpatialAcceleration; - if (getChildBodyNode() != inCoordinatesOf) { - targetRelSpatialAcc = math::AdR( - inCoordinatesOf->getTransform(getChildBodyNode()), - newSpatialAcceleration); - } - - // Compute the target relative spatial acceleration from the parent body node - // to the child body node. - if (getChildBodyNode()->getParentFrame() != relativeTo) { - if (relativeTo->isWorld()) { - const Eigen::Vector6d parentAcceleration - = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialAcceleration()) - + math::ad( - getChildBodyNode()->getSpatialVelocity(), - getRelativeJacobianStatic() * getVelocitiesStatic()); - - targetRelSpatialAcc -= parentAcceleration; - } else { - const Eigen::Vector6d parentAcceleration - = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialAcceleration()) - + math::ad( - getChildBodyNode()->getSpatialVelocity(), - getRelativeJacobianStatic() * getVelocitiesStatic()); - const Eigen::Vector6d arbitraryAcceleration - = math::AdT( - relativeTo->getTransform(getChildBodyNode()), - relativeTo->getSpatialAcceleration()) - - math::ad( - getChildBodyNode()->getSpatialVelocity(), - math::AdT( - relativeTo->getTransform(getChildBodyNode()), - relativeTo->getSpatialVelocity())); - - targetRelSpatialAcc += -parentAcceleration + arbitraryAcceleration; - } - } - - setRelativeSpatialAcceleration(targetRelSpatialAcc); - } - + //============================================================================== void KinematicJoint::setLinearAcceleration( const Eigen::Vector3d& newLinearAcceleration, @@ -462,46 +347,10 @@ // * (newLinearAcceleration - V.head<3>().cross(V.tail<3>())); // but faster. - setSpatialAcceleration( - targetSpatialAcceleration, relativeTo, getChildBodyNode()); - } - - //============================================================================== - void KinematicJoint::setAngularAcceleration( - const Eigen::Vector3d& newAngularAcceleration, - const Frame* relativeTo, - const Frame* inCoordinatesOf) - { - assert(nullptr != relativeTo); - assert(nullptr != inCoordinatesOf); - - Eigen::Vector6d targetSpatialAcceleration; - - targetSpatialAcceleration.head<3>() - = getChildBodyNode()->getWorldTransform().linear().transpose() - * inCoordinatesOf->getWorldTransform().linear() - * newAngularAcceleration; - // Above code is equivalent to: - // targetSpatialAcceleration.head<3>() - // = getChildBodyNode()->getTransform( - // inCoordinatesOf).linear().transpose() - // * newAngularAcceleration; - // but faster. - - if (Frame::World() == relativeTo) { - targetSpatialAcceleration.tail<3>() - = getChildBodyNode()->getSpatialAcceleration().tail<3>(); - } else { - targetSpatialAcceleration.tail<3>() - = getChildBodyNode() - ->getSpatialAcceleration(relativeTo, getChildBodyNode()) - .tail<3>(); - } - - setSpatialAcceleration( - targetSpatialAcceleration, relativeTo, getChildBodyNode()); + //setSpatialAcceleration( + // targetSpatialAcceleration, relativeTo, getChildBodyNode()); } - + //============================================================================== Eigen::Matrix6d KinematicJoint::getRelativeJacobianStatic( const Eigen::Vector6d& /*positions*/) const @@ -566,57 +415,24 @@ const Eigen::Isometry3d QdiffInv = Qdiff.inverse(); setVelocitiesStatic(math::AdR(QdiffInv, getVelocitiesStatic())); - setAccelerationsStatic(math::AdR(QdiffInv, getAccelerationsStatic())); + //setAccelerationsStatic(math::AdR(QdiffInv, getAccelerationsStatic())); setPositionsStatic(convertToPositions(Qnext)); } + //============================================================================== void KinematicJoint::integrateVelocities(double _dt) { - dterr << "[KinematicJoint::integrateVelocities] My function.\n"; - // Integrating the acceleration gives us the new velocity of child body frame. - // But if there is any linear acceleration, the frame will be displaced. If we - // apply euler integration direcly on the spatial acceleration, it will - // produce the velocity of a point that is instantaneously coincident with the - // previous location of the child body frame. However, we want to compute the - // spatial velocity at the current location of the child body frame. To - // accomplish this, we first convert the linear portion of the spatial - // acceleration into classical linear acceleration and apply the integration. + // For KinematicJoint, we don't need to integrate the velocity. We just + // need to set the velocity to the current velocity, ignoring the acceleration. + + // SKIP Velocity should be set directly + // TODO To be removed + dtdbg << "[KinematicJoint::integrateVelocities] This function is not " + << "using dt for integration which value is "<< _dt <<".\n"; Eigen::Vector6d accel = getAccelerationsStatic(); - //const Eigen::Vector6d& velBefore = getVelocitiesStatic(); - //accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>()); - dtwarn << "accel: " << accel.transpose() << " dt " << _dt << "\n"; - setVelocitiesStatic(getVelocitiesStatic()); - - // Since the velocity has been updated, we use the new velocity to recompute - // the spatial acceleration. This is needed to ensure that functions like - // BodyNode::getLinearAcceleration work properly. - //const Eigen::Vector6d& velAfter = getVelocitiesStatic(); - accel.tail<3>() = Eigen::Vector3d::Zero();// velAfter.head<3>().cross(velAfter.tail<3>()); - setAccelerationsStatic(accel); - } - - //============================================================================== - void KinematicJoint::updateConstrainedTerms(double timeStep) - { - dtwarn << "[KinematicJoint::updateConstrainedTerms] My other function.\n"; - //const double invTimeStep = 1.0 / timeStep; - - //const Eigen::Vector6d& velBefore = getVelocitiesStatic(); - Eigen::Vector6d accel = getAccelerationsStatic(); - dtwarn << "accel: " << accel.transpose() << timeStep << "\n"; - dtwarn << "vel: " << getVelocitiesStatic().transpose() << "\n"; - dtwarn << "vel: " << mVelocityChanges << "\n"; - - //accel.tail<3>() += velBefore.head<3>().cross(velBefore.tail<3>()); - - setVelocitiesStatic(getVelocitiesStatic()); - - //const Eigen::Vector6d& velAfter = getVelocitiesStatic(); - //accel.tail<3>() -= velAfter.head<3>().cross(velAfter.tail<3>()); + setVelocitiesStatic(getVelocitiesStatic()); setAccelerationsStatic(accel); - //this->mAspectState.mForces.noalias() += mImpulses * invTimeStep; - // Note: As long as this is only called from BodyNode::updateConstrainedTerms } //============================================================================== diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index f79aa2c0dbbf0..d3e210fa6e889 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -182,10 +182,7 @@ const Frame* withRespectTo, const Eigen::Vector6d* newSpatialVelocity, const Frame* velRelativeTo, - const Frame* velInCoordinatesOf, - const Eigen::Vector6d* newSpatialAcceleration, - const Frame* accRelativeTo, - const Frame* accInCoordinatesOf); + const Frame* velInCoordinatesOf); /// Set the transform of the child BodyNode relative to the parent BodyNode /// \param[in] newTransform Desired transform of the child BodyNode. @@ -205,7 +202,7 @@ /// BodyNode. void setRelativeSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity); - /// Set the spatial velocity of the child BodyNode relative to the parent + /// Set the spatial velocity of tfinhe child BodyNode relative to the parent /// BodyNode. /// \param[in] newSpatialVelocity Desired spatial velocity of the child /// BodyNode. @@ -235,8 +232,7 @@ /// \param[in] inCoordinatesOf The reference frame of "newLinearVelocity". void setLinearVelocity( const Eigen::Vector3d& newLinearVelocity, - const Frame* relativeTo = Frame::World(), - const Frame* inCoordinatesOf = Frame::World()); + const Frame* relativeTo = Frame::World()); /// Set the angular portion of classical velocity of the child BodyNode /// relative to an arbitrary Frame. @@ -251,37 +247,7 @@ const Eigen::Vector3d& newAngularVelocity, const Frame* relativeTo = Frame::World(), const Frame* inCoordinatesOf = Frame::World()); - - /// Set the spatial acceleration of the child BodyNode relative to the parent - /// BodyNode. - /// \param[in] newSpatialAcceleration Desired spatial acceleration of the - /// child BodyNode. The reference frame of "newSpatialAcceleration" is the - /// child BodyNode. - void setRelativeSpatialAcceleration( - const Eigen::Vector6d& newSpatialAcceleration); - - /// Set the spatial acceleration of the child BodyNode relative to the parent - /// BodyNode. - /// \param[in] newSpatialAcceleration Desired spatial acceleration of the - /// child BodyNode. - /// \param[in] inCoordinatesOf The reference frame of - /// "newSpatialAcceleration". - void setRelativeSpatialAcceleration( - const Eigen::Vector6d& newSpatialAcceleration, - const Frame* inCoordinatesOf); - - /// Set the spatial acceleration of the child BodyNode relative to an - /// arbitrary Frame. - /// \param[in] newSpatialAcceleration Desired spatial acceleration of the - /// child BodyNode. - /// \param[in] relativeTo The relative frame of "newSpatialAcceleration". - /// \param[in] inCoordinatesOf The reference frame of - /// "newSpatialAcceleration". - void setSpatialAcceleration( - const Eigen::Vector6d& newSpatialAcceleration, - const Frame* relativeTo, - const Frame* inCoordinatesOf); - + /// Set the linear portion of classical acceleration of the child BodyNode /// relative to an arbitrary Frame. /// @@ -296,21 +262,6 @@ const Frame* relativeTo = Frame::World(), const Frame* inCoordinatesOf = Frame::World()); - /// Set the angular portion of classical acceleration of the child BodyNode - /// relative to an arbitrary Frame. - /// - /// Note that the linear portion of claasical acceleration of the child - /// BodyNode will not be changed after this function called. - /// - /// \param[in] newAngularAcceleration - /// \param[in] relativeTo The relative frame of "newAngularAcceleration". - /// \param[in] inCoordinatesOf The reference frame of - /// "newAngularAcceleration". - void setAngularAcceleration( - const Eigen::Vector3d& newAngularAcceleration, - const Frame* relativeTo = Frame::World(), - const Frame* inCoordinatesOf = Frame::World()); - // Documentation inherited Eigen::Matrix6d getRelativeJacobianStatic( const Eigen::Vector6d& _positions) const override; @@ -334,9 +285,6 @@ // Documentation inherited void integrateVelocities(double _dt) override; - // Documentation inherited - void updateConstrainedTerms(double timeStep) override; - // Documentation inherited void updateDegreeOfFreedomNames() override; From 5c047ba65d208795f8658a203f6cc31a54ed086c Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 9 May 2025 16:52:40 +0000 Subject: [PATCH 03/24] cleangup not-needed functions --- dart/dynamics/KinematicJoint.cpp | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 5488dd8bd9aee..99c375d17a609 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -340,15 +340,6 @@ = getChildBodyNode()->getWorldTransform().linear().transpose() * inCoordinatesOf->getWorldTransform().linear() * (newLinearAcceleration - V.head<3>().cross(V.tail<3>())); - // Above code is equivalent to: - // targetSpatialAcceleration.tail<3>() - // = getChildBodyNode()->getTransform( - // inCoordinatesOf).linear().transpose() - // * (newLinearAcceleration - V.head<3>().cross(V.tail<3>())); - // but faster. - - //setSpatialAcceleration( - // targetSpatialAcceleration, relativeTo, getChildBodyNode()); } //============================================================================== @@ -415,7 +406,6 @@ const Eigen::Isometry3d QdiffInv = Qdiff.inverse(); setVelocitiesStatic(math::AdR(QdiffInv, getVelocitiesStatic())); - //setAccelerationsStatic(math::AdR(QdiffInv, getAccelerationsStatic())); setPositionsStatic(convertToPositions(Qnext)); } @@ -432,7 +422,6 @@ << "using dt for integration which value is "<< _dt <<".\n"; Eigen::Vector6d accel = getAccelerationsStatic(); setVelocitiesStatic(getVelocitiesStatic()); - setAccelerationsStatic(accel); } //============================================================================== From d1344aaf47067dacf3deea7873cfae3a2c205bd6 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 9 May 2025 17:06:59 +0000 Subject: [PATCH 04/24] removing unused param --- dart/dynamics/KinematicJoint.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 99c375d17a609..bd9f2f28e5570 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -420,7 +420,6 @@ // TODO To be removed dtdbg << "[KinematicJoint::integrateVelocities] This function is not " << "using dt for integration which value is "<< _dt <<".\n"; - Eigen::Vector6d accel = getAccelerationsStatic(); setVelocitiesStatic(getVelocitiesStatic()); } From ff53bc5ebd1988552f60536d906112546e9abb5e Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 12 May 2025 17:57:27 +0000 Subject: [PATCH 05/24] suppress warning --- dart/dynamics/KinematicJoint.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index bd9f2f28e5570..7212fe7708813 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -413,17 +413,18 @@ //============================================================================== void KinematicJoint::integrateVelocities(double _dt) { - // For KinematicJoint, we don't need to integrate the velocity. We just + (void)_dt; // To avoid unused variable warning + // For KinematicJoint, we don't need to integrate the velocity. We just // need to set the velocity to the current velocity, ignoring the acceleration. // SKIP Velocity should be set directly // TODO To be removed - dtdbg << "[KinematicJoint::integrateVelocities] This function is not " - << "using dt for integration which value is "<< _dt <<".\n"; - setVelocitiesStatic(getVelocitiesStatic()); + // dtmsg << "[KinematicJoint::integrateVelocities] This function is not " + // << "using dt for integration which value is "<< _dt <<".\n"; + setVelocitiesStatic(getVelocitiesStatic()); } - //============================================================================== + //==============================================ss================================ void KinematicJoint::updateDegreeOfFreedomNames() { if (!mDofs[0]->isNamePreserved()) From e18720ee12398e1190293268819e191a50076c6d Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 20 Jun 2025 17:15:06 +0000 Subject: [PATCH 06/24] remove deprecated functions --- dart/dynamics/KinematicJoint.cpp | 26 +----------------------- dart/dynamics/KinematicJoint.hpp | 35 +------------------------------- 2 files changed, 2 insertions(+), 59 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 7212fe7708813..f32bf71b7d5d8 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -79,13 +79,6 @@ return tf; } - //============================================================================== - void KinematicJoint::setTransform( - Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) - { - return setTransformOf(joint, tf, withRespectTo); - } - //============================================================================== void KinematicJoint::setTransformOf( Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) @@ -104,14 +97,7 @@ kinematicJoint->setTransform(tf, withRespectTo); } - - //============================================================================== - void KinematicJoint::setTransform( - BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) - { - setTransformOf(bodyNode, tf, withRespectTo); - } - + //============================================================================== void KinematicJoint::setTransformOf( BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) @@ -122,16 +108,6 @@ setTransformOf(bodyNode->getParentJoint(), tf, withRespectTo); } - //============================================================================== - void KinematicJoint::setTransform( - Skeleton* skeleton, - const Eigen::Isometry3d& tf, - const Frame* withRespectTo, - bool applyToAllRootBodies) - { - setTransformOf(skeleton, tf, withRespectTo, applyToAllRootBodies); - } - //============================================================================== void KinematicJoint::setTransformOf( Skeleton* skeleton, diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index d3e210fa6e889..495d75b1fc79a 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -89,18 +89,7 @@ /// 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". - /// - /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead - DART_DEPRECATED(6.9) - static void setTransform( - Joint* joint, - const Eigen::Isometry3d& tf, - const Frame* withRespectTo = Frame::World()); - + /// 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". @@ -109,16 +98,6 @@ 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". - /// - /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead - DART_DEPRECATED(6.9) - static void setTransform( - BodyNode* bodyNode, - 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 @@ -128,18 +107,6 @@ 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(). - /// - /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead - DART_DEPRECATED(6.9) - static void setTransform( - Skeleton* skeleton, - const Eigen::Isometry3d& tf, - const Frame* withRespectTo = Frame::World(), - bool applyToAllRootBodies = true); /// Apply setTransform(bodyNode, tf, withRespecTo) for all the root BodyNodes /// of the given Skeleton. If false is passed in "applyToAllRootBodies", then From 510053975830a301ff616065e38bb44b858eaffa Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 20 Jun 2025 22:19:02 +0000 Subject: [PATCH 07/24] remove unchanged functions --- dart/dynamics/KinematicJoint.cpp | 20 ++------------------ dart/dynamics/KinematicJoint.hpp | 3 --- 2 files changed, 2 insertions(+), 21 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index f32bf71b7d5d8..5c6e0bde7d8f3 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -362,8 +362,7 @@ //============================================================================== const std::string& KinematicJoint::getStaticType() { - static const std::string name = "KinematicJoint"; - return name; + return "KinematicJoint"; } //============================================================================== @@ -384,22 +383,7 @@ setVelocitiesStatic(math::AdR(QdiffInv, getVelocitiesStatic())); setPositionsStatic(convertToPositions(Qnext)); } - - - //============================================================================== - void KinematicJoint::integrateVelocities(double _dt) - { - (void)_dt; // To avoid unused variable warning - // For KinematicJoint, we don't need to integrate the velocity. We just - // need to set the velocity to the current velocity, ignoring the acceleration. - - // SKIP Velocity should be set directly - // TODO To be removed - // dtmsg << "[KinematicJoint::integrateVelocities] This function is not " - // << "using dt for integration which value is "<< _dt <<".\n"; - setVelocitiesStatic(getVelocitiesStatic()); - } - + //==============================================ss================================ void KinematicJoint::updateDegreeOfFreedomNames() { diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 495d75b1fc79a..2d73df2eb4658 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -249,9 +249,6 @@ // Documentation inherited void integratePositions(double _dt) override; - // Documentation inherited - void integrateVelocities(double _dt) override; - // Documentation inherited void updateDegreeOfFreedomNames() override; From bb064f581992b3e3184dadd65efad01f91f7bf48 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 23 Jun 2025 15:11:19 +0000 Subject: [PATCH 08/24] clean code --- dart/dynamics/KinematicJoint.cpp | 40 +++----------------------------- dart/dynamics/KinematicJoint.hpp | 20 ---------------- 2 files changed, 3 insertions(+), 57 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 5c6e0bde7d8f3..368f9976dc33d 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -288,35 +288,6 @@ setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); } - - //============================================================================== - void KinematicJoint::setLinearAcceleration( - const Eigen::Vector3d& newLinearAcceleration, - const Frame* relativeTo, - const Frame* inCoordinatesOf) - { - assert(nullptr != relativeTo); - assert(nullptr != inCoordinatesOf); - - Eigen::Vector6d targetSpatialAcceleration; - - if (Frame::World() == relativeTo) { - targetSpatialAcceleration.head<3>() - = getChildBodyNode()->getSpatialAcceleration().head<3>(); - } else { - targetSpatialAcceleration.head<3>() - = getChildBodyNode() - ->getSpatialAcceleration(relativeTo, getChildBodyNode()) - .head<3>(); - } - - const Eigen::Vector6d& V - = getChildBodyNode()->getSpatialVelocity(relativeTo, inCoordinatesOf); - targetSpatialAcceleration.tail<3>() - = getChildBodyNode()->getWorldTransform().linear().transpose() - * inCoordinatesOf->getWorldTransform().linear() - * (newLinearAcceleration - V.head<3>().cross(V.tail<3>())); - } //============================================================================== Eigen::Matrix6d KinematicJoint::getRelativeJacobianStatic( @@ -329,10 +300,7 @@ 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); + return convertToPositions(convertToTransform(_q1).inverse() * convertToTransform(_q2)); } //============================================================================== @@ -340,9 +308,6 @@ : 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); } @@ -362,7 +327,8 @@ //============================================================================== const std::string& KinematicJoint::getStaticType() { - return "KinematicJoint"; + static const std::string name = "KinematicJoint"; + return name; } //============================================================================== diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 2d73df2eb4658..066ea2e27a3a1 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -35,8 +35,6 @@ #include - #include - #include #include @@ -139,11 +137,7 @@ /// BodyNode. /// \param[in] velRelativeTo The relative frame of "newSpatialVelocity". /// \param[in] velInCoordinatesOf The reference frame of "newSpatialVelocity". - /// \param[in] newSpatialAcceleration Desired spatial acceleration of the /// child BodyNode. - /// \param[in] accRelativeTo The relative frame of "newSpatialAcceleration". - /// \param[in] accInCoordinatesOf The reference frame of - /// "newSpatialAcceleration". void setSpatialMotion( const Eigen::Isometry3d* newTransform, const Frame* withRespectTo, @@ -214,20 +208,6 @@ const Eigen::Vector3d& newAngularVelocity, const Frame* relativeTo = Frame::World(), const Frame* inCoordinatesOf = Frame::World()); - - /// Set the linear portion of classical acceleration of the child BodyNode - /// relative to an arbitrary Frame. - /// - /// Note that the angular portion of claasical acceleration of the child - /// BodyNode will not be changed after this function called. - /// - /// \param[in] newLinearAcceleration - /// \param[in] relativeTo The relative frame of "newLinearAcceleration". - /// \param[in] inCoordinatesOf The reference frame of "newLinearAcceleration". - void setLinearAcceleration( - const Eigen::Vector3d& newLinearAcceleration, - const Frame* relativeTo = Frame::World(), - const Frame* inCoordinatesOf = Frame::World()); // Documentation inherited Eigen::Matrix6d getRelativeJacobianStatic( From 676d7fd3816d0cdb5a4a081d72d52cc2033cf6d5 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 23 Jun 2025 15:13:33 +0000 Subject: [PATCH 09/24] add explanation --- dart/dynamics/KinematicJoint.hpp | 27 +++++++++------------------ 1 file changed, 9 insertions(+), 18 deletions(-) diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 066ea2e27a3a1..d44a8f1caa51d 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -42,7 +42,13 @@ namespace dart { namespace dynamics { - /// class KinematicJoint +/// 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: @@ -77,11 +83,7 @@ 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. + /// of a KinematicJoint. static Eigen::Vector6d convertToPositions(const Eigen::Isometry3d& _tf); /// Convert a KinematicJoint-style 6D vector into a transform @@ -89,7 +91,7 @@ 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 + /// Joint's child BodyNode so it transforms with respect to /// "withRespecTo" is equal to "tf". static void setTransformOf( Joint* joint, @@ -120,17 +122,6 @@ /// 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", or - /// "newSpatialAcceleration" 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 From 8e135d5c11269317c79e499b6645a06432c20be5 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 23 Jun 2025 16:39:49 +0000 Subject: [PATCH 10/24] format --- dart/dynamics/KinematicJoint.cpp | 749 +++++++++++++++---------------- dart/dynamics/KinematicJoint.hpp | 429 +++++++++--------- 2 files changed, 586 insertions(+), 592 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 368f9976dc33d..59a839f7e6032 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -30,379 +30,376 @@ * 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 (nullptr == skeleton) - return; - - const std::size_t numTrees = skeleton->getNumTrees(); - - if (0 == numTrees) - return; - - if (!applyToAllRootBodies) { - setTransformOf(skeleton->getRootBodyNode(), tf, withRespectTo); - return; - } - - for (std::size_t i = 0; i < numTrees; ++i) - setTransformOf(skeleton->getRootBodyNode(i), 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; - } - - // Change the reference frame of "newSpatialVelocity" to the child body node - // frame. - Eigen::Vector6d targetRelSpatialVel = newSpatialVelocity; - if (getChildBodyNode() != inCoordinatesOf) { - targetRelSpatialVel = math::AdR( - inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); - } - - // Compute the target relative spatial velocity from the parent body node to - // the child body node. - if (getChildBodyNode()->getParentFrame() != relativeTo) { - if (relativeTo->isWorld()) { - const Eigen::Vector6d parentVelocity = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialVelocity()); - - targetRelSpatialVel -= parentVelocity; - } else { - const Eigen::Vector6d parentVelocity = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialVelocity()); - const Eigen::Vector6d arbitraryVelocity = math::AdT( - relativeTo->getTransform(getChildBodyNode()), - relativeTo->getSpatialVelocity()); - - targetRelSpatialVel += -parentVelocity + arbitraryVelocity; - } - } - - 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; - - // Hean Angular Velocities - targetSpatialVelocity.head<3>() - = getChildBodyNode()->getWorldTransform().linear().transpose() - * inCoordinatesOf->getWorldTransform().linear() * newAngularVelocity; - - // Translate Velocities if a non world coordiinate frame is used - if (Frame::World() == relativeTo) { - targetSpatialVelocity.tail<3>() - = getChildBodyNode()->getSpatialVelocity().tail<3>(); - } else { - targetSpatialVelocity.tail<3>() - = getChildBodyNode() - ->getSpatialVelocity(relativeTo, getChildBodyNode()) - .tail<3>(); - } - - 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 - { - return convertToPositions(convertToTransform(_q1).inverse() * convertToTransform(_q2)); - } - - //============================================================================== - KinematicJoint::KinematicJoint(const Properties& properties) - : Base(properties), mQ(Eigen::Isometry3d::Identity()) - { - mJacobianDeriv = Eigen::Matrix6d::Zero(); - 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)); - } - - //==============================================ss================================ - void KinematicJoint::updateDegreeOfFreedomNames() - { - if (!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(Joint::mAspectProperties.mName + "_rot_x", false); - if (!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(Joint::mAspectProperties.mName + "_rot_y", false); - if (!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(Joint::mAspectProperties.mName + "_rot_z", false); - if (!mDofs[3]->isNamePreserved()) - mDofs[3]->setName(Joint::mAspectProperties.mName + "_pos_x", false); - if (!mDofs[4]->isNamePreserved()) - mDofs[4]->setName(Joint::mAspectProperties.mName + "_pos_y", false); - if (!mDofs[5]->isNamePreserved()) - mDofs[5]->setName(Joint::mAspectProperties.mName + "_pos_z", 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 - \ No newline at end of file +#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 (nullptr == skeleton) + return; + + const std::size_t numTrees = skeleton->getNumTrees(); + + if (0 == numTrees) + return; + + if (!applyToAllRootBodies) { + setTransformOf(skeleton->getRootBodyNode(), tf, withRespectTo); + return; + } + + for (std::size_t i = 0; i < numTrees; ++i) + setTransformOf(skeleton->getRootBodyNode(i), 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; + } + + // Change the reference frame of "newSpatialVelocity" to the child body node + // frame. + Eigen::Vector6d targetRelSpatialVel = newSpatialVelocity; + if (getChildBodyNode() != inCoordinatesOf) { + targetRelSpatialVel = math::AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); + } + + // Compute the target relative spatial velocity from the parent body node to + // the child body node. + if (getChildBodyNode()->getParentFrame() != relativeTo) { + if (relativeTo->isWorld()) { + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + + targetRelSpatialVel -= parentVelocity; + } else { + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + const Eigen::Vector6d arbitraryVelocity = math::AdT( + relativeTo->getTransform(getChildBodyNode()), + relativeTo->getSpatialVelocity()); + + targetRelSpatialVel += -parentVelocity + arbitraryVelocity; + } + } + + 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; + + // Translate Velocities if a non world coordiinate frame is used + if (Frame::World() == relativeTo) { + targetSpatialVelocity.tail<3>() + = getChildBodyNode()->getSpatialVelocity().tail<3>(); + } else { + targetSpatialVelocity.tail<3>() + = getChildBodyNode() + ->getSpatialVelocity(relativeTo, getChildBodyNode()) + .tail<3>(); + } + + // Hean Angular Velocities + targetSpatialVelocity.head<3>() + = getChildBodyNode()->getWorldTransform().linear().transpose() + * inCoordinatesOf->getWorldTransform().linear() * newAngularVelocity; + + 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 +{ + return convertToPositions( + convertToTransform(_q1).inverse() * convertToTransform(_q2)); +} + +//============================================================================== +KinematicJoint::KinematicJoint(const Properties& properties) + : Base(properties), mQ(Eigen::Isometry3d::Identity()) +{ + mJacobianDeriv = Eigen::Matrix6d::Zero(); + 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)); +} + +//==============================================ss================================ +void KinematicJoint::updateDegreeOfFreedomNames() +{ + static const char* dofNames[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 + dofNames[i], false); + } + } +} + +//============================================================================== +void KinematicJoint::updateRelativeTransform() const +{ + mT = Joint::mAspectProperties.mT_ParentBodyToJoint + * convertToTransform(getPositionsStatic()) + * 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 index d44a8f1caa51d..6d9e5ec7f0566 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -30,224 +30,221 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef DART_DYNAMICS_KINEMATICJOINT_HPP_ - #define DART_DYNAMICS_KINEMATICJOINT_HPP_ - - #include - - #include - - #include - - namespace dart { - namespace dynamics { - +#ifndef DART_DYNAMICS_KINEMATICJOINT_HPP_ +#define DART_DYNAMICS_KINEMATICJOINT_HPP_ + +#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 +/// 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 +/// 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. - 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 it transforms 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. - /// - /// \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". - /// child BodyNode. - 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 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_ - \ No newline at end of file +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. + 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 it transforms 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. + /// + /// \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". + /// child BodyNode. + 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 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_ From 4336d4ad1915ce991931a8efaf46ac29fbe1054c Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 23 Jun 2025 16:40:46 +0000 Subject: [PATCH 11/24] format --- dart/dynamics/KinematicJoint.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 6d9e5ec7f0566..13aee6d1e8d30 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -33,7 +33,7 @@ #ifndef DART_DYNAMICS_KINEMATICJOINT_HPP_ #define DART_DYNAMICS_KINEMATICJOINT_HPP_ -#include +#include #include From 6c15a6e0c78eafc990135a0ea80802ea7a42a3f2 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 23 Jun 2025 16:52:37 +0000 Subject: [PATCH 12/24] setVelocity --- dart/dynamics/KinematicJoint.cpp | 28 ++++++++++------------------ 1 file changed, 10 insertions(+), 18 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 59a839f7e6032..48697406be948 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -206,31 +206,23 @@ void KinematicJoint::setSpatialVelocity( return; } - // Change the reference frame of "newSpatialVelocity" to the child body node - // frame. - Eigen::Vector6d targetRelSpatialVel = newSpatialVelocity; - if (getChildBodyNode() != inCoordinatesOf) { - targetRelSpatialVel = math::AdR( - inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); - } + // Transform newSpatialVelocity into the child body node frame if needed + Eigen::Vector6d targetRelSpatialVel = (getChildBodyNode() == inCoordinatesOf) + ? newSpatialVelocity + : math::AdR(inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); - // Compute the target relative spatial velocity from the parent body node to - // the child body node. + // Adjust for parent frame if necessary if (getChildBodyNode()->getParentFrame() != relativeTo) { - if (relativeTo->isWorld()) { - const Eigen::Vector6d parentVelocity = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + if (relativeTo->isWorld()) { targetRelSpatialVel -= parentVelocity; } else { - const Eigen::Vector6d parentVelocity = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialVelocity()); const Eigen::Vector6d arbitraryVelocity = math::AdT( relativeTo->getTransform(getChildBodyNode()), relativeTo->getSpatialVelocity()); - targetRelSpatialVel += -parentVelocity + arbitraryVelocity; } } @@ -242,7 +234,7 @@ void KinematicJoint::setSpatialVelocity( void KinematicJoint::setLinearVelocity( const Eigen::Vector3d& newLinearVelocity, const Frame* relativeTo) { - assert(nullptr != relativeTo); + assert(nullptr != relativeTo); assert(nullptr != inCoordinatesOf); Eigen::Vector6d targetSpatialVelocity; From fa9eeb821647aba6ce6fd9dc71532af6b3774fe0 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 23 Jun 2025 19:11:18 +0000 Subject: [PATCH 13/24] rewrite setTransformOf --- dart/dynamics/KinematicJoint.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 48697406be948..f585dd2ebb53f 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -117,21 +117,19 @@ void KinematicJoint::setTransformOf( const Frame* withRespectTo, bool applyToAllRootBodies) { - if (nullptr == skeleton) + if (!skeleton) return; - const std::size_t numTrees = skeleton->getNumTrees(); - - if (0 == numTrees) + if (skeleton->getNumTrees() == 0) return; - if (!applyToAllRootBodies) { + if (applyToAllRootBodies) { + for (std::size_t i = 0; i < skeleton->getNumTrees(); ++i) { + setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); + } + } else { setTransformOf(skeleton->getRootBodyNode(), tf, withRespectTo); - return; } - - for (std::size_t i = 0; i < numTrees; ++i) - setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); } //============================================================================== From cec394c0560c3a5f5fd389e4ffad8b667616a146 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 23 Jun 2025 20:09:22 +0000 Subject: [PATCH 14/24] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9c36cb77ebbdb..32e27e30f7934 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,6 +2,11 @@ ## DART 6 +### [DART 6.15.0 (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 From 8e3adb962089ca8e0f0ba2c4756cc82de80dd54f Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 18:34:22 +0000 Subject: [PATCH 15/24] revert some removed functions --- dart/dynamics/KinematicJoint.cpp | 156 ++++++++++++++++++++++++------- dart/dynamics/KinematicJoint.hpp | 76 ++++++++++++++- 2 files changed, 196 insertions(+), 36 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index f585dd2ebb53f..3c6022272cb7b 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -79,6 +79,13 @@ Eigen::Isometry3d KinematicJoint::convertToTransform( return tf; } +//============================================================================== +void KinematicJoint::setTransform( + Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) +{ + return setTransformOf(joint, tf, withRespectTo); +} + //============================================================================== void KinematicJoint::setTransformOf( Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) @@ -100,6 +107,13 @@ void KinematicJoint::setTransformOf( kinematicJoint->setTransform(tf, withRespectTo); } +//============================================================================== +void KinematicJoint::setTransform( + BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) +{ + setTransformOf(bodyNode, tf, withRespectTo); +} + //============================================================================== void KinematicJoint::setTransformOf( BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) @@ -110,6 +124,16 @@ void KinematicJoint::setTransformOf( setTransformOf(bodyNode->getParentJoint(), tf, withRespectTo); } +//============================================================================== +void KinematicJoint::setTransform( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo, + bool applyToAllRootBodies) +{ + setTransformOf(skeleton, tf, withRespectTo, applyToAllRootBodies); +} + //============================================================================== void KinematicJoint::setTransformOf( Skeleton* skeleton, @@ -117,19 +141,21 @@ void KinematicJoint::setTransformOf( const Frame* withRespectTo, bool applyToAllRootBodies) { - if (!skeleton) + if (nullptr == skeleton) return; - if (skeleton->getNumTrees() == 0) + const std::size_t numTrees = skeleton->getNumTrees(); + + if (0 == numTrees) return; - if (applyToAllRootBodies) { - for (std::size_t i = 0; i < skeleton->getNumTrees(); ++i) { - setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); - } - } else { + if (!applyToAllRootBodies) { setTransformOf(skeleton->getRootBodyNode(), tf, withRespectTo); + return; } + + for (std::size_t i = 0; i < numTrees; ++i) + setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); } //============================================================================== @@ -204,23 +230,31 @@ void KinematicJoint::setSpatialVelocity( return; } - // Transform newSpatialVelocity into the child body node frame if needed - Eigen::Vector6d targetRelSpatialVel = (getChildBodyNode() == inCoordinatesOf) - ? newSpatialVelocity - : math::AdR(inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); + // Change the reference frame of "newSpatialVelocity" to the child body node + // frame. + Eigen::Vector6d targetRelSpatialVel = newSpatialVelocity; + if (getChildBodyNode() != inCoordinatesOf) { + targetRelSpatialVel = math::AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); + } - // Adjust for parent frame if necessary + // Compute the target relative spatial velocity from the parent body node to + // the child body node. if (getChildBodyNode()->getParentFrame() != relativeTo) { - const Eigen::Vector6d parentVelocity = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialVelocity()); - if (relativeTo->isWorld()) { + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + targetRelSpatialVel -= parentVelocity; } else { + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); const Eigen::Vector6d arbitraryVelocity = math::AdT( relativeTo->getTransform(getChildBodyNode()), relativeTo->getSpatialVelocity()); + targetRelSpatialVel += -parentVelocity + arbitraryVelocity; } } @@ -232,7 +266,7 @@ void KinematicJoint::setSpatialVelocity( void KinematicJoint::setLinearVelocity( const Eigen::Vector3d& newLinearVelocity, const Frame* relativeTo) { - assert(nullptr != relativeTo); + assert(nullptr != relativeTo); assert(nullptr != inCoordinatesOf); Eigen::Vector6d targetSpatialVelocity; @@ -262,6 +296,11 @@ void KinematicJoint::setAngularVelocity( Eigen::Vector6d targetSpatialVelocity; + // Hean Angular Velocities + targetSpatialVelocity.head<3>() + = getChildBodyNode()->getWorldTransform().linear().transpose() + * inCoordinatesOf->getWorldTransform().linear() * newAngularVelocity; + // Translate Velocities if a non world coordiinate frame is used if (Frame::World() == relativeTo) { targetSpatialVelocity.tail<3>() @@ -273,14 +312,38 @@ void KinematicJoint::setAngularVelocity( .tail<3>(); } - // Hean Angular Velocities - targetSpatialVelocity.head<3>() - = getChildBodyNode()->getWorldTransform().linear().transpose() - * inCoordinatesOf->getWorldTransform().linear() * newAngularVelocity; - setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); } +//============================================================================== +void KinematicJoint::setLinearAcceleration( + const Eigen::Vector3d& newLinearAcceleration, + const Frame* relativeTo, + const Frame* inCoordinatesOf) +{ + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + Eigen::Vector6d targetSpatialAcceleration; + + if (Frame::World() == relativeTo) { + targetSpatialAcceleration.head<3>() + = getChildBodyNode()->getSpatialAcceleration().head<3>(); + } else { + targetSpatialAcceleration.head<3>() + = getChildBodyNode() + ->getSpatialAcceleration(relativeTo, getChildBodyNode()) + .head<3>(); + } + + const Eigen::Vector6d& V + = getChildBodyNode()->getSpatialVelocity(relativeTo, inCoordinatesOf); + targetSpatialAcceleration.tail<3>() + = getChildBodyNode()->getWorldTransform().linear().transpose() + * inCoordinatesOf->getWorldTransform().linear() + * (newLinearAcceleration - V.head<3>().cross(V.tail<3>())); +} + //============================================================================== Eigen::Matrix6d KinematicJoint::getRelativeJacobianStatic( const Eigen::Vector6d& /*positions*/) const @@ -292,8 +355,10 @@ Eigen::Matrix6d KinematicJoint::getRelativeJacobianStatic( Eigen::Vector6d KinematicJoint::getPositionDifferencesStatic( const Eigen::Vector6d& _q2, const Eigen::Vector6d& _q1) const { - return convertToPositions( - convertToTransform(_q1).inverse() * convertToTransform(_q2)); + const Eigen::Isometry3d T1 = convertToTransform(_q1); + const Eigen::Isometry3d T2 = convertToTransform(_q2); + + return convertToPositions(T1.inverse() * T2); } //============================================================================== @@ -301,6 +366,9 @@ 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); } @@ -343,24 +411,44 @@ void KinematicJoint::integratePositions(double _dt) setPositionsStatic(convertToPositions(Qnext)); } +//============================================================================== +void KinematicJoint::integrateVelocities(double _dt) +{ + (void)_dt; // To avoid unused variable warning + // For KinematicJoint, we don't need to integrate the velocity. We just + // need to set the velocity to the current velocity, ignoring the + // acceleration. + + // SKIP Velocity should be set directly + // TODO To be removed + // dtmsg << "[KinematicJoint::integrateVelocities] This function is not " + // << "using dt for integration which value is "<< _dt <<".\n"; + setVelocitiesStatic(getVelocitiesStatic()); +} + //==============================================ss================================ void KinematicJoint::updateDegreeOfFreedomNames() { - static const char* dofNames[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 + dofNames[i], false); - } - } + if (!mDofs[0]->isNamePreserved()) + mDofs[0]->setName(Joint::mAspectProperties.mName + "_rot_x", false); + if (!mDofs[1]->isNamePreserved()) + mDofs[1]->setName(Joint::mAspectProperties.mName + "_rot_y", false); + if (!mDofs[2]->isNamePreserved()) + mDofs[2]->setName(Joint::mAspectProperties.mName + "_rot_z", false); + if (!mDofs[3]->isNamePreserved()) + mDofs[3]->setName(Joint::mAspectProperties.mName + "_pos_x", false); + if (!mDofs[4]->isNamePreserved()) + mDofs[4]->setName(Joint::mAspectProperties.mName + "_pos_y", false); + if (!mDofs[5]->isNamePreserved()) + mDofs[5]->setName(Joint::mAspectProperties.mName + "_pos_z", false); } //============================================================================== void KinematicJoint::updateRelativeTransform() const { - mT = Joint::mAspectProperties.mT_ParentBodyToJoint - * convertToTransform(getPositionsStatic()) + mQ = convertToTransform(getPositionsStatic()); + + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * mQ * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); assert(math::verifyTransform(mT)); diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 13aee6d1e8d30..7123cb39827f6 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -35,6 +35,8 @@ #include +#include + #include #include @@ -83,7 +85,10 @@ class KinematicJoint : public GenericJoint 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. + /// 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 @@ -91,13 +96,35 @@ class KinematicJoint : public GenericJoint const Eigen::Vector6d& _positions); /// If the given joint is a KinematicJoint, then set the transform of the - /// given Joint's child BodyNode so it transforms with respect to + /// given Joint's child BodyNode so that its transform with respect to + /// "withRespecTo" is equal to "tf". + /// + /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead + DART_DEPRECATED(6.9) + static void setTransform( + Joint* joint, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World()); + + /// 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". + /// + /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead + DART_DEPRECATED(6.9) + static void setTransform( + BodyNode* bodyNode, + 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". @@ -106,6 +133,19 @@ class KinematicJoint : public GenericJoint 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(). + /// + /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead + DART_DEPRECATED(6.9) + static void setTransform( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World(), + bool applyToAllRootBodies = true); + /// 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 @@ -120,13 +160,28 @@ class KinematicJoint : public GenericJoint /// 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", or + /// "newSpatialAcceleration" 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". + /// \param[in] newSpatialAcceleration Desired spatial acceleration of the /// child BodyNode. + /// \param[in] accRelativeTo The relative frame of "newSpatialAcceleration". + /// \param[in] accInCoordinatesOf The reference frame of + /// "newSpatialAcceleration". void setSpatialMotion( const Eigen::Isometry3d* newTransform, const Frame* withRespectTo, @@ -198,6 +253,20 @@ class KinematicJoint : public GenericJoint const Frame* relativeTo = Frame::World(), const Frame* inCoordinatesOf = Frame::World()); + /// Set the linear portion of classical acceleration of the child BodyNode + /// relative to an arbitrary Frame. + /// + /// Note that the angular portion of claasical acceleration of the child + /// BodyNode will not be changed after this function called. + /// + /// \param[in] newLinearAcceleration + /// \param[in] relativeTo The relative frame of "newLinearAcceleration". + /// \param[in] inCoordinatesOf The reference frame of "newLinearAcceleration". + void setLinearAcceleration( + const Eigen::Vector3d& newLinearAcceleration, + const Frame* relativeTo = Frame::World(), + const Frame* inCoordinatesOf = Frame::World()); + // Documentation inherited Eigen::Matrix6d getRelativeJacobianStatic( const Eigen::Vector6d& _positions) const override; @@ -218,6 +287,9 @@ class KinematicJoint : public GenericJoint // Documentation inherited void integratePositions(double _dt) override; + // Documentation inherited + void integrateVelocities(double _dt) override; + // Documentation inherited void updateDegreeOfFreedomNames() override; From cc6fce38e8d7de4e55ca63981701ef2b5795873b Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 18:54:21 +0000 Subject: [PATCH 16/24] tracking down errors --- dart/dynamics/KinematicJoint.cpp | 53 ----------------------------- dart/dynamics/KinematicJoint.hpp | 58 ++------------------------------ 2 files changed, 2 insertions(+), 109 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 3c6022272cb7b..09909babcba00 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -79,13 +79,6 @@ Eigen::Isometry3d KinematicJoint::convertToTransform( return tf; } -//============================================================================== -void KinematicJoint::setTransform( - Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) -{ - return setTransformOf(joint, tf, withRespectTo); -} - //============================================================================== void KinematicJoint::setTransformOf( Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) @@ -107,13 +100,6 @@ void KinematicJoint::setTransformOf( kinematicJoint->setTransform(tf, withRespectTo); } -//============================================================================== -void KinematicJoint::setTransform( - BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) -{ - setTransformOf(bodyNode, tf, withRespectTo); -} - //============================================================================== void KinematicJoint::setTransformOf( BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) @@ -124,16 +110,6 @@ void KinematicJoint::setTransformOf( setTransformOf(bodyNode->getParentJoint(), tf, withRespectTo); } -//============================================================================== -void KinematicJoint::setTransform( - Skeleton* skeleton, - const Eigen::Isometry3d& tf, - const Frame* withRespectTo, - bool applyToAllRootBodies) -{ - setTransformOf(skeleton, tf, withRespectTo, applyToAllRootBodies); -} - //============================================================================== void KinematicJoint::setTransformOf( Skeleton* skeleton, @@ -315,35 +291,6 @@ void KinematicJoint::setAngularVelocity( setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); } -//============================================================================== -void KinematicJoint::setLinearAcceleration( - const Eigen::Vector3d& newLinearAcceleration, - const Frame* relativeTo, - const Frame* inCoordinatesOf) -{ - assert(nullptr != relativeTo); - assert(nullptr != inCoordinatesOf); - - Eigen::Vector6d targetSpatialAcceleration; - - if (Frame::World() == relativeTo) { - targetSpatialAcceleration.head<3>() - = getChildBodyNode()->getSpatialAcceleration().head<3>(); - } else { - targetSpatialAcceleration.head<3>() - = getChildBodyNode() - ->getSpatialAcceleration(relativeTo, getChildBodyNode()) - .head<3>(); - } - - const Eigen::Vector6d& V - = getChildBodyNode()->getSpatialVelocity(relativeTo, inCoordinatesOf); - targetSpatialAcceleration.tail<3>() - = getChildBodyNode()->getWorldTransform().linear().transpose() - * inCoordinatesOf->getWorldTransform().linear() - * (newLinearAcceleration - V.head<3>().cross(V.tail<3>())); -} - //============================================================================== Eigen::Matrix6d KinematicJoint::getRelativeJacobianStatic( const Eigen::Vector6d& /*positions*/) const diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 7123cb39827f6..819266ebf5f8d 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -95,17 +95,6 @@ class KinematicJoint : public GenericJoint 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". - /// - /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead - DART_DEPRECATED(6.9) - static void setTransform( - Joint* joint, - const Eigen::Isometry3d& tf, - const Frame* withRespectTo = Frame::World()); - /// 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". @@ -114,17 +103,6 @@ class KinematicJoint : public GenericJoint 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". - /// - /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead - DART_DEPRECATED(6.9) - static void setTransform( - BodyNode* bodyNode, - 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". @@ -133,19 +111,6 @@ class KinematicJoint : public GenericJoint 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(). - /// - /// \deprecated Deprecated in DART 6.9. Use setTransformOf() instead - DART_DEPRECATED(6.9) - static void setTransform( - Skeleton* skeleton, - const Eigen::Isometry3d& tf, - const Frame* withRespectTo = Frame::World(), - bool applyToAllRootBodies = true); - /// 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 @@ -168,8 +133,8 @@ class KinematicJoint : public GenericJoint /// the right order so that all the desired Cartetian coordinates are properly /// set. /// - /// Pass nullptr for "newTransform", "newSpatialVelocity", or - /// "newSpatialAcceleration" if you don't want to set them. + /// 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". @@ -177,11 +142,6 @@ class KinematicJoint : public GenericJoint /// BodyNode. /// \param[in] velRelativeTo The relative frame of "newSpatialVelocity". /// \param[in] velInCoordinatesOf The reference frame of "newSpatialVelocity". - /// \param[in] newSpatialAcceleration Desired spatial acceleration of the - /// child BodyNode. - /// \param[in] accRelativeTo The relative frame of "newSpatialAcceleration". - /// \param[in] accInCoordinatesOf The reference frame of - /// "newSpatialAcceleration". void setSpatialMotion( const Eigen::Isometry3d* newTransform, const Frame* withRespectTo, @@ -253,20 +213,6 @@ class KinematicJoint : public GenericJoint const Frame* relativeTo = Frame::World(), const Frame* inCoordinatesOf = Frame::World()); - /// Set the linear portion of classical acceleration of the child BodyNode - /// relative to an arbitrary Frame. - /// - /// Note that the angular portion of claasical acceleration of the child - /// BodyNode will not be changed after this function called. - /// - /// \param[in] newLinearAcceleration - /// \param[in] relativeTo The relative frame of "newLinearAcceleration". - /// \param[in] inCoordinatesOf The reference frame of "newLinearAcceleration". - void setLinearAcceleration( - const Eigen::Vector3d& newLinearAcceleration, - const Frame* relativeTo = Frame::World(), - const Frame* inCoordinatesOf = Frame::World()); - // Documentation inherited Eigen::Matrix6d getRelativeJacobianStatic( const Eigen::Vector6d& _positions) const override; From 0ce7743dcf019c34c8df58708ae9e6c2fd86a5e2 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 19:03:18 +0000 Subject: [PATCH 17/24] refactor functions v2.0 --- dart/dynamics/KinematicJoint.cpp | 38 +++++++++++++------------------- 1 file changed, 15 insertions(+), 23 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 09909babcba00..4a95fbc8aef78 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -117,21 +117,19 @@ void KinematicJoint::setTransformOf( const Frame* withRespectTo, bool applyToAllRootBodies) { - if (nullptr == skeleton) + if (!skeleton) return; - const std::size_t numTrees = skeleton->getNumTrees(); - - if (0 == numTrees) + const auto numTrees = skeleton->getNumTrees(); + if (numTrees == 0) return; - if (!applyToAllRootBodies) { + if (applyToAllRootBodies) { + for (std::size_t i = 0; i < numTrees; ++i) + setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); + } else { setTransformOf(skeleton->getRootBodyNode(), tf, withRespectTo); - return; } - - for (std::size_t i = 0; i < numTrees; ++i) - setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); } //============================================================================== @@ -206,32 +204,26 @@ void KinematicJoint::setSpatialVelocity( return; } - // Change the reference frame of "newSpatialVelocity" to the child body node - // frame. + // Transform newSpatialVelocity into the child body node frame if needed Eigen::Vector6d targetRelSpatialVel = newSpatialVelocity; if (getChildBodyNode() != inCoordinatesOf) { targetRelSpatialVel = math::AdR( inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); } - // Compute the target relative spatial velocity from the parent body node to - // the child body node. + // Adjust for parent frame velocity if relativeTo is not the parent frame if (getChildBodyNode()->getParentFrame() != relativeTo) { - if (relativeTo->isWorld()) { - const Eigen::Vector6d parentVelocity = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + const Eigen::Vector6d parentVelocity = math::AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + if (relativeTo->isWorld()) { targetRelSpatialVel -= parentVelocity; } else { - const Eigen::Vector6d parentVelocity = math::AdInvT( - getRelativeTransform(), - getChildBodyNode()->getParentFrame()->getSpatialVelocity()); - const Eigen::Vector6d arbitraryVelocity = math::AdT( + const Eigen::Vector6d relVelocity = math::AdT( relativeTo->getTransform(getChildBodyNode()), relativeTo->getSpatialVelocity()); - - targetRelSpatialVel += -parentVelocity + arbitraryVelocity; + targetRelSpatialVel += relVelocity - parentVelocity; } } From e192312a7ab46257eb4c3e4beec7fdbf1a13d7c9 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 19:48:06 +0000 Subject: [PATCH 18/24] refactor update DegreesofFreedomNames --- dart/dynamics/KinematicJoint.cpp | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 4a95fbc8aef78..85372dc3d38e3 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -368,18 +368,14 @@ void KinematicJoint::integrateVelocities(double _dt) //==============================================ss================================ void KinematicJoint::updateDegreeOfFreedomNames() { - if (!mDofs[0]->isNamePreserved()) - mDofs[0]->setName(Joint::mAspectProperties.mName + "_rot_x", false); - if (!mDofs[1]->isNamePreserved()) - mDofs[1]->setName(Joint::mAspectProperties.mName + "_rot_y", false); - if (!mDofs[2]->isNamePreserved()) - mDofs[2]->setName(Joint::mAspectProperties.mName + "_rot_z", false); - if (!mDofs[3]->isNamePreserved()) - mDofs[3]->setName(Joint::mAspectProperties.mName + "_pos_x", false); - if (!mDofs[4]->isNamePreserved()) - mDofs[4]->setName(Joint::mAspectProperties.mName + "_pos_y", false); - if (!mDofs[5]->isNamePreserved()) - mDofs[5]->setName(Joint::mAspectProperties.mName + "_pos_z", false); + 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); + } } //============================================================================== From 85ff62863c2e6a35eaad3d4fd9950b00587ed607 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 20:00:09 +0000 Subject: [PATCH 19/24] remove integrateVel --- dart/dynamics/KinematicJoint.cpp | 15 --------------- dart/dynamics/KinematicJoint.hpp | 3 --- 2 files changed, 18 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 85372dc3d38e3..10282ccca8c7f 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -350,21 +350,6 @@ void KinematicJoint::integratePositions(double _dt) setPositionsStatic(convertToPositions(Qnext)); } -//============================================================================== -void KinematicJoint::integrateVelocities(double _dt) -{ - (void)_dt; // To avoid unused variable warning - // For KinematicJoint, we don't need to integrate the velocity. We just - // need to set the velocity to the current velocity, ignoring the - // acceleration. - - // SKIP Velocity should be set directly - // TODO To be removed - // dtmsg << "[KinematicJoint::integrateVelocities] This function is not " - // << "using dt for integration which value is "<< _dt <<".\n"; - setVelocitiesStatic(getVelocitiesStatic()); -} - //==============================================ss================================ void KinematicJoint::updateDegreeOfFreedomNames() { diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 819266ebf5f8d..1cfd388aef008 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -233,9 +233,6 @@ class KinematicJoint : public GenericJoint // Documentation inherited void integratePositions(double _dt) override; - // Documentation inherited - void integrateVelocities(double _dt) override; - // Documentation inherited void updateDegreeOfFreedomNames() override; From 47970974cd27b0ad822d9a6ee0bae6ef4b70fa5a Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 21:47:41 +0000 Subject: [PATCH 20/24] fix version --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 32e27e30f7934..083e25dc514c1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,7 +2,7 @@ ## DART 6 -### [DART 6.15.0 (2025-06-23)](https://github.com/dartsim/dart/milestone/77?closed=1) +### [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) From f9ed5b454c7a4db2203a4380a5abcbda0436cff6 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 21:47:45 +0000 Subject: [PATCH 21/24] fix version --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 4160437efbb647ba8d9659d6da80b6752631b048 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 22:08:31 +0000 Subject: [PATCH 22/24] necessary stuff --- dart/dynamics/KinematicJoint.cpp | 11 ++++++++--- dart/dynamics/KinematicJoint.hpp | 3 +++ 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 10282ccca8c7f..998da738a8493 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -350,12 +350,17 @@ void KinematicJoint::integratePositions(double _dt) setPositionsStatic(convertToPositions(Qnext)); } +//============================================================================== +void KinematicJoint::integrateVelocities(double /*_dt*/) +{ + setVelocitiesStatic(getVelocitiesStatic()); +} + //==============================================ss================================ void KinematicJoint::updateDegreeOfFreedomNames() { - static const char* suffixes[6] = { - "_rot_x", "_rot_y", "_rot_z", "_pos_x", "_pos_y", "_pos_z" - }; + 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()) diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 1cfd388aef008..819266ebf5f8d 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -233,6 +233,9 @@ class KinematicJoint : public GenericJoint // Documentation inherited void integratePositions(double _dt) override; + // Documentation inherited + void integrateVelocities(double _dt) override; + // Documentation inherited void updateDegreeOfFreedomNames() override; From 1ea274055b1044e263d6798c08d840c820585194 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 24 Jun 2025 23:24:29 +0000 Subject: [PATCH 23/24] small fixes --- dart/dynamics/KinematicJoint.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 998da738a8493..0307afd78559c 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -262,14 +262,14 @@ void KinematicJoint::setAngularVelocity( assert(nullptr != relativeTo); assert(nullptr != inCoordinatesOf); - Eigen::Vector6d targetSpatialVelocity; + Eigen::Vector6d targetSpatialVelocity = Eigen::Vector6d::Zero(); - // Hean Angular Velocities + // 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 coordiinate frame is used + // Translate Velocities if a non world coordinate frame is used if (Frame::World() == relativeTo) { targetSpatialVelocity.tail<3>() = getChildBodyNode()->getSpatialVelocity().tail<3>(); @@ -280,6 +280,7 @@ void KinematicJoint::setAngularVelocity( .tail<3>(); } + // Set the spatial velocity of the child body node setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); } From 4817fed8d8c3b276cbc3b089f4f71b9f79bae464 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 7 Jul 2025 18:31:10 +0000 Subject: [PATCH 24/24] fix --- dart/dynamics/KinematicJoint.cpp | 7 +++++++ dart/dynamics/KinematicJoint.hpp | 3 +++ 2 files changed, 10 insertions(+) diff --git a/dart/dynamics/KinematicJoint.cpp b/dart/dynamics/KinematicJoint.cpp index 0307afd78559c..352b5486b528e 100644 --- a/dart/dynamics/KinematicJoint.cpp +++ b/dart/dynamics/KinematicJoint.cpp @@ -357,6 +357,13 @@ void KinematicJoint::integrateVelocities(double /*_dt*/) setVelocitiesStatic(getVelocitiesStatic()); } +//============================================================================== +void KinematicJoint::updateConstrainedTerms(double /*timeStep*/) +{ + const Eigen::Vector6d& velBefore = getVelocitiesStatic(); + setVelocitiesStatic(velBefore); +} + //==============================================ss================================ void KinematicJoint::updateDegreeOfFreedomNames() { diff --git a/dart/dynamics/KinematicJoint.hpp b/dart/dynamics/KinematicJoint.hpp index 819266ebf5f8d..7d231081a3f53 100644 --- a/dart/dynamics/KinematicJoint.hpp +++ b/dart/dynamics/KinematicJoint.hpp @@ -236,6 +236,9 @@ class KinematicJoint : public GenericJoint // Documentation inherited void integrateVelocities(double _dt) override; + // Documentation inherited + void updateConstrainedTerms(double timeStep) override; + // Documentation inherited void updateDegreeOfFreedomNames() override;