From 30140180555a22d0b20674c2618de46e3a910b12 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 28 Mar 2024 22:44:28 -0700 Subject: [PATCH] w --- dart/dynamics/RigidBody.cpp | 36 ++++++++++---- dart/dynamics/RigidBody.hpp | 88 +++++++++++++++++++++++++++++++++-- examples/hello_world/main.cpp | 29 ++++-------- 3 files changed, 119 insertions(+), 34 deletions(-) diff --git a/dart/dynamics/RigidBody.cpp b/dart/dynamics/RigidBody.cpp index f275475041388..3a8b943677471 100644 --- 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::create(const Config& config) { - return SkeletonPtr(new RigidBody(config)); + return std::make_shared(config); } -RigidBody::RigidBody(const Config& config) : Skeleton(config.name) +RigidBody::RigidBody(const Config& config) { - auto jointAndBody = this->createJointAndBodyNodePair(); - auto body = jointAndBody.second; - auto shapeNode = body->createShapeNodeWith< + mSkeleton = Skeleton::create(config.name); + DART_ASSERT(mSkeleton); + + std::tie(mParentJoint, mBodyNode) + = mSkeleton->createJointAndBodyNodePair(); + DART_ASSERT(mParentJoint); + DART_ASSERT(mBodyNode); + + // TODO: Add shape factory by the string type + if (config.shapeType == BoxShape::getStaticType()) { + mShape + = std::make_shared(Eigen::Vector3d(0.5, 0.5, 0.5)); + } else { + mShape = std::make_shared(0.5); + } + DART_ASSERT(mShape); + + mShapeNode = mBodyNode->createShapeNodeWith< dynamics::VisualAspect, dynamics::CollisionAspect, - dynamics::DynamicsAspect>( - std::make_shared(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 index 05ca1e5f9963b..6c23afe7aaacc 100644 --- a/dart/dynamics/RigidBody.hpp +++ b/dart/dynamics/RigidBody.hpp @@ -32,6 +32,8 @@ #pragma once +#include +#include #include namespace dart::dynamics { @@ -40,6 +42,7 @@ 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) { @@ -47,20 +50,95 @@ struct RigidBodyConfig } }; -class RigidBody : public Skeleton +class RigidBody { public: using Config = RigidBodyConfig; - static SkeletonPtr create(const Config& config = Config()); + static std::shared_ptr create(const Config& config = Config()); /// Constructor explicit RigidBody(const Config& config = Config()); /// Destructor - ~RigidBody() override; + virtual ~RigidBody(); - void setPosition(const Eigen::Vector3d& position); + template + void setPosition(const Eigen::MatrixBase& position) + { + auto tf = mBodyNode->getTransform(); + tf.translation() = position; + FreeJoint::setTransformOf(mParentJoint, tf); + } + + [[nodiscard]] Eigen::Vector3d getPosition() const + { + return mBodyNode->getTransform().translation(); + } + + template + void setOrientation(const Eigen::MatrixBase& 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 \ No newline at end of file +} // namespace dart::dynamics diff --git a/examples/hello_world/main.cpp b/examples/hello_world/main.cpp index ab6174590c025..833408894f05c 100644 --- 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(); - 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(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().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