w

dartsim · Mar 29, 2024 · 3014018 · 3014018
1 parent 3026b57
commit 3014018
Show file tree

Hide file tree

Showing 3 changed files with 119 additions and 34 deletions.
diff --git a/dart/dynamics/RigidBody.cpp b/dart/dynamics/RigidBody.cpp
@@ -37,29 +37,45 @@
 #include "dart/dynamics/FreeJoint.hpp"
 #include "dart/dynamics/ShapeFrame.hpp"
 #include "dart/dynamics/ShapeNode.hpp"
+#include "dart/dynamics/SphereShape.hpp"
 
 namespace dart::dynamics {
 
-SkeletonPtr RigidBody::create(const Config& config)
+std::shared_ptr<RigidBody> RigidBody::create(const Config& config)
 {
-  return SkeletonPtr(new RigidBody(config));
+  return std::make_shared<RigidBody>(config);
 }
 
-RigidBody::RigidBody(const Config& config) : Skeleton(config.name)
+RigidBody::RigidBody(const Config& config)
 {
-  auto jointAndBody = this->createJointAndBodyNodePair<dynamics::FreeJoint>();
-  auto body = jointAndBody.second;
-  auto shapeNode = body->createShapeNodeWith<
+  mSkeleton = Skeleton::create(config.name);
+  DART_ASSERT(mSkeleton);
+
+  std::tie(mParentJoint, mBodyNode)
+      = mSkeleton->createJointAndBodyNodePair<FreeJoint>();
+  DART_ASSERT(mParentJoint);
+  DART_ASSERT(mBodyNode);
+
+  // TODO: Add shape factory by the string type
+  if (config.shapeType == BoxShape::getStaticType()) {
+    mShape
+        = std::make_shared<dynamics::BoxShape>(Eigen::Vector3d(0.5, 0.5, 0.5));
+  } else {
+    mShape = std::make_shared<dynamics::SphereShape>(0.5);
+  }
+  DART_ASSERT(mShape);
+
+  mShapeNode = mBodyNode->createShapeNodeWith<
       dynamics::VisualAspect,
       dynamics::CollisionAspect,
-      dynamics::DynamicsAspect>(
-      std::make_shared<dynamics::BoxShape>(Eigen::Vector3d(0.3, 0.3, 0.3)));
-  (void)shapeNode;
+      dynamics::DynamicsAspect>(mShape);
+  DART_ASSERT(mShapeNode);
+  mShapeNode->setShape(nullptr);
 }
 
 RigidBody::~RigidBody()
 {
-  // Do nothing
+  // Empty
 }
 
 } // namespace dart::dynamics
diff --git a/dart/dynamics/RigidBody.hpp b/dart/dynamics/RigidBody.hpp
@@ -32,6 +32,8 @@
 
 #pragma once
 
+#include <dart/dynamics/BoxShape.hpp>
+#include <dart/dynamics/FreeJoint.hpp>
 #include <dart/dynamics/Skeleton.hpp>
 
 namespace dart::dynamics {
@@ -40,27 +42,103 @@ struct RigidBodyConfig
 {
   std::string name = "RigidBody";
   Eigen::Vector3d position = Eigen::Vector3d::Zero();
+  std::string shapeType = BoxShape::getStaticType();
 
   RigidBodyConfig(const std::string& name = "RigidBody") : name(name)
   {
     // Empty
   }
 };
 
-class RigidBody : public Skeleton
+class RigidBody
 {
 public:
   using Config = RigidBodyConfig;
 
-  static SkeletonPtr create(const Config& config = Config());
+  static std::shared_ptr<RigidBody> create(const Config& config = Config());
 
   /// Constructor
   explicit RigidBody(const Config& config = Config());
 
   /// Destructor
-  ~RigidBody() override;
+  virtual ~RigidBody();
 
-  void setPosition(const Eigen::Vector3d& position);
+  template <typename Derived>
+  void setPosition(const Eigen::MatrixBase<Derived>& position)
+  {
+    auto tf = mBodyNode->getTransform();
+    tf.translation() = position;
+    FreeJoint::setTransformOf(mParentJoint, tf);
+  }
+
+  [[nodiscard]] Eigen::Vector3d getPosition() const
+  {
+    return mBodyNode->getTransform().translation();
+  }
+
+  template <typename Derived>
+  void setOrientation(const Eigen::MatrixBase<Derived>& rotationMap)
+  {
+    auto tf = mBodyNode->getTransform();
+    tf.linear() = rotationMap;
+    FreeJoint::setTransformOf(mParentJoint, tf);
+  }
+
+  [[nodiscard]] Eigen::Matrix3d getOrientation() const
+  {
+    return mBodyNode->getTransform().linear();
+  }
+
+  void setInertia(const Eigen::Matrix3d& inertia)
+  {
+    const auto& inertial = mBodyNode->getInertia();
+    mBodyNode->setInertia(
+        Inertia(inertial.getMass(), inertial.getLocalCOM(), inertia));
+  }
+
+  ConstShapePtr getShape() const
+  {
+    return mShape;
+  }
+
+  ShapePtr getShape()
+  {
+    return mShape;
+  }
+
+  void setColor(const Eigen::Vector3d& rgb)
+  {
+    mShapeNode->getVisualAspect()->setColor(rgb);
+  }
+
+  SkeletonPtr getSkeleton()
+  {
+    return mSkeleton;
+  }
+
+  ConstSkeletonPtr getSkeleton() const
+  {
+    return mSkeleton;
+  }
+
+  void setStatic(bool value = true) {
+    if (value) {
+      mParentJoint->setActuatorType(Joint::LOCKED);
+    } else {
+      mParentJoint->setActuatorType(Joint::FORCE);
+    }
+  }
+
+  [[nodiscard]] bool isStatic() const {
+    return mParentJoint->isKinematic();
+  }
+
+private:
+  SkeletonPtr mSkeleton;
+  BodyNode* mBodyNode;
+  FreeJoint* mParentJoint;
+  ShapeNodePtr mShapeNode;
+  ShapePtr mShape;
 };
 
-} // namespace dart::dynamics
+} // namespace dart::dynamics
diff --git a/examples/hello_world/main.cpp b/examples/hello_world/main.cpp
@@ -41,26 +41,17 @@ using namespace dart;
 int main()
 {
   // Create a box-shaped rigid body
-  auto skeleton = dynamics::Skeleton::create();
-  auto jointAndBody
-      = skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>();
-  auto body = jointAndBody.second;
-  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
-  tf.translation() << 0, 0, 1;
-  tf.linear() = math::expMapRot(Eigen::Vector3d::Random());
-  body->getParentJoint()->setTransformFromParentBodyNode(tf);
-  auto shapeNode = body->createShapeNodeWith<
-      dynamics::VisualAspect,
-      dynamics::CollisionAspect,
-      dynamics::DynamicsAspect>(
-      std::make_shared<dynamics::BoxShape>(Eigen::Vector3d(0.3, 0.3, 0.3)));
-  body->setInertia(dynamics::Inertia(
-      1, Eigen::Vector3d::Zero(), shapeNode->getShape()->computeInertia(1.0)));
-  shapeNode->getVisualAspect()->setColor(dart::Color::Blue());
-  auto body2 = dynamics::RigidBody::create();
-  (void)body2;
+  auto box = dynamics::RigidBody::create(dynamics::RigidBodyConfig("box"));
+  box->setPosition(Eigen::Vector3d(0, 0, 1));
+  box->setOrientation(math::expMapRot(Eigen::Vector3d::Random()));
+  box->setInertia(box->getShape()->computeInertia(1.0));
+  box->setColor(Color::Blue());
 
   // Create ground
+  // auto ground = dynamics::RigidBody::create("ground");
+  // ground->setColor(Color::LightGray());
+  // ground->setStatic();
+
   auto ground = dynamics::Skeleton::create("ground");
   auto groundBody
       = ground->createJointAndBodyNodePair<dynamics::WeldJoint>().second;
@@ -73,7 +64,7 @@ int main()
 
   // Create a world and add the rigid body and ground to the world
   auto world = simulation::World::create();
-  world->addSkeleton(skeleton);
+  world->addSkeleton(box->getSkeleton());
   world->addSkeleton(ground);
 
   // Wrap a WorldNode around it