dartpy 7.0.0.dev0__cp312-cp312-manylinux_2_39_x86_64.whl

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

@@ -0,0 +1,289 @@
+/*
+ * 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 <dart/gui/osg/osg.hpp>
+
+#include <dart/dart.hpp>
+
+//==============================================================================
+class CustomWorldNode : public dart::gui::osg::WorldNode
+{
+public:
+  CustomWorldNode(const dart::simulation::WorldPtr& world = nullptr)
+    : dart::gui::osg::WorldNode(world)
+  {
+    // Set up the customized WorldNode
+  }
+
+  void customPreRefresh()
+  {
+    // Use this function to execute custom code before each time that the
+    // window is rendered. This function can be deleted if it does not need
+    // to be used.
+  }
+
+  void customPostRefresh()
+  {
+    // Use this function to execute custom code after each time that the
+    // window is rendered. This function can be deleted if it does not need
+    // to be used.
+  }
+
+  void customPreStep()
+  {
+    // Use this function to execute custom code before each simulation time
+    // step is performed. This function can be deleted if it does not need
+    // to be used.
+  }
+
+  void customPostStep()
+  {
+    // Use this function to execute custom code after each simulation time
+    // step is performed. This function can be deleted if it does not need
+    // to be used.
+  }
+};
+
+//==============================================================================
+class CustomEventHandler : public osgGA::GUIEventHandler
+{
+public:
+  CustomEventHandler(/*Pass in any necessary arguments*/)
+  {
+    // Set up the customized event handler
+  }
+
+  virtual bool handle(
+      const osgGA::GUIEventAdapter& ea, osgGA::GUIActionAdapter&) override
+  {
+    if (ea.getEventType() == osgGA::GUIEventAdapter::KEYDOWN) {
+      if (ea.getKey() == 'q') {
+        std::cout << "Lowercase q pressed" << std::endl;
+        return true;
+      } else if (ea.getKey() == 'Q') {
+        std::cout << "Capital Q pressed" << std::endl;
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Left) {
+        std::cout << "Left arrow key pressed" << std::endl;
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Right) {
+        std::cout << "Right arrow key pressed" << std::endl;
+        return true;
+      }
+    } else if (ea.getEventType() == osgGA::GUIEventAdapter::KEYUP) {
+      if (ea.getKey() == 'q') {
+        std::cout << "Lowercase q released" << std::endl;
+        return true;
+      } else if (ea.getKey() == 'Q') {
+        std::cout << "Capital Q released" << std::endl;
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Left) {
+        std::cout << "Left arrow key released" << std::endl;
+        return true;
+      } else if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Right) {
+        std::cout << "Right arrow key released" << std::endl;
+        return true;
+      }
+    }
+
+    // The return value should be 'true' if the input has been fully handled
+    // and should not be visible to any remaining event handlers. It should be
+    // false if the input has not been fully handled and should be viewed by
+    // any remaining event handlers.
+    return false;
+  }
+};
+
+//==============================================================================
+class TestWidget : public dart::gui::osg::ImGuiWidget
+{
+public:
+  /// Constructor
+  TestWidget(
+      dart::gui::osg::ImGuiViewer* viewer, dart::simulation::WorldPtr world)
+    : mViewer(viewer),
+      mWorld(std::move(world)),
+      mGuiGravity(true),
+      mGravity(true),
+      mGuiHeadlights(true)
+  {
+    // Do nothing
+  }
+
+  // Documentation inherited
+  void render() override
+  {
+    ImGui::SetNextWindowPos(ImVec2(10, 20));
+    ImGui::SetNextWindowSize(ImVec2(240, 320));
+    ImGui::SetNextWindowBgAlpha(0.5f);
+    if (!ImGui::Begin(
+            "Tinkertoy Control",
+            nullptr,
+            ImGuiWindowFlags_NoResize | ImGuiWindowFlags_MenuBar
+                | ImGuiWindowFlags_HorizontalScrollbar)) {
+      // Early out if the window is collapsed, as an optimization.
+      ImGui::End();
+      return;
+    }
+
+    // Menu
+    if (ImGui::BeginMenuBar()) {
+      if (ImGui::BeginMenu("Menu")) {
+        if (ImGui::MenuItem("Exit"))
+          mViewer->setDone(true);
+        ImGui::EndMenu();
+      }
+      if (ImGui::BeginMenu("Help")) {
+        if (ImGui::MenuItem("About DART"))
+          mViewer->showAbout();
+        ImGui::EndMenu();
+      }
+      ImGui::EndMenuBar();
+    }
+
+    ImGui::Text("An empty OSG example with ImGui");
+    ImGui::Spacing();
+
+    if (ImGui::CollapsingHeader("Simulation", ImGuiTreeNodeFlags_DefaultOpen)) {
+      int e = mViewer->isSimulating() ? 0 : 1;
+      if (mViewer->isAllowingSimulation()) {
+        if (ImGui::RadioButton("Play", &e, 0) && !mViewer->isSimulating())
+          mViewer->simulate(true);
+        ImGui::SameLine();
+        if (ImGui::RadioButton("Pause", &e, 1) && mViewer->isSimulating())
+          mViewer->simulate(false);
+      }
+
+      ImGui::Text("Time: %.3f", mWorld->getTime());
+    }
+
+    if (ImGui::CollapsingHeader(
+            "World Options", ImGuiTreeNodeFlags_DefaultOpen)) {
+      // Gravity
+      ImGui::Checkbox("Gravity On/Off", &mGuiGravity);
+      setGravity(mGuiGravity);
+
+      ImGui::Spacing();
+
+      // Headlights
+      mGuiHeadlights = mViewer->checkHeadlights();
+      ImGui::Checkbox("Headlights On/Off", &mGuiHeadlights);
+      mViewer->switchHeadlights(mGuiHeadlights);
+    }
+
+    if (ImGui::CollapsingHeader("View", ImGuiTreeNodeFlags_DefaultOpen)) {
+      osg::Vec3d eye;
+      osg::Vec3d center;
+      osg::Vec3d up;
+      mViewer->getCamera()->getViewMatrixAsLookAt(eye, center, up);
+
+      ImGui::Text("Eye   : (%.2f, %.2f, %.2f)", eye.x(), eye.y(), eye.z());
+      ImGui::Text(
+          "Center: (%.2f, %.2f, %.2f)", center.x(), center.y(), center.z());
+      ImGui::Text("Up    : (%.2f, %.2f, %.2f)", up.x(), up.y(), up.z());
+    }
+
+    if (ImGui::CollapsingHeader("Help")) {
+      ImGui::PushTextWrapPos(ImGui::GetCursorPos().x + 320);
+      ImGui::Text("User Guide:\n");
+      ImGui::Text("%s", mViewer->getInstructions().c_str());
+      ImGui::PopTextWrapPos();
+    }
+
+    ImGui::End();
+  }
+
+protected:
+  void setGravity(bool gravity)
+  {
+    if (mGravity == gravity)
+      return;
+
+    mGravity = gravity;
+
+    if (mGravity)
+      mWorld->setGravity(-9.81 * Eigen::Vector3d::UnitZ());
+    else
+      mWorld->setGravity(Eigen::Vector3d::Zero());
+  }
+
+  osg::ref_ptr<dart::gui::osg::ImGuiViewer> mViewer;
+  dart::simulation::WorldPtr mWorld;
+  bool mGuiGravity;
+  bool mGravity;
+  bool mGuiHeadlights;
+};
+
+//==============================================================================
+int main()
+{
+  // Create a world
+  dart::simulation::WorldPtr world(new dart::simulation::World);
+
+  // Add a target object to the world
+  dart::gui::osg::InteractiveFramePtr target(
+      new dart::gui::osg::InteractiveFrame(dart::dynamics::Frame::World()));
+  world->addSimpleFrame(target);
+
+  // Wrap a WorldNode around it
+  osg::ref_ptr<CustomWorldNode> node = new CustomWorldNode(world);
+
+  // Create a Viewer and set it up with the WorldNode
+  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<TestWidget>(viewer, world));
+
+  // Active the drag-and-drop feature for the target
+  viewer->enableDragAndDrop(target.get());
+
+  // Pass in the custom event handler
+  viewer->addEventHandler(new CustomEventHandler);
+
+  // Set up the window to be 640x480
+  viewer->setUpViewInWindow(0, 0, 640, 480);
+
+  // Adjust the viewpoint of the Viewer
+  viewer->getCameraManipulator()->setHomePosition(
+      ::osg::Vec3(2.57f, 3.14f, 1.64f),
+      ::osg::Vec3(0.00f, 0.00f, 0.00f),
+      ::osg::Vec3(-0.24f, -0.25f, 0.94f));
+  // We need to re-dirty the CameraManipulator by passing it into the viewer
+  // again, so that the viewer knows to update its HomePosition setting
+  viewer->setCameraManipulator(viewer->getCameraManipulator());
+
+  // Begin running the application loop
+  viewer->run();
+}

share/doc/dart/examples/joint_constraints/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/joint_constraints/Controller.cpp

@@ -0,0 +1,110 @@
+/*
+ * 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 "Controller.hpp"
+
+using namespace dart;
+using namespace dynamics;
+using namespace math;
+
+Controller::Controller(
+    const dynamics::SkeletonPtr& _skel,
+    constraint::ConstraintSolver* _collisionSolver,
+    double _t)
+{
+  mSkel = _skel;
+  mCollisionHandle = _collisionSolver;
+  mTimestep = _t;
+  mFrame = 0;
+  int nDof = mSkel->getNumDofs();
+  mKp = Eigen::MatrixXd::Identity(nDof, nDof);
+  mKd = Eigen::MatrixXd::Identity(nDof, nDof);
+  mConstrForces = Eigen::VectorXd::Zero(nDof);
+
+  mTorques.resize(nDof);
+  mDesiredDofs.resize(nDof);
+  for (int i = 0; i < nDof; i++) {
+    mTorques[i] = 0.0;
+    mDesiredDofs[i] = mSkel->getPosition(i);
+  }
+
+  // using SPD results in simple Kp coefficients
+  for (int i = 0; i < 6; i++) {
+    mKp(i, i) = 0.0;
+    mKd(i, i) = 0.0;
+  }
+  for (int i = 6; i < 22; i++)
+    mKp(i, i) = 200.0; // lower body + lower back
+  for (int i = 22; i < nDof; i++)
+    mKp(i, i) = 20.0;
+  for (int i = 6; i < 22; i++)
+    mKd(i, i) = 100.0;
+  for (int i = 22; i < nDof; i++)
+    mKd(i, i) = 10.0;
+
+  mPreOffset = 0.0;
+}
+
+void Controller::computeTorques(
+    const Eigen::VectorXd& _dof, const Eigen::VectorXd& _dofVel)
+{
+  // SPD tracking
+  Eigen::MatrixXd invM = (mSkel->getMassMatrix() + mKd * mTimestep).inverse();
+  Eigen::VectorXd p = -mKp * (_dof + _dofVel * mTimestep - mDesiredDofs);
+  Eigen::VectorXd d = -mKd * _dofVel;
+  Eigen::VectorXd qddot
+      = invM * (-mSkel->getCoriolisAndGravityForces() + p + d + mConstrForces);
+  mTorques = p + d - mKd * qddot * mTimestep;
+
+  // ankle strategy for sagital plane
+  Eigen::Vector3d com = mSkel->getCOM();
+  Eigen::Vector3d cop = mSkel->getBodyNode("h_heel_left")->getTransform()
+                        * Eigen::Vector3d(0.05, 0, 0);
+  Eigen::Vector2d diff(com[0] - cop[0], com[2] - cop[2]);
+  if (diff[0] < 0.1) {
+    double offset = com[0] - cop[0];
+    double k1 = 20.0;
+    double k2 = 10.0;
+    double kd = 100.0;
+    mTorques[17] += -k1 * offset + kd * (mPreOffset - offset);
+    mTorques[25] += -k2 * offset + kd * (mPreOffset - offset);
+    mTorques[19] += -k1 * offset + kd * (mPreOffset - offset);
+    mTorques[26] += -k2 * offset + kd * (mPreOffset - offset);
+    mPreOffset = offset;
+  }
+
+  // Just to make sure no illegal torque is used
+  for (int i = 0; i < 6; i++)
+    mTorques[i] = 0.0;
+
+  mFrame++;
+}

share/doc/dart/examples/joint_constraints/Controller.hpp

@@ -0,0 +1,105 @@
+/*
+ * 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 EXAMPLES_JOINTCONSTRAINTS_CONTROLLER_HPP_
+#define EXAMPLES_JOINTCONSTRAINTS_CONTROLLER_HPP_
+
+#include <dart/dart.hpp>
+
+#include <Eigen/Dense>
+
+#include <vector>
+
+class Controller
+{
+public:
+  Controller(
+      const dart::dynamics::SkeletonPtr& _skel,
+      dart::constraint::ConstraintSolver* _collisionSolver,
+      double _t);
+  virtual ~Controller() {}
+
+  Eigen::VectorXd getTorques()
+  {
+    return mTorques;
+  }
+  double getTorque(int _index)
+  {
+    return mTorques[_index];
+  }
+  void setDesiredDof(int _index, double _val)
+  {
+    mDesiredDofs[_index] = _val;
+  }
+  void computeTorques(
+      const Eigen::VectorXd& _dof, const Eigen::VectorXd& _dofVel);
+  dart::dynamics::SkeletonPtr getSkel()
+  {
+    return mSkel;
+  }
+  Eigen::VectorXd getDesiredDofs()
+  {
+    return mDesiredDofs;
+  }
+  Eigen::MatrixXd getKp()
+  {
+    return mKp;
+  }
+  Eigen::MatrixXd getKd()
+  {
+    return mKd;
+  }
+  void setConstrForces(const Eigen::VectorXd& _constrForce)
+  {
+    mConstrForces = _constrForce;
+  }
+
+protected:
+  bool computeCoP(dart::dynamics::BodyNode* _node, Eigen::Vector3d* _cop);
+  Eigen::Vector3d evalLinMomentum(const Eigen::VectorXd& _dofVel);
+  Eigen::Vector3d evalAngMomentum(const Eigen::VectorXd& _dofVel);
+  Eigen::VectorXd adjustAngMomentum(
+      Eigen::VectorXd _deltaMomentum, Eigen::VectorXd _controlledAxis);
+  dart::dynamics::SkeletonPtr mSkel;
+  dart::constraint::ConstraintSolver* mCollisionHandle;
+  Eigen::VectorXd mTorques;
+  Eigen::VectorXd mDesiredDofs;
+  Eigen::MatrixXd mKp;
+  Eigen::MatrixXd mKd;
+  int mFrame;
+  double mTimestep;
+  double mPreOffset;
+  Eigen::VectorXd
+      mConstrForces; // SPD utilizes the current info about contact forces
+};
+
+#endif // EXAMPLES_JOINTCONSTRAINTS_CONTROLLER_HPP_

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

@@ -0,0 +1,22 @@
+# Joint Constraints
+
+This example demonstrates joint constraint handling with SPD (Stable Proportional-Derivative) tracking control and dynamic perturbation testing. The example features a humanoid character that maintains balance while being subjected to external forces.
+
+## Controls
+
+- `1`: Push forward (apply force in +X direction)
+- `2`: Push backward (apply force in -X direction)
+- `3`: Push right (apply force in +Z direction)
+- `4`: Push left (apply force in -Z direction)
+- `h`: Toggle harness on/off (adds/removes weld joint constraint to pelvis)
+- `Space`: Simulation on/off
+
+## Description
+
+The joint constraints example showcases:
+- SPD (Stable Proportional-Derivative) tracking control for balance
+- External force perturbations to test robustness
+- Dynamic constraint addition/removal (harness feature)
+- Ankle strategy for center of pressure control
+
+The controller computes torques to maintain desired joint configurations while the character responds to external forces. The harness feature allows you to weld the pelvis to the world frame, which can help with testing and debugging.