add fixes

Signed-off-by: Jose Mayoral <jocamaba1989@gmail.com>

Author: jcmayoral

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 >=)

dartsim/src/FreeGroupFeatures.cc

@@ -18,6 +18,7 @@
 #include "FreeGroupFeatures.hh"
 
 #include <dart/constraint/ConstraintSolver.hpp>
+#include <dart/dynamics/KinematicJoint.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
 #include <gz/common/Console.hh>
 
@@ -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;
     }

dartsim/src/SDFFeatures.cc

@@ -31,6 +31,7 @@
 #include <dart/dynamics/CylinderShape.hpp>
 #include <dart/dynamics/EllipsoidShape.hpp>
 #include <dart/dynamics/FreeJoint.hpp>
+#include <dart/dynamics/KinematicJoint.hpp>
 #include <dart/dynamics/HeightmapShape.hpp>
 #include <dart/dynamics/MeshShape.hpp>
 #include <dart/dynamics/PlaneShape.hpp>
@@ -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 ["