From a39f1d0d9a640f6ceb5868b0bb913dfb0864d812 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 19 May 2025 15:47:34 +0000 Subject: [PATCH 01/21] add fixes Signed-off-by: Jose Mayoral --- CMakeLists.txt | 2 +- dartsim/src/FreeGroupFeatures.cc | 11 ++++-- dartsim/src/SDFFeatures.cc | 59 ++++++++++++++++++++++++++------ 3 files changed, 57 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 06c8859ff..d69707eab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,7 +78,7 @@ gz_find_package(DART utils utils-urdf CONFIG - VERSION 6.10 + VERSION 6.16.0 REQUIRED_BY dartsim PKGCONFIG dart PKGCONFIG_VER_COMPARISON >=) diff --git a/dartsim/src/FreeGroupFeatures.cc b/dartsim/src/FreeGroupFeatures.cc index 70b8c04fa..4bbef4d05 100644 --- a/dartsim/src/FreeGroupFeatures.cc +++ b/dartsim/src/FreeGroupFeatures.cc @@ -18,6 +18,7 @@ #include "FreeGroupFeatures.hh" #include +#include #include #include @@ -38,11 +39,13 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( if (skeleton->getNumBodyNodes() == 0 && modelInfo->nestedModels.empty()) return this->GenerateInvalidId(); - // Verify that all root joints are FreeJoints + // Verify that all root joints are FreeJoints or KinematicJoints for (std::size_t i = 0; i < skeleton->getNumTrees(); ++i) { if (skeleton->getRootJoint(i)->getType() - != dart::dynamics::FreeJoint::getStaticType()) + != dart::dynamics::FreeJoint::getStaticType() && + skeleton->getRootJoint(i)->getType() + != dart::dynamics::KinematicJoint::getStaticType()) { return this->GenerateInvalidId(); } @@ -85,7 +88,9 @@ Identity FreeGroupFeatures::FindFreeGroupForLink( while (bn) { if (bn->getParentJoint()->getType() - == dart::dynamics::FreeJoint::getStaticType()) + == dart::dynamics::FreeJoint::getStaticType() || + bn->getParentJoint()->getType() + == dart::dynamics::KinematicJoint::getStaticType()) { break; } diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index f06e108d0..c9f491def 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include #include #include @@ -672,28 +673,63 @@ Identity SDFFeatures::ConstructSdfLink( const Eigen::Vector3d localCom = math::eigen3::convert(sdfInertia.Pose().Pos()); + const bool isKinematic = _sdfLink.Kinematic(); + bodyProperties.mInertia.setLocalCOM(localCom); bodyProperties.mGravityMode = _sdfLink.EnableGravity(); dart::dynamics::FreeJoint::Properties jointProperties; jointProperties.mName = bodyProperties.mName + "_FreeJoint"; - // TODO(MXG): Consider adding a UUID to this joint name in order to avoid any - // potential (albeit unlikely) name collisions. - // Note: When constructing a link from this function, we always instantiate - // it as a standalone free body within the model. If it should have any joint - // constraints, those will be added later. - const auto result = modelInfo.model->createJointAndBodyNodePair< - dart::dynamics::FreeJoint>(nullptr, jointProperties, bodyProperties); + dart::dynamics::BodyNode * bn; + + bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass()); + bodyProperties.mGravityMode = _sdfLink.EnableGravity(); + bodyProperties.mInertia.setMoment(I_link); + + bodyProperties.mInertia.setLocalCOM(localCom); + bodyProperties.mFrictionCoeff = 0; - dart::dynamics::FreeJoint * const joint = result.first; const Eigen::Isometry3d tf = GetParentModelFrame(modelInfo) * ResolveSdfPose(_sdfLink.SemanticPose()); - joint->setTransform(tf); + if(isKinematic){ + gzdbg << "Kinematic tag found -> " << bodyProperties.mName << std::endl; + jointProperties.mName = bodyProperties.mName + "_KinematicJoint"; + bodyProperties.mGravityMode = false; + + //jointProperties.mActuatorType = dart::dynamics::Joint::ActuatorType::PASSIVE; + //jointProperties.mName = bodyProperties.mName + "_KinematicJoint"; + + auto result = modelInfo.model->createJointAndBodyNodePair< + dart::dynamics::KinematicJoint>(nullptr, jointProperties, bodyProperties); + // result.first->setAccelerations(Eigen::Vector6d::Zero()); + //`result.second->setAccelerations(Eigen::Vector6d::Zero()); + + dart::dynamics::KinematicJoint * const joint = result.first; + joint->setTransform(tf); + + bn = result.second; + } + + else{ + // Note: When constructing a link from this function, we always instantiate + // it as a standalone free body within the model. If it should have any joint + // constraints, those will be added later. + + // TODO(MXG): Consider adding a UUID to this joint name in order to avoid any + // potential (albeit unlikely) name collisions. + + auto result = modelInfo.model->createJointAndBodyNodePair< + dart::dynamics::FreeJoint>(nullptr, jointProperties, bodyProperties); + + dart::dynamics::FreeJoint * const joint = result.first; + joint->setTransform(tf); + + bn = result.second; + } - dart::dynamics::BodyNode * const bn = result.second; auto worldID = this->GetWorldOfModelImpl(_modelID); if (worldID == INVALID_ENTITY_ID) @@ -1106,7 +1142,8 @@ Identity SDFFeatures::ConstructSdfJoint( { auto childsParentJoint = _child->getParentJoint(); std::string parentName = worldParent? "world" : _parent->getName(); - if (childsParentJoint->getType() != "FreeJoint") + if (childsParentJoint->getType() != "FreeJoint" && + childsParentJoint->getType() != "KinematicJoint") { gzerr << "Asked to create a joint between links " << "[" << parentName << "] as parent and [" From 57a544e53f98bf0480b91d2340966775f6a5900f Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 14 Jul 2025 21:00:23 +0000 Subject: [PATCH 02/21] remove strong dependency on dartsim MR Signed-off-by: Jose Mayoral --- CMakeLists.txt | 2 +- dartsim/src/FreeGroupFeatures.cc | 1 - dartsim/src/SDFFeatures.cc | 1 - 3 files changed, 1 insertion(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d69707eab..06c8859ff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,7 +78,7 @@ gz_find_package(DART utils utils-urdf CONFIG - VERSION 6.16.0 + VERSION 6.10 REQUIRED_BY dartsim PKGCONFIG dart PKGCONFIG_VER_COMPARISON >=) diff --git a/dartsim/src/FreeGroupFeatures.cc b/dartsim/src/FreeGroupFeatures.cc index 4bbef4d05..00829681a 100644 --- a/dartsim/src/FreeGroupFeatures.cc +++ b/dartsim/src/FreeGroupFeatures.cc @@ -18,7 +18,6 @@ #include "FreeGroupFeatures.hh" #include -#include #include #include diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index c9f491def..f85060be4 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -31,7 +31,6 @@ #include #include #include -#include #include #include #include From 5ab207e25ba2c9624c11c4508066ed89869d56ab Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 22 Jul 2025 11:31:45 -0600 Subject: [PATCH 03/21] clean for tests Signed-off-by: Jose Mayoral --- dartsim/src/FreeGroupFeatures.cc | 4 ++-- dartsim/src/SDFFeatures.cc | 4 ++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/dartsim/src/FreeGroupFeatures.cc b/dartsim/src/FreeGroupFeatures.cc index 00829681a..e2b4df70d 100644 --- a/dartsim/src/FreeGroupFeatures.cc +++ b/dartsim/src/FreeGroupFeatures.cc @@ -44,7 +44,7 @@ Identity FreeGroupFeatures::FindFreeGroupForModel( if (skeleton->getRootJoint(i)->getType() != dart::dynamics::FreeJoint::getStaticType() && skeleton->getRootJoint(i)->getType() - != dart::dynamics::KinematicJoint::getStaticType()) + != gz::dynamics::KinematicJoint::getStaticType()) { return this->GenerateInvalidId(); } @@ -89,7 +89,7 @@ Identity FreeGroupFeatures::FindFreeGroupForLink( if (bn->getParentJoint()->getType() == dart::dynamics::FreeJoint::getStaticType() || bn->getParentJoint()->getType() - == dart::dynamics::KinematicJoint::getStaticType()) + == gz::dynamics::KinematicJoint::getStaticType()) { break; } diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index f85060be4..c3d0af1b0 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -672,7 +672,11 @@ Identity SDFFeatures::ConstructSdfLink( const Eigen::Vector3d localCom = math::eigen3::convert(sdfInertia.Pose().Pos()); +<<<<<<< HEAD const bool isKinematic = _sdfLink.Kinematic(); +======= + const bool isKinematic = false; //TODO temporal fix for kinematic tag //_sdfLink.Kinematic(); +>>>>>>> 72e1caf4 (clean for tests) bodyProperties.mInertia.setLocalCOM(localCom); From 6b5b32612cbf0b0525b87887caacf0220339bbcd Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Mon, 14 Jul 2025 21:00:23 +0000 Subject: [PATCH 04/21] remove strong dependency on dartsim MR Signed-off-by: Jose Mayoral --- dartsim/src/FreeGroupFeatures.hh | 1 + dartsim/src/KinematicJoint.cc | 391 +++++++++++++++++++++++++++++++ dartsim/src/KinematicJoint.hh | 260 ++++++++++++++++++++ dartsim/src/SDFFeatures.cc | 11 +- dartsim/src/SDFFeatures.hh | 2 + 5 files changed, 657 insertions(+), 8 deletions(-) create mode 100644 dartsim/src/KinematicJoint.cc create mode 100644 dartsim/src/KinematicJoint.hh diff --git a/dartsim/src/FreeGroupFeatures.hh b/dartsim/src/FreeGroupFeatures.hh index df72c2563..dd91f324d 100644 --- a/dartsim/src/FreeGroupFeatures.hh +++ b/dartsim/src/FreeGroupFeatures.hh @@ -19,6 +19,7 @@ #define GZ_PHYSICS_DARTSIM_SRC_FREEGROUPFEATURES_HH_ #include +#include "KinematicJoint.hh" #include "Base.hh" diff --git a/dartsim/src/KinematicJoint.cc b/dartsim/src/KinematicJoint.cc new file mode 100644 index 000000000..12d35096f --- /dev/null +++ b/dartsim/src/KinematicJoint.cc @@ -0,0 +1,391 @@ +/* + * 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 "KinematicJoint.hh" +#include "dart/math/Geometry.hpp" +#include "dart/math/Helpers.hpp" + +using namespace gz::dynamics; +using namespace dart::dynamics; +using namespace dart::math; + +//============================================================================== +Eigen::Vector6d KinematicJoint::convertToPositions(const Eigen::Isometry3d& _tf) +{ + Eigen::Vector6d x; + x.head<3>() = 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() = expMapRot(_positions.head<3>()); + tf.translation() = _positions.tail<3>(); + return tf; +} + +//============================================================================== +void KinematicJoint::setTransformOf( + Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) +{ + if (nullptr == joint) + return; + + KinematicJoint* kinematicJoint = dynamic_cast(joint); + + if (nullptr == kinematicJoint) { + dtwarn + << "[KinematicJoint::setTransform] Invalid joint type. Setting " + "transform " + << "is only allowed to KinematicJoint. The joint type of given joint [" + << joint->getName() << "] is [" << joint->getType() << "].\n"; + return; + } + + kinematicJoint->setTransform(tf, withRespectTo); +} + +//============================================================================== +void KinematicJoint::setTransformOf( + BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) +{ + if (nullptr == bodyNode) + return; + + setTransformOf(bodyNode->getParentJoint(), tf, withRespectTo); +} + +//============================================================================== +void KinematicJoint::setTransformOf( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo, + bool applyToAllRootBodies) +{ + if (!skeleton) + return; + + const auto numTrees = skeleton->getNumTrees(); + if (numTrees == 0) + return; + + if (applyToAllRootBodies) { + for (std::size_t i = 0; i < numTrees; ++i) + setTransformOf(skeleton->getRootBodyNode(i), tf, withRespectTo); + } else { + setTransformOf(skeleton->getRootBodyNode(), tf, withRespectTo); + } +} + +//============================================================================== +void KinematicJoint::setSpatialMotion( + const Eigen::Isometry3d* newTransform, + const Frame* withRespectTo, + const Eigen::Vector6d* newSpatialVelocity, + const Frame* velRelativeTo, + const Frame* velInCoordinatesOf) +{ + if (newTransform) + setTransform(*newTransform, withRespectTo); + + if (newSpatialVelocity) + setSpatialVelocity(*newSpatialVelocity, velRelativeTo, velInCoordinatesOf); +} + +//============================================================================== +void KinematicJoint::setRelativeTransform(const Eigen::Isometry3d& newTransform) +{ + setPositionsStatic(convertToPositions( + Joint::mAspectProperties.mT_ParentBodyToJoint.inverse() * newTransform + * Joint::mAspectProperties.mT_ChildBodyToJoint)); +} + +//============================================================================== +void KinematicJoint::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( + AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), + newSpatialVelocity)); + } +} + +//============================================================================== +void KinematicJoint::setSpatialVelocity( + const Eigen::Vector6d& newSpatialVelocity, + const Frame* relativeTo, + const Frame* inCoordinatesOf) +{ + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + if (getChildBodyNode() == relativeTo) { + dtwarn << "[KinematicJoint::setSpatialVelocity] Invalid reference frame " + "for newSpatialVelocity. It shouldn't be the child BodyNode.\n"; + return; + } + + // Transform newSpatialVelocity into the child body node frame if needed + Eigen::Vector6d targetRelSpatialVel = newSpatialVelocity; + if (getChildBodyNode() != inCoordinatesOf) { + targetRelSpatialVel = AdR( + inCoordinatesOf->getTransform(getChildBodyNode()), newSpatialVelocity); + } + + // Adjust for parent frame velocity if relativeTo is not the parent frame + if (getChildBodyNode()->getParentFrame() != relativeTo) { + const Eigen::Vector6d parentVelocity = AdInvT( + getRelativeTransform(), + getChildBodyNode()->getParentFrame()->getSpatialVelocity()); + + if (relativeTo->isWorld()) { + targetRelSpatialVel -= parentVelocity; + } else { + const Eigen::Vector6d relVelocity = AdT( + relativeTo->getTransform(getChildBodyNode()), + relativeTo->getSpatialVelocity()); + targetRelSpatialVel += relVelocity - parentVelocity; + } + } + + setRelativeSpatialVelocity(targetRelSpatialVel); +} + +//============================================================================== +void KinematicJoint::setTransform( + const Eigen::Isometry3d& newTransform, const Frame* withRespectTo) +{ + assert(nullptr != withRespectTo); + + setRelativeTransform( + withRespectTo->getTransform(getChildBodyNode()->getParentFrame()) + * newTransform); +} + +//============================================================================== +void KinematicJoint::setLinearVelocity( + const Eigen::Vector3d& newLinearVelocity, const Frame* relativeTo) +{ + assert(nullptr != relativeTo); + + Eigen::Vector6d targetSpatialVelocity; + + if (Frame::World() == relativeTo) { + targetSpatialVelocity.head<3>() + = getChildBodyNode()->getSpatialVelocity().head<3>(); + } else { + targetSpatialVelocity.head<3>() + = getChildBodyNode() + ->getSpatialVelocity(relativeTo, getChildBodyNode()) + .head<3>(); + } + + targetSpatialVelocity.tail<3>() = newLinearVelocity; + setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); +} + +//============================================================================== +void KinematicJoint::setAngularVelocity( + const Eigen::Vector3d& newAngularVelocity, + const Frame* relativeTo, + const Frame* inCoordinatesOf) +{ + assert(nullptr != relativeTo); + assert(nullptr != inCoordinatesOf); + + Eigen::Vector6d targetSpatialVelocity = Eigen::Vector6d::Zero(); + + // Gather the angular velocity in the child body node frame + targetSpatialVelocity.head<3>() + = getChildBodyNode()->getWorldTransform().linear().transpose() + * inCoordinatesOf->getWorldTransform().linear() * newAngularVelocity; + + // Translate Velocities if a non world coordinate frame is used + if (Frame::World() == relativeTo) { + targetSpatialVelocity.tail<3>() + = getChildBodyNode()->getSpatialVelocity().tail<3>(); + } else { + targetSpatialVelocity.tail<3>() + = getChildBodyNode() + ->getSpatialVelocity(relativeTo, getChildBodyNode()) + .tail<3>(); + } + + // Set the spatial velocity of the child body node + setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); +} + +//============================================================================== +Eigen::Matrix6d KinematicJoint::getRelativeJacobianStatic( + const Eigen::Vector6d& /*positions*/) const +{ + return mJacobian; +} + +//============================================================================== +Eigen::Vector6d KinematicJoint::getPositionDifferencesStatic( + const Eigen::Vector6d& _q2, const Eigen::Vector6d& _q1) const +{ + const Eigen::Isometry3d T1 = convertToTransform(_q1); + const Eigen::Isometry3d T2 = convertToTransform(_q2); + + return convertToPositions(T1.inverse() * T2); +} + +//============================================================================== +KinematicJoint::KinematicJoint(const Properties& properties) + : Base(properties), + mQ(Eigen::Isometry3d::Identity()) +{ + mJacobianDeriv = Eigen::Matrix6d::Zero(); + + // Inherited Aspects must be created in the final joint class in reverse order + // or else we get pure virtual function calls + createGenericJointAspect(properties); + createJointAspect(properties); +} + +//============================================================================== +Joint* KinematicJoint::clone() const +{ + return new KinematicJoint(getGenericJointProperties()); +} + +//============================================================================== +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(AdR(QdiffInv, getVelocitiesStatic())); + setPositionsStatic(convertToPositions(Qnext)); +} + +//============================================================================== +void KinematicJoint::integrateVelocities(double /*_dt*/) +{ + setVelocitiesStatic(getVelocitiesStatic()); +} + +//============================================================================== +void KinematicJoint::updateConstrainedTerms(double /*timeStep*/) +{ + const Eigen::Vector6d& velBefore = getVelocitiesStatic(); + setVelocitiesStatic(velBefore); +} + +//==============================================ss================================ +void KinematicJoint::updateDegreeOfFreedomNames() +{ + static const char* suffixes[6] + = {"_rot_x", "_rot_y", "_rot_z", "_pos_x", "_pos_y", "_pos_z"}; + + for (std::size_t i = 0; i < 6; ++i) { + if (!mDofs[i]->isNamePreserved()) + mDofs[i]->setName(Joint::mAspectProperties.mName + suffixes[i], false); + } +} + +//============================================================================== +void KinematicJoint::updateRelativeTransform() const +{ + mQ = convertToTransform(getPositionsStatic()); + + mT = Joint::mAspectProperties.mT_ParentBodyToJoint * mQ + * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); + + assert(verifyTransform(mT)); +} + +//============================================================================== +void KinematicJoint::updateRelativeJacobian(bool _mandatory) const +{ + if (_mandatory) + mJacobian + = 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; +} diff --git a/dartsim/src/KinematicJoint.hh b/dartsim/src/KinematicJoint.hh new file mode 100644 index 000000000..141d981f0 --- /dev/null +++ b/dartsim/src/KinematicJoint.hh @@ -0,0 +1,260 @@ +/* + * 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 GZ_DYNAMICS_KINEMATICJOINT_HPP_ +#define GZ_DYNAMICS_KINEMATICJOINT_HPP_ + +#include +//#include "GenericJoint.hpp" +#include + +#include + + +using namespace dart::dynamics; + +namespace gz { +namespace dynamics { + +/// This class represents a joint that allows for kinematic control of the +/// child BodyNode's transform and spatial velocity. It does not enforce any +/// dynamics, meaning it does not compute forces or torques based on the +/// kinematic state. Instead, it provides methods to set the transform and +/// spatial velocity of the child BodyNode directly, allowing for precise +/// control over its position and orientation in space. + +class KinematicJoint : public FreeJoint +{ +public: + using Base = FreeJoint; + + KinematicJoint(const KinematicJoint&) = delete; + + /// Destructor + virtual ~KinematicJoint() = default; + + /// Get the Properties of this KinematicJoint + //Properties getKinematicJointProperties() const; + + // Documentation inherited + const std::string& getType() const override; + + /// Get joint type for this class + static const std::string& getStaticType(); + + // Documentation inherited + bool isCyclic(std::size_t _index) const override; + + /// Convert a transform into a 6D vector that can be used to set the positions + /// of a KinematicJoint. The positions returned by this function will result + /// in a relative transform of getTransformFromParentBodyNode() * _tf * + /// getTransformFromChildBodyNode().inverse() between the parent BodyNode and + /// the child BodyNode frames when applied to a KinematicJoint. + static Eigen::Vector6d convertToPositions(const Eigen::Isometry3d& _tf); + + /// Convert a KinematicJoint-style 6D vector into a transform + static Eigen::Isometry3d convertToTransform( + const Eigen::Vector6d& _positions); + + /// If the given joint is a KinematicJoint, then set the transform of the + /// given Joint's child BodyNode so that its transform with respect to + /// "withRespecTo" is equal to "tf". + static void setTransformOf( + Joint* joint, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World()); + + /// If the parent Joint of the given BodyNode is a KinematicJoint, then set + /// the transform of the given BodyNode so that its transform with respect to + /// "withRespecTo" is equal to "tf". + static void setTransformOf( + BodyNode* bodyNode, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World()); + + // + void setTransform( + const Eigen::Isometry3d& newTransform, + const Frame* withRespectTo = Frame::World()); + + /// Apply setTransform(bodyNode, tf, withRespecTo) for all the root BodyNodes + /// of the given Skeleton. If false is passed in "applyToAllRootBodies", then + /// it will be applied to only the default root BodyNode that will be obtained + /// by Skeleton::getRootBodyNode(). + static void setTransformOf( + Skeleton* skeleton, + const Eigen::Isometry3d& tf, + const Frame* withRespectTo = Frame::World(), + bool applyToAllRootBodies = true); + + /// Set the transform, spatial velocity, and spatial acceleration of the child + /// BodyNode relative to an arbitrary Frame. The reference frame can be + /// arbitrarily specified. + /// + /// If you want to set more than one kind of Cartetian coordinates (e.g., + /// transform and spatial velocity) at the same time, you should call + /// corresponding setters in a certain order (transform -> velocity -> + /// acceleration), If you don't velocity or acceleration can be corrupted by + /// transform or velocity. This function calls the corresponding setters in + /// the right order so that all the desired Cartetian coordinates are properly + /// set. + /// + /// Pass nullptr for "newTransform", "newSpatialVelocity", + /// if you don't want to set them. + /// + /// \param[in] newTransform Desired transform of the child BodyNode. + /// \param[in] withRespectTo The relative Frame of "newTransform". + /// \param[in] newSpatialVelocity Desired spatial velocity of the child + /// BodyNode. + /// \param[in] velRelativeTo The relative frame of "newSpatialVelocity". + /// \param[in] velInCoordinatesOf The reference frame of "newSpatialVelocity". + void setSpatialMotion( + const Eigen::Isometry3d* newTransform, + const Frame* withRespectTo, + const Eigen::Vector6d* newSpatialVelocity, + const Frame* velRelativeTo, + const Frame* velInCoordinatesOf); + + /// Set the transform of the child BodyNode relative to the parent BodyNode + /// \param[in] newTransform Desired transform of the child BodyNode. + void setRelativeTransform(const Eigen::Isometry3d& newTransform); + + /// Set the 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; + + /// Constructor called by Skeleton class + // This must be public so that Skeleton can create KinematicJoints + KinematicJoint(const Properties& properties); + +protected: + + // 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 diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index c3d0af1b0..1c5609ef9 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -702,17 +702,12 @@ Identity SDFFeatures::ConstructSdfLink( jointProperties.mName = bodyProperties.mName + "_KinematicJoint"; bodyProperties.mGravityMode = false; - //jointProperties.mActuatorType = dart::dynamics::Joint::ActuatorType::PASSIVE; - //jointProperties.mName = bodyProperties.mName + "_KinematicJoint"; - auto result = modelInfo.model->createJointAndBodyNodePair< - dart::dynamics::KinematicJoint>(nullptr, jointProperties, bodyProperties); - // result.first->setAccelerations(Eigen::Vector6d::Zero()); - //`result.second->setAccelerations(Eigen::Vector6d::Zero()); + gz::dynamics::KinematicJoint>(nullptr, jointProperties, bodyProperties); - dart::dynamics::KinematicJoint * const joint = result.first; + auto const joint = result.first; joint->setTransform(tf); - + bn = result.second; } diff --git a/dartsim/src/SDFFeatures.hh b/dartsim/src/SDFFeatures.hh index f171c2939..b6fe757f6 100644 --- a/dartsim/src/SDFFeatures.hh +++ b/dartsim/src/SDFFeatures.hh @@ -35,6 +35,8 @@ #include #include +#include "KinematicJoint.hh" + #include "Base.hh" #include "EntityManagementFeatures.hh" From bb662206369ddd05590845adb8b6a7d32f1cabc8 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Thu, 24 Jul 2025 17:11:29 -0600 Subject: [PATCH 05/21] missing version contorl conflict solved Signed-off-by: Jose Mayoral --- dartsim/src/SDFFeatures.cc | 4 ---- 1 file changed, 4 deletions(-) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 1c5609ef9..1e8bd7e61 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -672,11 +672,7 @@ Identity SDFFeatures::ConstructSdfLink( const Eigen::Vector3d localCom = math::eigen3::convert(sdfInertia.Pose().Pos()); -<<<<<<< HEAD const bool isKinematic = _sdfLink.Kinematic(); -======= - const bool isKinematic = false; //TODO temporal fix for kinematic tag //_sdfLink.Kinematic(); ->>>>>>> 72e1caf4 (clean for tests) bodyProperties.mInertia.setLocalCOM(localCom); From 4f1ec0d3cfcbf85daa50a662bf3e290aae238345 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Wed, 6 Aug 2025 22:07:54 +0000 Subject: [PATCH 06/21] add test Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 200 +++++++++++++++++++++++++ 1 file changed, 200 insertions(+) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 3b5459bee..eec03c288 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -34,6 +34,7 @@ #include #include +#include #include template @@ -240,6 +241,205 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose) } } +// Kinematic Tag Test +using SetKinematicFeaturesList = gz::physics::FeatureList< + gz::physics::sdf::ConstructSdfModel, + gz::physics::sdf::ConstructSdfWorld, + gz::physics::ForwardStep, + gz::physics::GetLinkFromModel, + gz::physics::GetJointFromModel, + gz::physics::SetBasicJointState, + gz::physics::SetJointVelocityCommandFeature, + gz::physics::GetModelFromWorld, + gz::physics::LinkFrameSemantics +>; + +using SetKinematicTestFeaturesList = + KinematicFeaturesTest; + +TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + // currently only dartsim is supported + if (this->PhysicsEngineName(name) != "dartsim") + continue; + + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d:: + From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + sdf::Errors errors = root.Load(common_test::worlds::kEmptySdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + EXPECT_NE(nullptr, world); + + const std::string modelStr = R"( + + + 0 0 5.0 0 0 0 + + false + + + + + + 0.5 0.5 0.5 + + + + + + )"; + + errors = root.LoadSdfString(modelStr); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto model = world->GetModel("box"); + ASSERT_NE(nullptr, model); + auto link = model->GetLink("parent"); + ASSERT_NE(nullptr, link); + auto joint = model->GetJoint("joint"); + ASSERT_NE(nullptr, joint); + + // verify box initial state + gz::math::Pose3d initialPose(0, 0, 5, 0, 0, 0); + auto frameData = link->FrameDataRelativeToWorld(); + EXPECT_EQ(initialPose, gz::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.angularVelocity)); + + double time = 1.0; + double stepSize = 0.001; + std::size_t steps = static_cast(time / stepSize); + + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + for (std::size_t i = 0; i < steps; ++i) + { + world->Step(output, state, input); + } + // BOX does not falls + frameData = link->FrameDataRelativeToWorld(); + // Falling box falls at a spped of 1 m/s + gzerr << "AFTER " << frameData.pose.translation().x() << std::endl; + gzerr << "AFTER " << frameData.pose.translation().y() << std::endl; + gzerr << "AFTER " << frameData.pose.translation().z() << std::endl; + gzerr << "AFTER " << frameData.linearVelocity.x() << std::endl; + gzerr << "AFTER " << frameData.linearVelocity.y() << std::endl; + gzerr << "AFTER " << frameData.linearVelocity.z() << std::endl; + gzerr << "AFTER " << frameData.angularVelocity.x() << std::endl; + gzerr << "AFTER " << frameData.angularVelocity.y() << std::endl; + gzerr << "AFTER " << frameData.angularVelocity.z() << std::endl; + + // Verify the sphere did not move + EXPECT_NEAR(0.0, frameData.pose.translation().x(), 1e-3); + EXPECT_NEAR(0.0, frameData.pose.translation().y(), 1e-3); + + //Falls and should be close to the floor + EXPECT_NEAR(0.0, + frameData.pose.translation().z(), 1e-2); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.angularVelocity)); + } +} + +TEST_F(SetKinematicTestFeaturesList, SetTrueKinematic) +{ + for (const std::string &name : this->pluginNames) + { + std::cout << "Testing plugin: " << name << std::endl; + // currently only dartsim is supported + if (this->PhysicsEngineName(name) != "dartsim") + continue; + + gz::plugin::PluginPtr plugin = this->loader.Instantiate(name); + + auto engine = gz::physics::RequestEngine3d:: + From(plugin); + ASSERT_NE(nullptr, engine); + + sdf::Root root; + sdf::Errors errors = root.Load(common_test::worlds::kEmptySdf); + ASSERT_TRUE(errors.empty()) << errors.front(); + + auto world = engine->ConstructWorld(*root.WorldByIndex(0)); + EXPECT_NE(nullptr, world); + + const std::string modelStr = R"( + + + 0 0 5.0 0 0 0 + + true + + + + + + 0.5 0.5 0.5 + + + + + + )"; + + errors = root.LoadSdfString(modelStr); + ASSERT_TRUE(errors.empty()) << errors.front(); + ASSERT_NE(nullptr, root.Model()); + world->ConstructModel(*root.Model()); + + auto model = world->GetModel("box"); + ASSERT_NE(nullptr, model); + auto link = model->GetLink("parent"); + ASSERT_NE(nullptr, link); + auto joint = model->GetJoint("joint"); + ASSERT_NE(nullptr, joint); + + // verify box initial state + gz::math::Pose3d initialPose(0, 0, 5, 0, 0, 0); + auto frameData = link->FrameDataRelativeToWorld(); + EXPECT_EQ(initialPose, gz::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.angularVelocity)); + + double time = 1.0; + double stepSize = 0.001; + std::size_t steps = static_cast(time / stepSize); + gz::physics::ForwardStep::Input input; + gz::physics::ForwardStep::State state; + gz::physics::ForwardStep::Output output; + for (std::size_t i = 0; i < steps; ++i) + { + world->Step(output, state, input); + } + // BOX does not falls + frameData = link->FrameDataRelativeToWorld(); + EXPECT_EQ(initialPose, gz::math::eigen3::convert(frameData.pose)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.linearVelocity)); + EXPECT_EQ(gz::math::Vector3d::Zero, + gz::math::eigen3::convert(frameData.angularVelocity)); + + } +} + int main(int argc, char *argv[]) { ::testing::InitGoogleTest(&argc, argv); From e5f1a390bcb9bca0d7f1b8750c01def377870afe Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Wed, 6 Aug 2025 22:11:11 +0000 Subject: [PATCH 07/21] remove unused logger in test Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index eec03c288..1913e8275 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -330,19 +330,8 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) { world->Step(output, state, input); } - // BOX does not falls + // BOX falls frameData = link->FrameDataRelativeToWorld(); - // Falling box falls at a spped of 1 m/s - gzerr << "AFTER " << frameData.pose.translation().x() << std::endl; - gzerr << "AFTER " << frameData.pose.translation().y() << std::endl; - gzerr << "AFTER " << frameData.pose.translation().z() << std::endl; - gzerr << "AFTER " << frameData.linearVelocity.x() << std::endl; - gzerr << "AFTER " << frameData.linearVelocity.y() << std::endl; - gzerr << "AFTER " << frameData.linearVelocity.z() << std::endl; - gzerr << "AFTER " << frameData.angularVelocity.x() << std::endl; - gzerr << "AFTER " << frameData.angularVelocity.y() << std::endl; - gzerr << "AFTER " << frameData.angularVelocity.z() << std::endl; - // Verify the sphere did not move EXPECT_NEAR(0.0, frameData.pose.translation().x(), 1e-3); EXPECT_NEAR(0.0, frameData.pose.translation().y(), 1e-3); From 661be75d700ba1e639878c3e153d3fac8a9a154b Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Thu, 7 Aug 2025 21:26:53 +0000 Subject: [PATCH 08/21] add pipeline lint fixes Signed-off-by: Jose Mayoral --- dartsim/src/KinematicJoint.cc | 28 ---------------------------- dartsim/src/KinematicJoint.hh | 13 +++++-------- 2 files changed, 5 insertions(+), 36 deletions(-) diff --git a/dartsim/src/KinematicJoint.cc b/dartsim/src/KinematicJoint.cc index 12d35096f..b35ea5b36 100644 --- a/dartsim/src/KinematicJoint.cc +++ b/dartsim/src/KinematicJoint.cc @@ -38,7 +38,6 @@ using namespace gz::dynamics; using namespace dart::dynamics; using namespace dart::math; -//============================================================================== Eigen::Vector6d KinematicJoint::convertToPositions(const Eigen::Isometry3d& _tf) { Eigen::Vector6d x; @@ -47,7 +46,6 @@ Eigen::Vector6d KinematicJoint::convertToPositions(const Eigen::Isometry3d& _tf) return x; } -//============================================================================== Eigen::Isometry3d KinematicJoint::convertToTransform( const Eigen::Vector6d& _positions) { @@ -57,7 +55,6 @@ Eigen::Isometry3d KinematicJoint::convertToTransform( return tf; } -//============================================================================== void KinematicJoint::setTransformOf( Joint* joint, const Eigen::Isometry3d& tf, const Frame* withRespectTo) { @@ -78,7 +75,6 @@ void KinematicJoint::setTransformOf( kinematicJoint->setTransform(tf, withRespectTo); } -//============================================================================== void KinematicJoint::setTransformOf( BodyNode* bodyNode, const Eigen::Isometry3d& tf, const Frame* withRespectTo) { @@ -88,7 +84,6 @@ void KinematicJoint::setTransformOf( setTransformOf(bodyNode->getParentJoint(), tf, withRespectTo); } -//============================================================================== void KinematicJoint::setTransformOf( Skeleton* skeleton, const Eigen::Isometry3d& tf, @@ -110,7 +105,6 @@ void KinematicJoint::setTransformOf( } } -//============================================================================== void KinematicJoint::setSpatialMotion( const Eigen::Isometry3d* newTransform, const Frame* withRespectTo, @@ -125,7 +119,6 @@ void KinematicJoint::setSpatialMotion( setSpatialVelocity(*newSpatialVelocity, velRelativeTo, velInCoordinatesOf); } -//============================================================================== void KinematicJoint::setRelativeTransform(const Eigen::Isometry3d& newTransform) { setPositionsStatic(convertToPositions( @@ -133,14 +126,12 @@ void KinematicJoint::setRelativeTransform(const Eigen::Isometry3d& newTransform) * Joint::mAspectProperties.mT_ChildBodyToJoint)); } -//============================================================================== void KinematicJoint::setRelativeSpatialVelocity( const Eigen::Vector6d& newSpatialVelocity) { setVelocitiesStatic(newSpatialVelocity); } -//============================================================================== void KinematicJoint::setRelativeSpatialVelocity( const Eigen::Vector6d& newSpatialVelocity, const Frame* inCoordinatesOf) { @@ -156,7 +147,6 @@ void KinematicJoint::setRelativeSpatialVelocity( } } -//============================================================================== void KinematicJoint::setSpatialVelocity( const Eigen::Vector6d& newSpatialVelocity, const Frame* relativeTo, @@ -197,7 +187,6 @@ void KinematicJoint::setSpatialVelocity( setRelativeSpatialVelocity(targetRelSpatialVel); } -//============================================================================== void KinematicJoint::setTransform( const Eigen::Isometry3d& newTransform, const Frame* withRespectTo) { @@ -208,7 +197,6 @@ void KinematicJoint::setTransform( * newTransform); } -//============================================================================== void KinematicJoint::setLinearVelocity( const Eigen::Vector3d& newLinearVelocity, const Frame* relativeTo) { @@ -230,7 +218,6 @@ void KinematicJoint::setLinearVelocity( setSpatialVelocity(targetSpatialVelocity, relativeTo, getChildBodyNode()); } -//============================================================================== void KinematicJoint::setAngularVelocity( const Eigen::Vector3d& newAngularVelocity, const Frame* relativeTo, @@ -261,14 +248,12 @@ void KinematicJoint::setAngularVelocity( 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 { @@ -278,7 +263,6 @@ Eigen::Vector6d KinematicJoint::getPositionDifferencesStatic( return convertToPositions(T1.inverse() * T2); } -//============================================================================== KinematicJoint::KinematicJoint(const Properties& properties) : Base(properties), mQ(Eigen::Isometry3d::Identity()) @@ -291,33 +275,28 @@ KinematicJoint::KinematicJoint(const Properties& properties) createJointAspect(properties); } -//============================================================================== Joint* KinematicJoint::clone() const { return new KinematicJoint(getGenericJointProperties()); } -//============================================================================== 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 @@ -329,20 +308,17 @@ void KinematicJoint::integratePositions(double _dt) setPositionsStatic(convertToPositions(Qnext)); } -//============================================================================== void KinematicJoint::integrateVelocities(double /*_dt*/) { setVelocitiesStatic(getVelocitiesStatic()); } -//============================================================================== void KinematicJoint::updateConstrainedTerms(double /*timeStep*/) { const Eigen::Vector6d& velBefore = getVelocitiesStatic(); setVelocitiesStatic(velBefore); } -//==============================================ss================================ void KinematicJoint::updateDegreeOfFreedomNames() { static const char* suffixes[6] @@ -354,7 +330,6 @@ void KinematicJoint::updateDegreeOfFreedomNames() } } -//============================================================================== void KinematicJoint::updateRelativeTransform() const { mQ = convertToTransform(getPositionsStatic()); @@ -365,7 +340,6 @@ void KinematicJoint::updateRelativeTransform() const assert(verifyTransform(mT)); } -//============================================================================== void KinematicJoint::updateRelativeJacobian(bool _mandatory) const { if (_mandatory) @@ -373,13 +347,11 @@ void KinematicJoint::updateRelativeJacobian(bool _mandatory) const = getAdTMatrix(Joint::mAspectProperties.mT_ChildBodyToJoint); } -//============================================================================== void KinematicJoint::updateRelativeJacobianTimeDeriv() const { assert(Eigen::Matrix6d::Zero() == mJacobianDeriv); } -//============================================================================== const Eigen::Isometry3d& KinematicJoint::getQ() const { if (mNeedTransformUpdate) { diff --git a/dartsim/src/KinematicJoint.hh b/dartsim/src/KinematicJoint.hh index 141d981f0..eba328525 100644 --- a/dartsim/src/KinematicJoint.hh +++ b/dartsim/src/KinematicJoint.hh @@ -34,7 +34,6 @@ #define GZ_DYNAMICS_KINEMATICJOINT_HPP_ #include -//#include "GenericJoint.hpp" #include #include @@ -62,8 +61,6 @@ public: /// Destructor virtual ~KinematicJoint() = default; - /// Get the Properties of this KinematicJoint - //Properties getKinematicJointProperties() const; // Documentation inherited const std::string& getType() const override; @@ -211,7 +208,7 @@ public: /// Constructor called by Skeleton class // This must be public so that Skeleton can create KinematicJoints - KinematicJoint(const Properties& properties); + explicit KinematicJoint(const Properties& properties); protected: @@ -246,7 +243,6 @@ protected: const Eigen::Isometry3d& getQ() const; /// Transformation matrix dependent on generalized coordinates - /// /// Do not use directly! Use getQ() to access this mutable Eigen::Isometry3d mQ; @@ -254,7 +250,8 @@ public: // To get byte-aligned Eigen vectors EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; -} // namespace dynamics -} // namespace dart +} +} + +#endif -#endif // DART_DYNAMICS_KinematicJoint_HPP_ \ No newline at end of file From 717b44e3cf81323f44aad5c341053ff5c1a8bc5c Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Thu, 7 Aug 2025 22:10:37 +0000 Subject: [PATCH 09/21] removing unused getJoints Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 4 ---- 1 file changed, 4 deletions(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 1913e8275..f426f6d30 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -307,8 +307,6 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) ASSERT_NE(nullptr, model); auto link = model->GetLink("parent"); ASSERT_NE(nullptr, link); - auto joint = model->GetJoint("joint"); - ASSERT_NE(nullptr, joint); // verify box initial state gz::math::Pose3d initialPose(0, 0, 5, 0, 0, 0); @@ -396,8 +394,6 @@ TEST_F(SetKinematicTestFeaturesList, SetTrueKinematic) ASSERT_NE(nullptr, model); auto link = model->GetLink("parent"); ASSERT_NE(nullptr, link); - auto joint = model->GetJoint("joint"); - ASSERT_NE(nullptr, joint); // verify box initial state gz::math::Pose3d initialPose(0, 0, 5, 0, 0, 0); From 819f564e49ceaa39034e37d3c62d70bb2edf2337 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Thu, 7 Aug 2025 22:31:13 +0000 Subject: [PATCH 10/21] give enough time for model to fall Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index f426f6d30..756243584 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -317,7 +317,7 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) EXPECT_EQ(gz::math::Vector3d::Zero, gz::math::eigen3::convert(frameData.angularVelocity)); - double time = 1.0; + double time = 2.0; double stepSize = 0.001; std::size_t steps = static_cast(time / stepSize); From b7b87e041fcc8e522acf3be0580849dfbda2309e Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Thu, 7 Aug 2025 22:54:17 +0000 Subject: [PATCH 11/21] change test condition to expect_lt Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 756243584..c8fdbcb7d 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -334,8 +334,8 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) EXPECT_NEAR(0.0, frameData.pose.translation().x(), 1e-3); EXPECT_NEAR(0.0, frameData.pose.translation().y(), 1e-3); - //Falls and should be close to the floor - EXPECT_NEAR(0.0, + //Falls and should be below the floor + EXPECT_LT(0.0, frameData.pose.translation().z(), 1e-2); EXPECT_EQ(gz::math::Vector3d::Zero, gz::math::eigen3::convert(frameData.linearVelocity)); From 7dc953b6c5f837300d1325cc834e587653eda4fe Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 8 Aug 2025 13:05:46 +0000 Subject: [PATCH 12/21] calculating expected_z for test Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index c8fdbcb7d..b6361e324 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -334,8 +334,10 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) EXPECT_NEAR(0.0, frameData.pose.translation().x(), 1e-3); EXPECT_NEAR(0.0, frameData.pose.translation().y(), 1e-3); - //Falls and should be below the floor - EXPECT_LT(0.0, + // Falls and should be below the floor + // z0 - 1/2 * g * t^2 + double expected_z = 5 - (0.5 * 9.81 * 4); + EXPECT_NEAR(expected_z, frameData.pose.translation().z(), 1e-2); EXPECT_EQ(gz::math::Vector3d::Zero, gz::math::eigen3::convert(frameData.linearVelocity)); From 938dea43b798b6108bf71a371964b71da8763a66 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 8 Aug 2025 13:38:21 +0000 Subject: [PATCH 13/21] fix gravity value to match test Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index b6361e324..d21e87c46 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -336,7 +336,7 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) // Falls and should be below the floor // z0 - 1/2 * g * t^2 - double expected_z = 5 - (0.5 * 9.81 * 4); + double expected_z = 5 - (0.5 * 9.8 * 4); EXPECT_NEAR(expected_z, frameData.pose.translation().z(), 1e-2); EXPECT_EQ(gz::math::Vector3d::Zero, From da948e3cc9882d6b793057f3ba60fedc49ef4e5c Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 8 Aug 2025 14:11:54 +0000 Subject: [PATCH 14/21] adding expected velocity Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index d21e87c46..3640a2a15 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -336,10 +336,13 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) // Falls and should be below the floor // z0 - 1/2 * g * t^2 - double expected_z = 5 - (0.5 * 9.8 * 4); + double expected_z = 5 - (0.5 * 9.8 * time * time); + double expected_vz = - 9.8 * time; + gz::physics::Vector3d expected_v(0.0, 0.0, expected_vz); + EXPECT_NEAR(expected_z, frameData.pose.translation().z(), 1e-2); - EXPECT_EQ(gz::math::Vector3d::Zero, + EXPECT_EQ(expected_v, gz::math::eigen3::convert(frameData.linearVelocity)); EXPECT_EQ(gz::math::Vector3d::Zero, gz::math::eigen3::convert(frameData.angularVelocity)); From fb858e46f3b07a9d09b1b5c85355fc11bfa692c0 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 8 Aug 2025 14:19:45 +0000 Subject: [PATCH 15/21] vector3d type change in test Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 3640a2a15..02ac4a202 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -338,7 +338,7 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) // z0 - 1/2 * g * t^2 double expected_z = 5 - (0.5 * 9.8 * time * time); double expected_vz = - 9.8 * time; - gz::physics::Vector3d expected_v(0.0, 0.0, expected_vz); + gz::math::Vector3d expected_v(0.0, 0.0, expected_vz); EXPECT_NEAR(expected_z, frameData.pose.translation().z(), 1e-2); From cd45f509450226026b095ec4d779bdcd8d47461f Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 8 Aug 2025 14:27:57 +0000 Subject: [PATCH 16/21] test convert for gz::math::Vector Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 02ac4a202..6c03f2f32 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -342,7 +342,7 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) EXPECT_NEAR(expected_z, frameData.pose.translation().z(), 1e-2); - EXPECT_EQ(expected_v, + EXPECT_EQ(gz::math::eigen::convert(expected_v), gz::math::eigen3::convert(frameData.linearVelocity)); EXPECT_EQ(gz::math::Vector3d::Zero, gz::math::eigen3::convert(frameData.angularVelocity)); From aa3d940b4a91ae31db9e0db3e652f22467f4e347 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 8 Aug 2025 14:34:21 +0000 Subject: [PATCH 17/21] typo on convert function Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 6c03f2f32..6f788c687 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -342,7 +342,7 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) EXPECT_NEAR(expected_z, frameData.pose.translation().z(), 1e-2); - EXPECT_EQ(gz::math::eigen::convert(expected_v), + EXPECT_EQ(gz::math::eigen3::convert(expected_v), gz::math::eigen3::convert(frameData.linearVelocity)); EXPECT_EQ(gz::math::Vector3d::Zero, gz::math::eigen3::convert(frameData.angularVelocity)); From 0f25f784554b7b41132bef4ffa5a0158c441f220 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 8 Aug 2025 14:44:55 +0000 Subject: [PATCH 18/21] test just 1d velocity Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index 6f788c687..bfcccdc84 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -342,8 +342,7 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) EXPECT_NEAR(expected_z, frameData.pose.translation().z(), 1e-2); - EXPECT_EQ(gz::math::eigen3::convert(expected_v), - gz::math::eigen3::convert(frameData.linearVelocity)); + EXPECT_NEAR(expectedVelZ, frameData.linearVelocity.z(), 1e-2); EXPECT_EQ(gz::math::Vector3d::Zero, gz::math::eigen3::convert(frameData.angularVelocity)); } From 680692c5fa62687036702a30fe571b6a8ab3afe5 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Fri, 8 Aug 2025 14:52:34 +0000 Subject: [PATCH 19/21] rename variable Signed-off-by: Jose Mayoral --- test/common_test/kinematic_features.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc index bfcccdc84..22b25d916 100644 --- a/test/common_test/kinematic_features.cc +++ b/test/common_test/kinematic_features.cc @@ -342,7 +342,7 @@ TEST_F(SetKinematicTestFeaturesList, SetFalseKinematic) EXPECT_NEAR(expected_z, frameData.pose.translation().z(), 1e-2); - EXPECT_NEAR(expectedVelZ, frameData.linearVelocity.z(), 1e-2); + EXPECT_NEAR(expected_vz, frameData.linearVelocity.z(), 1e-2); EXPECT_EQ(gz::math::Vector3d::Zero, gz::math::eigen3::convert(frameData.angularVelocity)); } From 8eff42ec644c5d756dc6313350273d6c8a64d9d8 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 19 Aug 2025 09:34:36 -0600 Subject: [PATCH 20/21] fix ci pipeline Signed-off-by: Jose Mayoral --- dartsim/src/KinematicJoint.cc | 8 ++++---- dartsim/src/KinematicJoint.hh | 4 +--- dartsim/src/SDFFeatures.cc | 19 ++++++++++--------- 3 files changed, 15 insertions(+), 16 deletions(-) diff --git a/dartsim/src/KinematicJoint.cc b/dartsim/src/KinematicJoint.cc index b35ea5b36..d0d61529a 100644 --- a/dartsim/src/KinematicJoint.cc +++ b/dartsim/src/KinematicJoint.cc @@ -249,7 +249,7 @@ void KinematicJoint::setAngularVelocity( } Eigen::Matrix6d KinematicJoint::getRelativeJacobianStatic( - const Eigen::Vector6d& /*positions*/) const + const Eigen::Vector6d& positions) const { return mJacobian; } @@ -264,7 +264,7 @@ Eigen::Vector6d KinematicJoint::getPositionDifferencesStatic( } KinematicJoint::KinematicJoint(const Properties& properties) - : Base(properties), + : Base(properties), mQ(Eigen::Isometry3d::Identity()) { mJacobianDeriv = Eigen::Matrix6d::Zero(); @@ -308,12 +308,12 @@ void KinematicJoint::integratePositions(double _dt) setPositionsStatic(convertToPositions(Qnext)); } -void KinematicJoint::integrateVelocities(double /*_dt*/) +void KinematicJoint::integrateVelocities(double _dt) { setVelocitiesStatic(getVelocitiesStatic()); } -void KinematicJoint::updateConstrainedTerms(double /*timeStep*/) +void KinematicJoint::updateConstrainedTerms(double timeStep) { const Eigen::Vector6d& velBefore = getVelocitiesStatic(); setVelocitiesStatic(velBefore); diff --git a/dartsim/src/KinematicJoint.hh b/dartsim/src/KinematicJoint.hh index eba328525..5ac201c36 100644 --- a/dartsim/src/KinematicJoint.hh +++ b/dartsim/src/KinematicJoint.hh @@ -100,7 +100,7 @@ public: // void setTransform( - const Eigen::Isometry3d& newTransform, + const Eigen::Isometry3d& newTransform, const Frame* withRespectTo = Frame::World()); /// Apply setTransform(bodyNode, tf, withRespecTo) for all the root BodyNodes @@ -215,8 +215,6 @@ protected: // Documentation inherited Joint* clone() const override; - //using Base::getRelativeJacobianStatic; - // Documentation inherited void integratePositions(double _dt) override; diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index fee79dfdd..039fd51e4 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -717,8 +717,8 @@ Identity SDFFeatures::ConstructSdfLink( bodyProperties.mInertia.setMass(sdfInertia.MassMatrix().Mass()); bodyProperties.mGravityMode = _sdfLink.EnableGravity(); bodyProperties.mInertia.setMoment(I_link); - - bodyProperties.mInertia.setLocalCOM(localCom); + + bodyProperties.mInertia.setLocalCOM(localCom); bodyProperties.mFrictionCoeff = 0; const Eigen::Isometry3d tf = @@ -734,17 +734,18 @@ Identity SDFFeatures::ConstructSdfLink( auto const joint = result.first; joint->setTransform(tf); - + bn = result.second; } - - else{ + + else + { // Note: When constructing a link from this function, we always instantiate - // it as a standalone free body within the model. If it should have any joint - // constraints, those will be added later. + // it as a standalone free body within the model. If it should have any + // joint constraints, those will be added later. - // TODO(MXG): Consider adding a UUID to this joint name in order to avoid any - // potential (albeit unlikely) name collisions. + // TODO(MXG): Consider adding a UUID to this joint name in order to avoid + // any sspotential (albeit unlikely) name collisions. auto result = modelInfo.model->createJointAndBodyNodePair< dart::dynamics::FreeJoint>(nullptr, jointProperties, bodyProperties); From dc1efacd0aadf2dba79b06de892fa40e2a40a9d2 Mon Sep 17 00:00:00 2001 From: Jose Mayoral Date: Tue, 19 Aug 2025 12:09:52 -0600 Subject: [PATCH 21/21] missing correction for pipeline Signed-off-by: Jose Mayoral --- dartsim/src/SDFFeatures.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 039fd51e4..92e468845 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -732,7 +732,7 @@ Identity SDFFeatures::ConstructSdfLink( auto result = modelInfo.model->createJointAndBodyNodePair< gz::dynamics::KinematicJoint>(nullptr, jointProperties, bodyProperties); - auto const joint = result.first; + auto const joint = result.first; joint->setTransform(tf); bn = result.second;