From 6209661b1f9ebd39bcc17afc9267f968bce2ec11 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 6 May 2025 16:45:56 +0000 Subject: [PATCH 1/5] 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 2/5] 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 3/5] 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 4/5] 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 5/5] 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())