dartpy 7.0.0.dev0__cp312-cp312-manylinux_2_39_x86_64.whl

include/dart/dynamics/LineSegmentShape.hpp

@@ -0,0 +1,140 @@
+/*
+ * 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_DYNAMICS_LINESEGMENTSHAPE_HPP_
+#define DART_DYNAMICS_LINESEGMENTSHAPE_HPP_
+
+#include <dart/dynamics/Shape.hpp>
+
+namespace dart {
+namespace dynamics {
+
+/// The LineSegmentShape facilitates creating graphs of line segments. The graph
+/// can consist of a single line segment or many interconnected line segments.
+/// Note: LineSegmentShape may NOT be used as a collision shape for BodyNodes,
+/// but it may be used for visualization purposes.
+class LineSegmentShape : public Shape
+{
+public:
+  /// Default constructor
+  LineSegmentShape(float _thickness = 1.0f);
+
+  /// Constructor for creating a simple line segment that connects two vertices
+  LineSegmentShape(
+      const Eigen::Vector3d& _v1,
+      const Eigen::Vector3d& _v2,
+      float _thickness = 1.0f);
+
+  // Documentation inherited.
+  const std::string& getType() const override;
+
+  /// Returns shape type for this class
+  static const std::string& getStaticType();
+
+  /// Set the line thickness/width for rendering
+  void setThickness(float _thickness);
+
+  /// Get the line thickness/width used for rendering
+  float getThickness() const;
+
+  /// Add a vertex as a child to the last vertex that was added
+  std::size_t addVertex(const Eigen::Vector3d& _v);
+
+  /// Add a vertex as a child to the specified vertex
+  std::size_t addVertex(const Eigen::Vector3d& _v, std::size_t _parent);
+
+  /// Remove a vertex from the list of vertices. IMPORTANT: Note that this
+  /// alters the indices of all vertices that follow it in the list, which also
+  /// clobbers the validity of the list of connections for all those vertices.
+  /// A safer and more efficient method might be to recycle vertices by moving
+  /// them around with setVertex() and/or altering their connections.
+  void removeVertex(std::size_t _idx);
+
+  /// Change the location of the specified vertex
+  void setVertex(std::size_t _idx, const Eigen::Vector3d& _v);
+
+  /// Get the location of the specified vertex. Returns a zero vector if an
+  /// out-of-bounds vertex is requested.
+  const Eigen::Vector3d& getVertex(std::size_t _idx) const;
+
+  /// Get all the vertices
+  const std::vector<Eigen::Vector3d>& getVertices() const;
+
+  /// Create a connection between the two specified vertices
+  void addConnection(std::size_t _idx1, std::size_t _idx2);
+
+  /// Search for a connection between two vertices and break it if it exists.
+  /// This is less efficient but more robust than removeConnection(std::size_t).
+  void removeConnection(std::size_t _vertexIdx1, std::size_t _vertexIdx2);
+
+  /// Remove the specified connection entry. Note that this will impact the
+  /// indices of all connections that come after _connectionIdx. This is more
+  /// efficient but less robust than removeConnection(std::size_t,std::size_t)
+  void removeConnection(std::size_t _connectionIdx);
+
+  /// Get all the connections
+  const common::aligned_vector<Eigen::Vector2i>& getConnections() const;
+
+  /// The returned inertia matrix will be like a very thin cylinder. The _mass
+  /// will be evenly distributed across all lines.
+  Eigen::Matrix3d computeInertia(double mass) const override;
+
+  // Documentation inherited.
+  ShapePtr clone() const override;
+
+  // TODO(MXG): Consider supporting colors-per-vertex
+
+protected:
+  // Documentation inherited.
+  void updateBoundingBox() const override;
+
+  // Documentation inherited
+  void updateVolume() const override;
+
+  /// Line thickness for rendering
+  float mThickness;
+
+  /// Vector of vertices
+  std::vector<Eigen::Vector3d> mVertices;
+
+  /// Vector of connections
+  common::aligned_vector<Eigen::Vector2i> mConnections;
+
+  /// A dummy vertex that can be returned when an out-of-bounds vertex is
+  /// requested
+  static const Eigen::Vector3d mDummyVertex;
+};
+
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_LINESEGMENTSHAPE_HPP_

include/dart/dynamics/Linkage.hpp

@@ -0,0 +1,246 @@
+/*
+ * 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_DYNAMICS_LINKAGE_HPP_
+#define DART_DYNAMICS_LINKAGE_HPP_
+
+#include <dart/dynamics/ReferentialSkeleton.hpp>
+
+#include <unordered_set>
+
+namespace dart {
+namespace dynamics {
+
+/// A Linkage is a ReferentialSkeleton with the special property that all the
+/// BodyNodes included in it form a contiguous graph. This property is only
+/// guaranteed during construction of the Linkage. After the Linkage has been
+/// constructed, a user might alter how the BodyNodes in a Linkage are
+/// assembled. The function Linkage::isAssembled() can be used to check whether
+/// the Linkage is still assembled. The function Linkage::satisfyCriteria() can
+/// be used to redefine the Linkage so that the original Linkage::Criteria is
+/// met again. The function Linkage::reassemble() will reassemble the BodyNodes
+/// so that they match whatever assembly they had the last time
+/// Linkage::reassemble() was called (or the assembly that they had when the
+/// Linkage was constructed, if Linkage::reassemble has never been called).
+class Linkage : public ReferentialSkeleton
+{
+public:
+  /// The Criteria class is used to specify how a Linkage should be constructed
+  struct Criteria
+  {
+    /// The ExpansionPolicy indicates how the collection of BodyNodes should
+    /// expand from the starting BodyNode (mStart)
+    enum ExpansionPolicy
+    {
+      INCLUDE = 0, ///< Do not expand from the target. Include everything up to
+                   ///< the target and then stop.
+      EXCLUDE, ///< Do not expand from the target. Include everything up to the
+               ///< target, but NOT the target, and then stop.
+      DOWNSTREAM, ///< Include the target, and then expand downstream, toward
+                  ///< the leaves of the tree.
+      UPSTREAM    ///< Include the target, and then expand upstream, toward the
+                  ///< root of the tree.
+    };
+
+    /// Return a vector of BodyNodes that satisfy the parameters of the Criteria
+    std::vector<BodyNode*> satisfy() const;
+
+    /// This structure defines targets for the expansion criteria and the
+    /// desired behavior for those targets
+    struct Target
+    {
+      /// Default constructor for Target
+      Target(
+          BodyNode* _target = nullptr,
+          ExpansionPolicy _policy = INCLUDE,
+          bool _chain = false);
+
+      /// The Linkage will expand from the starting BodyNode up to this node
+      WeakBodyNodePtr mNode;
+
+      /// After the target has been reached (if it is reached), the Linkage will
+      /// start to follow this expansion policy.
+      ExpansionPolicy mPolicy;
+
+      /// If this is set to true, the expansion towards this target will
+      /// terminate if (1) a fork/split in the kinematics is reached or (2) a
+      /// FreeJoint is reached.
+      bool mChain;
+    };
+
+    /// This Target will serve as the starting point for the criteria
+    /// satisfaction
+    Target mStart;
+
+    /// The Linkage will extend from mStart to each of these targets. Each
+    /// BodyNode along the way will be included in the Linkage, including the
+    /// mTarget. However, if a terminal BodyNode is reached along the way, then
+    /// nothing past the terminal BodyNode will be included. Therefore, If you
+    /// want to expand towards a target but not include the target, you can set
+    /// the BodyNode as both an mTarget and an mTerminal, and set the mInclusive
+    /// flag in mTerminal to false.
+    std::vector<Target> mTargets;
+
+    /// Any expansion performed by the criteria will be halted if mTerminal is
+    /// reached. If mInclusive is set to true, then mTerminal will be included
+    /// in the Linkage. If mInclusive is set to false, then mTerminal will not
+    /// be included in the Linkage. Note that the BodyNode of mStart may be
+    /// included as an inclusive terminal, but NOT as an exclusive terminal.
+    struct Terminal
+    {
+      /// Default constructor for Terminal
+      Terminal(BodyNode* _terminal = nullptr, bool _inclusive = true);
+
+      /// BodyNode that should halt any expansion
+      WeakBodyNodePtr mTerminal;
+
+      /// Whether or not the BodyNode should be included after expansion has
+      /// halted
+      bool mInclusive;
+    };
+
+    /// Any expansion (whether from an ExpansionPolicy or an attempt to reach an
+    /// entry in mTargets) will be halted if it reaches any entry in mTerminal
+    std::vector<Terminal> mTerminals;
+
+    /// Constructs an empty criteria that will lead to creating an empty Linkage
+    Criteria() = default;
+
+    /// Utility constructor to create a contiguous sequence of BodyNodes that
+    /// doesn't include branches in it.
+    ///
+    /// The start and target can be interchangeably set if both are included in
+    /// the sequence.
+    ///
+    /// \param[in] start The first BodyNode in the sequence. If \c nullptr is
+    /// passed the sequence will expand from \c target to the root.
+    /// \param[in] target The second BodyNode in the sequence. If \c nullptr is
+    /// passed the sequence will expand from \c start to the root.
+    /// \param[in] includeUpstreamParentJoint Set this to true if the parent
+    /// joint of whichever is upstream of the other should be included.
+    Criteria(
+        BodyNode* start,
+        BodyNode* target,
+        bool includeUpstreamParentJoint = false);
+
+  protected:
+    /// Refresh the content of mMapOfTerminals
+    void refreshTerminalMap() const;
+
+    /// Satisfy the expansion policy of a target
+    void expansionPolicy(
+        BodyNode* _start,
+        ExpansionPolicy _policy,
+        std::vector<BodyNode*>& _bns) const;
+
+    /// Expand downstream
+    void expandDownstream(
+        BodyNode* _start,
+        std::vector<BodyNode*>& _bns,
+        bool _includeStart) const;
+
+    /// Expand upstream
+    void expandUpstream(
+        BodyNode* _start,
+        std::vector<BodyNode*>& _bns,
+        bool _includeStart) const;
+
+    /// Construct a path from start to target
+    void expandToTarget(
+        const Target& _start,
+        const Target& _target,
+        std::vector<BodyNode*>& _bns) const;
+
+    /// Expand upwards from the _start BodyNode to the _target BodyNode
+    std::vector<BodyNode*> climbToTarget(
+        BodyNode* _start, BodyNode* _target) const;
+
+    /// Expand upwards from both BodyNodes to a common root
+    std::vector<BodyNode*> climbToCommonRoot(
+        const Target& _start, const Target& _target, bool _chain) const;
+
+    /// Crawl through the list and cut it off anywhere that the criteria is
+    /// violated
+    void trimBodyNodes(
+        std::vector<BodyNode*>& _bns, bool _chain, bool _movingUpstream) const;
+
+    /// Hashed set for terminals to allow quick lookup
+    mutable std::unordered_map<BodyNode*, bool> mMapOfTerminals;
+  };
+
+  /// Create a Linkage with the given Criteria
+  static LinkagePtr create(
+      const Criteria& _criteria, const std::string& _name = "Linkage");
+
+  /// Creates and returns a clone of this Linkage.
+  LinkagePtr cloneLinkage() const;
+
+  /// Creates and returns a clone of this Linkage.
+  LinkagePtr cloneLinkage(const std::string& cloneName) const;
+
+  // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName.
+  using MetaSkeleton::cloneMetaSkeleton;
+
+  // Documentation inherited
+  MetaSkeletonPtr cloneMetaSkeleton(
+      const std::string& cloneName) const override;
+
+  /// Returns false if the original assembly of this Linkage has been broken in
+  /// some way
+  bool isAssembled() const;
+
+  /// Revert the assembly of this Linkage to its original structure
+  void reassemble();
+
+  /// Redefine this Linkage so that its Criteria is satisfied
+  void satisfyCriteria();
+
+protected:
+  /// Constructor for the Linkage class. satisfyCriteria() will be called during
+  /// construction.
+  Linkage(const Criteria& _criteria, const std::string& _name = "Linkage");
+
+  /// Update any metadata needed by the Linkage or its derived classes
+  virtual void update();
+
+  /// Criteria that defines the structure of this Linkage
+  Criteria mCriteria;
+
+  /// Recording of the parent BodyNodes that were held by each of the BodyNodes
+  /// when the Linkage was constructed
+  std::vector<WeakBodyNodePtr> mParentBodyNodes;
+};
+
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_LINKAGE_HPP_

include/dart/dynamics/Marker.hpp

@@ -0,0 +1,126 @@
+/*
+ * 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_DYNAMICS_MARKER_HPP_
+#define DART_DYNAMICS_MARKER_HPP_
+
+#include <dart/dynamics/FixedJacobianNode.hpp>
+#include <dart/dynamics/detail/MarkerAspect.hpp>
+
+#include <dart/common/Deprecated.hpp>
+
+#include <Eigen/Dense>
+
+namespace dart {
+namespace dynamics {
+
+class BodyNode;
+
+class Marker final : public common::EmbedPropertiesOnTopOf<
+                         Marker,
+                         detail::MarkerProperties,
+                         FixedJacobianNode>
+{
+public:
+  using ConstraintType = detail::MarkerProperties::ConstraintType;
+  static constexpr ConstraintType NO = detail::MarkerProperties::NO;
+  static constexpr ConstraintType HARD = detail::MarkerProperties::HARD;
+  static constexpr ConstraintType SOFT = detail::MarkerProperties::SOFT;
+
+  using BasicProperties
+      = common::Composite::MakeProperties<NameAspect, FixedFrame, Marker>;
+
+  using Properties = common::Composite::Properties;
+
+  /// Destructor
+  virtual ~Marker() = default;
+
+  /// Set the AspectProperties of this Marker
+  void setAspectProperties(const AspectProperties& properties);
+
+  /// Get the BodyNode this Marker belongs to
+  ///
+  /// Deprecated: Use getBodyNodePtr() instead
+  DART_DEPRECATED(6.0)
+  BodyNode* getBodyNode();
+
+  /// Get the (const) BodyNode this Marker belongs to
+  ///
+  /// Deprecated: Use getBodyNodePtr() instead
+  DART_DEPRECATED(6.0)
+  const BodyNode* getBodyNode() const;
+
+  /// Get position of this marker in the parent body node coordinates
+  Eigen::Vector3d getLocalPosition() const;
+
+  /// Set position of this marker in the parent body node coordinates
+  void setLocalPosition(const Eigen::Vector3d& offset);
+
+  /// Get position in the world coordinates
+  Eigen::Vector3d getWorldPosition() const;
+
+  /// Get global unique ID
+  int getID() const;
+
+  /// Set constraint type. which will be useful for inverse kinematics
+  void setConstraintType(ConstraintType type);
+
+  /// Get constraint type. which will be useful for inverse kinematics
+  ConstraintType getConstraintType() const;
+
+  /// Set the color of this Marker
+  void setColor(const Eigen::Vector4d& color);
+
+  /// Return color of this Marker
+  const Eigen::Vector4d& getColor() const;
+
+  friend class BodyNode;
+
+protected:
+  /// Constructor used by BodyNode
+  Marker(BodyNode* parent, const BasicProperties& properties);
+
+  // Documentation inherited
+  Node* cloneNode(BodyNode* parent) const override;
+
+private:
+  /// Unique ID of this marker globally.
+  int mID;
+
+  /// Counts the number of markers globally.
+  static int msMarkerCount;
+};
+
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_MARKER_HPP_