dartpy 7.0.0.dev0__cp312-cp312-manylinux_2_39_x86_64.whl

share/doc/dart/examples/tinkertoy/TinkertoyWorldNode.hpp

@@ -0,0 +1,470 @@
+/*
+ * Copyright (c) 2011-2025, 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_EXAMPLE_OSG_OSGATLASSIMBICON_TINKERTOYWORLDNODE_HPP_
+#define DART_EXAMPLE_OSG_OSGATLASSIMBICON_TINKERTOYWORLDNODE_HPP_
+
+#include <dart/gui/osg/osg.hpp>
+
+#include <dart/utils/utils.hpp>
+
+#include <dart/dart.hpp>
+
+const double DefaultBlockLength = 0.5;
+const double DefaultBlockWidth = 0.075;
+const double DefaultJointRadius = 1.5 * DefaultBlockWidth / 2.0;
+const double BalsaWoodDensity = 0.16 * 10e3; // kg/m^3
+const double DefaultBlockMass
+    = BalsaWoodDensity * DefaultBlockLength * pow(DefaultBlockWidth, 2);
+const double DefaultDamping = 0.4;
+
+const Eigen::Vector4d DefaultSimulationColor
+    = Eigen::Vector4d(0.5, 0.5, 1.0, 1.0);
+const Eigen::Vector4d DefaultPausedColor
+    = Eigen::Vector4d(0xEE, 0xC9, 0x00, 0.0) / 255.0
+      + Eigen::Vector4d(0, 0, 0, 1.0);
+const Eigen::Vector4d DefaultSelectedColor = dart::Color::Red(1.0);
+const Eigen::Vector4d DefaultForceBodyColor = dart::Color::Fuchsia(1.0);
+const Eigen::Vector4d DefaultForceLineColor
+    = Eigen::Vector4d(1.0, 0.63, 0.0, 1.0);
+
+const double MaxForce = 200.0;
+const double DefaultForceCoeff = 100.0;
+const double MaxForceCoeff = 1000.0;
+const double MinForceCoeff = 10.0;
+const double ForceIncrement = 10.0;
+
+//==============================================================================
+class TinkertoyWorldNode : public dart::gui::osg::RealTimeWorldNode
+{
+public:
+  TinkertoyWorldNode(const dart::simulation::WorldPtr& world)
+    : dart::gui::osg::RealTimeWorldNode(world),
+      mForceCoeff(DefaultForceCoeff),
+      mWasSimulating(false)
+  {
+    mTarget = dart::gui::osg::InteractiveFrame::createShared(
+        dart::dynamics::Frame::World());
+    getWorld()->addSimpleFrame(mTarget);
+
+    createShapes();
+    createInitialToy1();
+    createInitialToy2();
+    createForceLine();
+  }
+
+  void setAllBodyColors(const Eigen::Vector4d& color)
+  {
+    getWorld()->eachSkeleton(
+        [&](dart::dynamics::Skeleton* skel) { skel->setColor(color); });
+  }
+
+  void setPickedNodeColor(const Eigen::Vector4d& color)
+  {
+    if (!mPickedNode)
+      return;
+
+    mPickedNode->setColor(color);
+  }
+
+  void resetForceLine()
+  {
+    if (mPickedNode) {
+      mForceLine->setVertex(0, mPickedNode->getWorldTransform() * mPickedPoint);
+      mForceLine->setVertex(1, mTarget->getWorldTransform().translation());
+    } else {
+      mForceLine->setVertex(0, Eigen::Vector3d::Zero());
+      mForceLine->setVertex(1, Eigen::Vector3d::Zero());
+    }
+  }
+
+  void customPreRefresh() override
+  {
+    if (isSimulating()) {
+      setAllBodyColors(DefaultSimulationColor);
+      setPickedNodeColor(DefaultForceBodyColor);
+    } else {
+      setAllBodyColors(DefaultPausedColor);
+      setPickedNodeColor(DefaultSelectedColor);
+    }
+
+    resetForceLine();
+  }
+
+  void customPreStep() override
+  {
+    if (mPickedNode) {
+      Eigen::Vector3d F = mForceCoeff
+                          * (mTarget->getWorldTransform().translation()
+                             - mPickedNode->getWorldTransform() * mPickedPoint);
+
+      const double F_norm = F.norm();
+      if (F_norm > MaxForce)
+        F = MaxForce * F / F_norm;
+
+      mPickedNode->addExtForce(F, mPickedPoint);
+    }
+  }
+
+  void handlePick(const dart::gui::osg::PickInfo& pick)
+  {
+    dart::dynamics::BodyNode* bn
+        = dynamic_cast<dart::dynamics::BodyNode*>(pick.frame->getParentFrame());
+
+    if (!bn)
+      return;
+
+    mPickedNode = bn;
+    mPickedPoint = bn->getWorldTransform().inverse() * pick.position;
+
+    Eigen::Isometry3d tf = bn->getWorldTransform();
+    tf.translation()
+        = pick.position + pick.normal.normalized() * DefaultBlockWidth / 2.0;
+
+    mTarget->setTransform(tf);
+  }
+
+  void clearPick()
+  {
+    mPickedNode = nullptr;
+    mTarget->setTransform(Eigen::Isometry3d::Identity());
+  }
+
+  void deletePick()
+  {
+    if (!mPickedNode)
+      return;
+
+    if (isSimulating()) {
+      std::cout << " -- Please pause simulation [using the Spacebar] before "
+                << "attempting to delete blocks." << std::endl;
+      return;
+    }
+
+    dart::dynamics::SkeletonPtr temporary = mPickedNode->remove();
+    for (size_t i = 0; i < temporary->getNumBodyNodes(); ++i) {
+      mViewer->disableDragAndDrop(
+          mViewer->enableDragAndDrop(temporary->getBodyNode(i)));
+    }
+
+    getWorld()->getConstraintSolver()->getCollisionGroup()->removeShapeFramesOf(
+        temporary.get());
+
+    clearPick();
+  }
+
+  void createShapes()
+  {
+    createWeldJointShape();
+    createRevoluteJointShape();
+    createBallJointShape();
+    createBlockShape();
+  }
+
+  void createWeldJointShape()
+  {
+    mWeldJointShape
+        = std::make_shared<dart::dynamics::BoxShape>(Eigen::Vector3d(
+            2.0 * DefaultJointRadius, DefaultBlockWidth, DefaultBlockWidth));
+
+    mWeldJointShape->addDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR);
+  }
+
+  void createRevoluteJointShape()
+  {
+    mRevoluteJointShape = std::make_shared<dart::dynamics::CylinderShape>(
+        DefaultJointRadius, 1.5 * DefaultBlockWidth);
+
+    mRevoluteJointShape->addDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR);
+  }
+
+  void createBallJointShape()
+  {
+    mBallJointShape
+        = std::make_shared<dart::dynamics::SphereShape>(DefaultJointRadius);
+
+    mBallJointShape->addDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR);
+  }
+
+  void createBlockShape()
+  {
+    mBlockShape = std::make_shared<dart::dynamics::BoxShape>(Eigen::Vector3d(
+        DefaultBlockLength, DefaultBlockWidth, DefaultBlockWidth));
+
+    mBlockShape->addDataVariance(dart::dynamics::Shape::DYNAMIC_COLOR);
+
+    mBlockOffset = Eigen::Isometry3d::Identity();
+    mBlockOffset.translation()[0] = DefaultBlockLength / 2.0;
+  }
+
+  template <class JointType>
+  std::pair<JointType*, dart::dynamics::BodyNode*> addBlock(
+      dart::dynamics::BodyNode* parent,
+      const Eigen::Isometry3d& relTf,
+      const dart::dynamics::ShapePtr& jointShape)
+  {
+    if (isSimulating()) {
+      std::cout << " -- Please pause simulation [using the Spacebar] before "
+                << "attempting to add new bodies" << std::endl;
+      return std::make_pair(nullptr, nullptr);
+    }
+
+    dart::dynamics::SkeletonPtr skel;
+    if (parent) {
+      skel = parent->getSkeleton();
+    } else {
+      skel = dart::dynamics::Skeleton::create(
+          "toy_#" + std::to_string(getWorld()->getNumSkeletons() + 1));
+      getWorld()->addSkeleton(skel);
+    }
+
+    auto pair = skel->createJointAndBodyNodePair<JointType>(parent);
+    JointType* joint = pair.first;
+    dart::dynamics::BodyNode* bn = pair.second;
+    bn->setName("block_#" + std::to_string(skel->getNumBodyNodes()));
+    joint->setName("joint_#" + std::to_string(skel->getNumJoints()));
+
+    joint->setTransformFromParentBodyNode(relTf);
+    for (size_t i = 0; i < joint->getNumDofs(); ++i)
+      joint->getDof(i)->setDampingCoefficient(DefaultDamping);
+
+    bn->createShapeNodeWith<
+        dart::dynamics::VisualAspect,
+        dart::dynamics::CollisionAspect,
+        dart::dynamics::DynamicsAspect>(jointShape);
+
+    dart::dynamics::ShapeNode* block = bn->createShapeNodeWith<
+        dart::dynamics::VisualAspect,
+        dart::dynamics::CollisionAspect,
+        dart::dynamics::DynamicsAspect>(mBlockShape);
+    block->setRelativeTransform(mBlockOffset);
+
+    dart::dynamics::Inertia inertia = bn->getInertia();
+    inertia.setMass(DefaultBlockMass);
+    inertia.setMoment(mBlockShape->computeInertia(DefaultBlockMass));
+    inertia.setLocalCOM(DefaultBlockLength / 2.0 * Eigen::Vector3d::UnitX());
+    bn->setInertia(inertia);
+
+    getWorld()->getConstraintSolver()->getCollisionGroup()->addShapeFramesOf(
+        bn);
+
+    clearPick();
+
+    return std::make_pair(joint, bn);
+  }
+
+  Eigen::Isometry3d getRelTf() const
+  {
+    return mPickedNode ? mTarget->getTransform(mPickedNode)
+                       : mTarget->getWorldTransform();
+  }
+
+  void addWeldJointBlock()
+  {
+    addWeldJointBlock(mPickedNode, getRelTf());
+  }
+
+  dart::dynamics::BodyNode* addWeldJointBlock(
+      dart::dynamics::BodyNode* parent, const Eigen::Isometry3d& relTf)
+  {
+    return addBlock<dart::dynamics::WeldJoint>(parent, relTf, mWeldJointShape)
+        .second;
+  }
+
+  void addRevoluteJointBlock()
+  {
+    addRevoluteJointBlock(mPickedNode, getRelTf());
+  }
+
+  dart::dynamics::BodyNode* addRevoluteJointBlock(
+      dart::dynamics::BodyNode* parent, const Eigen::Isometry3d& relTf)
+  {
+    auto pair = addBlock<dart::dynamics::RevoluteJoint>(
+        parent, relTf, mRevoluteJointShape);
+
+    if (pair.first)
+      pair.first->setAxis(Eigen::Vector3d::UnitZ());
+
+    return pair.second;
+  }
+
+  void addBallJointBlock()
+  {
+    addBallJointBlock(mPickedNode, getRelTf());
+  }
+
+  dart::dynamics::BodyNode* addBallJointBlock(
+      dart::dynamics::BodyNode* parent, const Eigen::Isometry3d& relTf)
+  {
+    return addBlock<dart::dynamics::BallJoint>(parent, relTf, mBallJointShape)
+        .second;
+  }
+
+  void createInitialToy1()
+  {
+    Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
+    tf.rotate(Eigen::AngleAxisd(
+        dart::math::toRadian(45.0), Eigen::Vector3d::UnitY()));
+    dart::dynamics::BodyNode* bn = addBallJointBlock(nullptr, tf);
+
+    tf = Eigen::Isometry3d::Identity();
+    tf.translation()[0] = DefaultBlockLength;
+    tf.linear() = Eigen::Matrix3d::Identity();
+    tf.prerotate(Eigen::AngleAxisd(
+        dart::math::toRadian(90.0), Eigen::Vector3d::UnitX()));
+    bn = addRevoluteJointBlock(bn, tf);
+
+    tf = Eigen::Isometry3d::Identity();
+    tf.rotate(Eigen::AngleAxisd(
+        dart::math::toRadian(90.0), Eigen::Vector3d::UnitZ()));
+    bn = addWeldJointBlock(bn, tf);
+
+    tf = Eigen::Isometry3d::Identity();
+    tf.translation()[0] = DefaultBlockLength / 2.0;
+    tf.translation()[2] = DefaultBlockWidth;
+    tf.rotate(Eigen::AngleAxisd(
+        dart::math::toRadian(-30.0), Eigen::Vector3d::UnitZ()));
+    bn = addBallJointBlock(bn, tf);
+  }
+
+  void createInitialToy2()
+  {
+    Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
+    tf.rotate(Eigen::AngleAxisd(
+        dart::math::toRadian(90.0), Eigen::Vector3d::UnitY()));
+    tf.pretranslate(-1.0 * Eigen::Vector3d::UnitX());
+    dart::dynamics::BodyNode* bn = addBallJointBlock(nullptr, tf);
+
+    tf = Eigen::Isometry3d::Identity();
+    tf.translation()[0] = DefaultBlockLength;
+    tf.translation()[2] = DefaultBlockLength / 2.0;
+    tf.rotate(Eigen::AngleAxisd(
+        dart::math::toRadian(90.0), Eigen::Vector3d::UnitY()));
+    bn = addWeldJointBlock(bn, tf);
+
+    tf = Eigen::Isometry3d::Identity();
+    tf.rotate(Eigen::AngleAxisd(
+        dart::math::toRadian(-90.0), Eigen::Vector3d::UnitX()));
+    tf.rotate(Eigen::AngleAxisd(
+        dart::math::toRadian(-90.0), Eigen::Vector3d::UnitZ()));
+    tf.translation()[2] = DefaultBlockWidth / 2.0;
+    addRevoluteJointBlock(bn, tf);
+
+    tf.translation()[0] = DefaultBlockLength;
+    bn = addRevoluteJointBlock(bn, tf);
+
+    tf = Eigen::Isometry3d::Identity();
+    tf.translation()[0] = DefaultBlockLength;
+    addBallJointBlock(bn, tf);
+  }
+
+  void createForceLine()
+  {
+    dart::dynamics::SimpleFramePtr lineFrame
+        = std::make_shared<dart::dynamics::SimpleFrame>(
+            dart::dynamics::Frame::World());
+
+    mForceLine = std::make_shared<dart::dynamics::LineSegmentShape>(
+        Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), 3.0);
+    mForceLine->addDataVariance(dart::dynamics::Shape::DYNAMIC_VERTICES);
+
+    lineFrame->setShape(mForceLine);
+    lineFrame->createVisualAspect();
+    lineFrame->getVisualAspect()->setColor(DefaultForceLineColor);
+
+    getWorld()->addSimpleFrame(lineFrame);
+  }
+
+  void setForceCoeff(double coeff)
+  {
+    mForceCoeff = coeff;
+
+    if (mForceCoeff > MaxForceCoeff)
+      mForceCoeff = MaxForceCoeff;
+    else if (mForceCoeff < MinForceCoeff)
+      mForceCoeff = MinForceCoeff;
+  }
+
+  double getForceCoeff() const
+  {
+    return mForceCoeff;
+  }
+
+  void incrementForceCoeff()
+  {
+    mForceCoeff += ForceIncrement;
+    if (mForceCoeff > MaxForceCoeff)
+      mForceCoeff = MaxForceCoeff;
+
+    std::cout << "[Force Coefficient: " << mForceCoeff << "]" << std::endl;
+  }
+
+  void decrementForceCoeff()
+  {
+    mForceCoeff -= ForceIncrement;
+    if (mForceCoeff < MinForceCoeff)
+      mForceCoeff = MinForceCoeff;
+
+    std::cout << "[Force Coefficient: " << mForceCoeff << "]" << std::endl;
+  }
+
+  void reorientTarget()
+  {
+    Eigen::Isometry3d tf = mTarget->getWorldTransform();
+    tf.linear() = Eigen::Matrix3d::Identity();
+    mTarget->setTransform(tf);
+  }
+
+  float mForceCoeff;
+
+protected:
+  void setupViewer() override
+  {
+    mViewer->enableDragAndDrop(mTarget.get());
+  }
+
+  dart::dynamics::ShapePtr mWeldJointShape;
+  dart::dynamics::ShapePtr mRevoluteJointShape;
+  dart::dynamics::ShapePtr mBallJointShape;
+  dart::dynamics::ShapePtr mBlockShape;
+  Eigen::Isometry3d mBlockOffset;
+
+  dart::dynamics::BodyNode* mPickedNode;
+  Eigen::Vector3d mPickedPoint;
+  dart::gui::osg::InteractiveFramePtr mTarget;
+
+  dart::dynamics::LineSegmentShapePtr mForceLine;
+
+  bool mWasSimulating;
+};
+
+#endif // DART_EXAMPLE_OSG_OSGATLASSIMBICON_TINKERTOYWORLDNODE_HPP_

share/doc/dart/examples/tinkertoy/main.cpp

@@ -0,0 +1,186 @@
+/*
+ * Copyright (c) 2011-2025, 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 "TinkertoyWidget.hpp"
+#include "TinkertoyWorldNode.hpp"
+
+#include <dart/gui/osg/osg.hpp>
+
+#include <dart/dart.hpp>
+
+//==============================================================================
+class TinkertoyInputHandler : public osgGA::GUIEventHandler
+{
+public:
+  TinkertoyInputHandler(
+      dart::gui::osg::Viewer* viewer, TinkertoyWorldNode* node)
+    : mViewer(viewer), mNode(node)
+  {
+    // Do nothing
+  }
+
+  bool handle(
+      const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter&) override
+  {
+    if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN) {
+      if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Tab) {
+        mViewer->home();
+        return true;
+      } else if (ea.getKey() == '1') {
+        mNode->addWeldJointBlock();
+        return true;
+      } else if (ea.getKey() == '2') {
+        mNode->addRevoluteJointBlock();
+        return true;
+      } else if (ea.getKey() == '3') {
+        mNode->addBallJointBlock();
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_BackSpace) {
+        mNode->clearPick();
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Delete) {
+        mNode->deletePick();
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Up) {
+        mNode->incrementForceCoeff();
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Down) {
+        mNode->decrementForceCoeff();
+        return true;
+      } else if (ea.getKey() == '`') {
+        mNode->reorientTarget();
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Return) {
+        if (!mViewer->isRecording())
+          mViewer->record(DART_DATA_PATH "/screencap");
+        else
+          mViewer->pauseRecording();
+        return true;
+      }
+    }
+
+    return false;
+  }
+
+  dart::gui::osg::Viewer* mViewer;
+  TinkertoyWorldNode* mNode;
+};
+
+//==============================================================================
+class TinkertoyMouseHandler : public dart::gui::osg::MouseEventHandler
+{
+public:
+  TinkertoyMouseHandler(TinkertoyInputHandler* inputHandler)
+    : mInputHandler(inputHandler)
+  {
+    // Do nothing
+  }
+
+  void update() override
+  {
+    dart::gui::osg::Viewer* viewer = mInputHandler->mViewer;
+    TinkertoyWorldNode* node = mInputHandler->mNode;
+
+    dart::gui::osg::MouseButtonEvent event
+        = viewer->getDefaultEventHandler()->getButtonEvent(
+            dart::gui::osg::LEFT_MOUSE);
+
+    if (dart::gui::osg::BUTTON_PUSH == event) {
+      const std::vector<dart::gui::osg::PickInfo>& picks
+          = viewer->getDefaultEventHandler()->getButtonPicks(
+              dart::gui::osg::LEFT_MOUSE, dart::gui::osg::BUTTON_PUSH);
+
+      if (picks.empty())
+        return;
+
+      node->handlePick(picks.front());
+    }
+  }
+
+  TinkertoyInputHandler* mInputHandler;
+};
+
+//==============================================================================
+int main()
+{
+  // Create a world
+  dart::simulation::WorldPtr world(new dart::simulation::World);
+
+  // Create some coordinate axes for the user's reference
+  dart::gui::osg::InteractiveFramePtr coordinates
+      = dart::gui::osg::InteractiveFrame::createShared(
+          dart::dynamics::Frame::World(),
+          "coordinates",
+          Eigen::Isometry3d::Identity(),
+          0.2);
+
+  for (size_t i = 0; i < 3; ++i)
+    for (size_t j = 0; j < 3; ++j)
+      coordinates->getTool((dart::gui::osg::InteractiveTool::Type)(i), j)
+          ->setEnabled(false);
+
+  world->addSimpleFrame(coordinates);
+
+  // Create the OSG Node that represents the world
+  osg::ref_ptr<TinkertoyWorldNode> node = new TinkertoyWorldNode(world);
+
+  // Create the viewer
+  osg::ref_ptr<dart::gui::osg::ImGuiViewer> viewer
+      = new dart::gui::osg::ImGuiViewer();
+  viewer->addWorldNode(node);
+
+  // Add control widget for atlas
+  viewer->getImGuiHandler()->addWidget(
+      std::make_shared<TinkertoyWidget>(viewer, node.get()));
+
+  // Add the keyboard input handler
+  osg::ref_ptr<TinkertoyInputHandler> input
+      = new TinkertoyInputHandler(viewer, node);
+  viewer->addEventHandler(input.get());
+
+  // Add the mouse input handler
+  std::shared_ptr<TinkertoyMouseHandler> mouse
+      = std::make_shared<TinkertoyMouseHandler>(input);
+  viewer->getDefaultEventHandler()->addMouseEventHandler(mouse.get());
+
+  // Set the dimensions for the window
+  viewer->setUpViewInWindow(0, 0, 1280, 720);
+
+  // Set the window name
+  viewer->realize();
+  osgViewer::Viewer::Windows windows;
+  viewer->getWindows(windows);
+  windows.front()->setWindowName("Tinkertoy");
+
+  // Run the GUI application
+  viewer->run();
+}

share/doc/dart/examples/vehicle/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.22.1)
+
+get_filename_component(example_name ${CMAKE_CURRENT_LIST_DIR} NAME)
+
+project(${example_name})
+
+set(required_components utils-urdf gui-osg)
+set(required_libraries dart dart-utils-urdf dart-gui-osg)
+
+if(DART_IN_SOURCE_BUILD)
+  dart_build_example_in_source(${example_name} LINK_LIBRARIES ${required_libraries})
+  return()
+endif()
+
+find_package(DART 6.15.0 REQUIRED COMPONENTS ${required_components} CONFIG)
+
+file(GLOB srcs "*.cpp" "*.hpp")
+add_executable(${example_name} ${srcs})
+target_link_libraries(${example_name} PUBLIC ${required_libraries})

share/doc/dart/examples/vehicle/README.md

@@ -0,0 +1,22 @@
+# Vehicle
+
+This example demonstrates a simple vehicle simulation with steering control and velocity control for wheels. The vehicle has four wheels with differential drive control.
+
+## Controls
+
+- `w`: Move forward (set back wheels to rotate forward)
+- `s`: Stop (set back wheel velocity to zero)
+- `x`: Move backward (set back wheels to rotate backward)
+- `a`: Rotate steering wheels to left (maximum 30 degrees)
+- `d`: Rotate steering wheels to right (maximum -30 degrees)
+- `Space`: Simulation on/off
+
+## Description
+
+The vehicle example showcases:
+- Basic vehicle kinematics and dynamics
+- Steering control with angle limits
+- Differential drive control for rear wheels
+- PD control for wheel steering and velocity tracking
+
+The vehicle uses proportional-derivative (PD) control to track desired steering angles and wheel velocities. The steering is limited to ±30 degrees for realistic behavior.