dartpy 7.0.0.dev0__cp313-cp313-manylinux_2_39_x86_64.whl

include/dart/dynamics/detail/TemplatedJacobianNode.hpp

@@ -0,0 +1,294 @@
+/*
+ * 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_DETAIL_TEMPLATEDJACOBIAN_HPP_
+#define DART_DYNAMICS_DETAIL_TEMPLATEDJACOBIAN_HPP_
+
+#include <dart/dynamics/TemplatedJacobianNode.hpp>
+
+namespace dart {
+namespace dynamics {
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getJacobian(
+    const Frame* _inCoordinatesOf) const
+{
+  if (this == _inCoordinatesOf)
+    return static_cast<const NodeType*>(this)->getJacobian();
+  else if (_inCoordinatesOf->isWorld())
+    return static_cast<const NodeType*>(this)->getWorldJacobian();
+
+  return math::AdRJac(
+      getTransform(_inCoordinatesOf),
+      static_cast<const NodeType*>(this)->getJacobian());
+}
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getJacobian(
+    const Eigen::Vector3d& _offset) const
+{
+  math::Jacobian J = static_cast<const NodeType*>(this)->getJacobian();
+  J.bottomRows<3>() += J.topRows<3>().colwise().cross(_offset);
+
+  return J;
+}
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getJacobian(
+    const Eigen::Vector3d& _offset, const Frame* _inCoordinatesOf) const
+{
+  if (this == _inCoordinatesOf)
+    return getJacobian(_offset);
+  else if (_inCoordinatesOf->isWorld())
+    return getWorldJacobian(_offset);
+
+  Eigen::Isometry3d T = getTransform(_inCoordinatesOf);
+  T.translation() = -T.linear() * _offset;
+
+  return math::AdTJac(T, static_cast<const NodeType*>(this)->getJacobian());
+}
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getWorldJacobian(
+    const Eigen::Vector3d& _offset) const
+{
+  math::Jacobian J = static_cast<const NodeType*>(this)->getWorldJacobian();
+  J.bottomRows<3>()
+      += J.topRows<3>().colwise().cross(getWorldTransform().linear() * _offset);
+
+  return J;
+}
+
+//==============================================================================
+template <class NodeType>
+math::LinearJacobian TemplatedJacobianNode<NodeType>::getLinearJacobian(
+    const Frame* _inCoordinatesOf) const
+{
+  if (this == _inCoordinatesOf) {
+    const math::Jacobian& J = static_cast<const NodeType*>(this)->getJacobian();
+
+    return J.bottomRows<3>();
+  } else if (_inCoordinatesOf->isWorld()) {
+    const math::Jacobian& JWorld
+        = static_cast<const NodeType*>(this)->getWorldJacobian();
+
+    return JWorld.bottomRows<3>();
+  }
+
+  const math::Jacobian& J = static_cast<const NodeType*>(this)->getJacobian();
+
+  return getTransform(_inCoordinatesOf).linear() * J.bottomRows<3>();
+}
+
+//==============================================================================
+template <class NodeType>
+math::LinearJacobian TemplatedJacobianNode<NodeType>::getLinearJacobian(
+    const Eigen::Vector3d& _offset, const Frame* _inCoordinatesOf) const
+{
+  const math::Jacobian& J = static_cast<const NodeType*>(this)->getJacobian();
+
+  math::LinearJacobian JLinear;
+  JLinear = J.bottomRows<3>() + J.topRows<3>().colwise().cross(_offset);
+
+  if (this == _inCoordinatesOf)
+    return JLinear;
+
+  return getTransform(_inCoordinatesOf).linear() * JLinear;
+}
+
+//==============================================================================
+template <class NodeType>
+math::AngularJacobian TemplatedJacobianNode<NodeType>::getAngularJacobian(
+    const Frame* _inCoordinatesOf) const
+{
+  if (this == _inCoordinatesOf) {
+    const math::Jacobian& J = static_cast<const NodeType*>(this)->getJacobian();
+    return J.topRows<3>();
+  } else if (_inCoordinatesOf->isWorld()) {
+    const math::Jacobian& JWorld
+        = static_cast<const NodeType*>(this)->getWorldJacobian();
+    return JWorld.topRows<3>();
+  }
+
+  const math::Jacobian& J = static_cast<const NodeType*>(this)->getJacobian();
+
+  return getTransform(_inCoordinatesOf).linear() * J.topRows<3>();
+}
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getJacobianSpatialDeriv(
+    const Frame* _inCoordinatesOf) const
+{
+  if (this == _inCoordinatesOf)
+    return static_cast<const NodeType*>(this)->getJacobianSpatialDeriv();
+
+  return math::AdRJac(
+      getTransform(_inCoordinatesOf),
+      static_cast<const NodeType*>(this)->getJacobianSpatialDeriv());
+}
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getJacobianSpatialDeriv(
+    const Eigen::Vector3d& _offset) const
+{
+  math::Jacobian J_d
+      = static_cast<const NodeType*>(this)->getJacobianSpatialDeriv();
+
+  J_d.bottomRows<3>() += J_d.topRows<3>().colwise().cross(_offset);
+
+  return J_d;
+}
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getJacobianSpatialDeriv(
+    const Eigen::Vector3d& _offset, const Frame* _inCoordinatesOf) const
+{
+  if (this == _inCoordinatesOf)
+    return getJacobianSpatialDeriv(_offset);
+
+  Eigen::Isometry3d T = getTransform(_inCoordinatesOf);
+  T.translation() = T.linear() * -_offset;
+
+  return math::AdTJac(
+      T, static_cast<const NodeType*>(this)->getJacobianSpatialDeriv());
+}
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getJacobianClassicDeriv(
+    const Frame* _inCoordinatesOf) const
+{
+  if (_inCoordinatesOf->isWorld())
+    return static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
+
+  return math::AdRInvJac(
+      _inCoordinatesOf->getWorldTransform(),
+      static_cast<const NodeType*>(this)->getJacobianClassicDeriv());
+}
+
+//==============================================================================
+template <class NodeType>
+math::Jacobian TemplatedJacobianNode<NodeType>::getJacobianClassicDeriv(
+    const Eigen::Vector3d& _offset, const Frame* _inCoordinatesOf) const
+{
+  math::Jacobian J_d
+      = static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
+
+  const math::Jacobian& J
+      = static_cast<const NodeType*>(this)->getWorldJacobian();
+
+  const Eigen::Vector3d& w = getAngularVelocity();
+  const Eigen::Vector3d& p = (getWorldTransform().linear() * _offset).eval();
+
+  J_d.bottomRows<3>() += J_d.topRows<3>().colwise().cross(p)
+                         + J.topRows<3>().colwise().cross(w.cross(p));
+
+  if (_inCoordinatesOf->isWorld())
+    return J_d;
+
+  return math::AdRInvJac(_inCoordinatesOf->getWorldTransform(), J_d);
+}
+
+//==============================================================================
+template <class NodeType>
+math::LinearJacobian TemplatedJacobianNode<NodeType>::getLinearJacobianDeriv(
+    const Frame* _inCoordinatesOf) const
+{
+  const math::Jacobian& J_d
+      = static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
+
+  if (_inCoordinatesOf->isWorld())
+    return J_d.bottomRows<3>();
+
+  return _inCoordinatesOf->getWorldTransform().linear().transpose()
+         * J_d.bottomRows<3>();
+}
+
+//==============================================================================
+template <class NodeType>
+math::LinearJacobian TemplatedJacobianNode<NodeType>::getLinearJacobianDeriv(
+    const Eigen::Vector3d& _offset, const Frame* _inCoordinatesOf) const
+{
+  const math::Jacobian& J_d
+      = static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
+
+  const math::Jacobian& J
+      = static_cast<const NodeType*>(this)->getWorldJacobian();
+
+  const Eigen::Vector3d& w = getAngularVelocity();
+  const Eigen::Vector3d& p = (getWorldTransform().linear() * _offset).eval();
+
+  if (_inCoordinatesOf->isWorld())
+    return J_d.bottomRows<3>() + J_d.topRows<3>().colwise().cross(p)
+           + J.topRows<3>().colwise().cross(w.cross(p));
+
+  return _inCoordinatesOf->getWorldTransform().linear().transpose()
+         * (J_d.bottomRows<3>() + J_d.topRows<3>().colwise().cross(p)
+            + J.topRows<3>().colwise().cross(w.cross(p)));
+}
+
+//==============================================================================
+template <class NodeType>
+math::AngularJacobian TemplatedJacobianNode<NodeType>::getAngularJacobianDeriv(
+    const Frame* _inCoordinatesOf) const
+{
+  const math::Jacobian& J_d
+      = static_cast<const NodeType*>(this)->getJacobianClassicDeriv();
+
+  if (_inCoordinatesOf->isWorld())
+    return J_d.topRows<3>();
+
+  return _inCoordinatesOf->getWorldTransform().linear().transpose()
+         * J_d.topRows<3>();
+}
+
+//==============================================================================
+template <class NodeType>
+TemplatedJacobianNode<NodeType>::TemplatedJacobianNode(BodyNode* bn)
+  : Entity(Entity::ConstructAbstract),
+    Frame(Frame::ConstructAbstract),
+    JacobianNode(bn)
+{
+  // Do nothing
+}
+
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_DETAIL_TEMPLATEDJACOBIAN_HPP_

include/dart/dynamics/detail/TranslationalJoint2DAspect.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_DETAIL_TRANSLATIONALJOINT2DASPECT_HPP_
+#define DART_DYNAMICS_DETAIL_TRANSLATIONALJOINT2DASPECT_HPP_
+
+#include <dart/dynamics/GenericJoint.hpp>
+#include <dart/dynamics/detail/PlanarJointAspect.hpp>
+
+#include <Eigen/Dense>
+
+#include <string>
+
+namespace dart {
+namespace dynamics {
+
+class TranslationalJoint2D;
+
+namespace detail {
+
+//==============================================================================
+class TranslationalJoint2DUniqueProperties
+{
+public:
+  /// Constructor for pre-defined plane types. Defaults to the XY plane if
+  /// PlaneType::ARBITRARY is specified.
+  explicit TranslationalJoint2DUniqueProperties(
+      PlaneType planeType = PlaneType::XY);
+
+  /// Constructor for arbitrary plane types. mPlaneType will be set to
+  /// PlaneType::ARBITRARY
+  explicit TranslationalJoint2DUniqueProperties(
+      const Eigen::Matrix<double, 3, 2>& transAxes);
+
+  /// Constructor for arbitrary plane types. mPlaneType will be set to
+  /// PlaneType::ARBITRARY
+  TranslationalJoint2DUniqueProperties(
+      const Eigen::Vector3d& transAxis1, const Eigen::Vector3d& transAxis2);
+
+  /// Copy-constructor, customized for robustness
+  TranslationalJoint2DUniqueProperties(
+      const TranslationalJoint2DUniqueProperties& other);
+
+  virtual ~TranslationalJoint2DUniqueProperties() = default;
+
+  /// Assignment operator
+  TranslationalJoint2DUniqueProperties& operator=(
+      const TranslationalJoint2DUniqueProperties& other);
+
+  /// Sets plane type as XY-plane
+  void setXYPlane();
+
+  /// Sets plane type as YZ-plane
+  void setYZPlane();
+
+  /// Sets plane type as ZX-plane
+  void setZXPlane();
+
+  /// Sets plane type as arbitrary plane with two orthogonal translational axes
+  void setArbitraryPlane(const Eigen::Matrix<double, 3, 2>& transAxes);
+
+  /// Sets plane type as arbitrary plane with two orthogonal translational axes
+  void setArbitraryPlane(
+      const Eigen::Vector3d& transAxis1, const Eigen::Vector3d& transAxis2);
+
+  /// Returns first and second translational axes
+  const Eigen::Matrix<double, 3, 2>& getTranslationalAxes() const;
+
+  /// Returns first translational axis
+  Eigen::Vector3d getTranslationalAxis1() const;
+
+  /// Returns second translational axis
+  Eigen::Vector3d getTranslationalAxis2() const;
+
+  /// Returns plane type
+  PlaneType getPlaneType() const;
+
+private:
+  /// Plane type
+  PlaneType mPlaneType;
+
+  /// First and second translational axes
+  Eigen::Matrix<double, 3, 2> mTransAxes;
+};
+
+//==============================================================================
+struct TranslationalJoint2DProperties : GenericJoint<math::R2Space>::Properties,
+                                        TranslationalJoint2DUniqueProperties
+{
+  DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(TranslationalJoint2DProperties)
+
+  TranslationalJoint2DProperties(
+      const GenericJoint<math::R2Space>::Properties& genericJointProperties
+      = GenericJoint<math::R2Space>::Properties(),
+      const TranslationalJoint2DUniqueProperties& universalProperties
+      = TranslationalJoint2DUniqueProperties());
+
+  virtual ~TranslationalJoint2DProperties() = default;
+};
+
+//==============================================================================
+using TranslationalJoint2DBase = common::EmbedPropertiesOnTopOf<
+    TranslationalJoint2D,
+    TranslationalJoint2DUniqueProperties,
+    GenericJoint<math::R2Space>>;
+
+} // namespace detail
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_DETAIL_TRANSLATIONALJOINT2DASPECT_HPP_

include/dart/dynamics/detail/UniversalJointAspect.hpp

@@ -0,0 +1,86 @@
+/*
+ * 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_DETAIL_UNIVERSALJOINTASPECT_HPP_
+#define DART_DYNAMICS_DETAIL_UNIVERSALJOINTASPECT_HPP_
+
+#include <dart/dynamics/GenericJoint.hpp>
+
+#include <Eigen/Dense>
+
+#include <string>
+
+namespace dart {
+namespace dynamics {
+
+class UniversalJoint;
+
+namespace detail {
+
+//==============================================================================
+struct UniversalJointUniqueProperties
+{
+  std::array<Eigen::Vector3d, 2> mAxis;
+
+  UniversalJointUniqueProperties(
+      const Eigen::Vector3d& _axis1 = Eigen::Vector3d::UnitX(),
+      const Eigen::Vector3d& _axis2 = Eigen::Vector3d::UnitY());
+
+  virtual ~UniversalJointUniqueProperties() = default;
+};
+
+//==============================================================================
+struct UniversalJointProperties : GenericJoint<math::R2Space>::Properties,
+                                  UniversalJointUniqueProperties
+{
+  DART_DEFINE_ALIGNED_SHARED_OBJECT_CREATOR(UniversalJointProperties)
+
+  UniversalJointProperties(
+      const GenericJoint<math::R2Space>::Properties& genericJointProperties
+      = GenericJoint<math::R2Space>::Properties(),
+      const UniversalJointUniqueProperties& universalProperties
+      = UniversalJointUniqueProperties());
+
+  virtual ~UniversalJointProperties() = default;
+};
+
+//==============================================================================
+using UniversalJointBase = common::EmbedPropertiesOnTopOf<
+    UniversalJoint,
+    UniversalJointUniqueProperties,
+    GenericJoint<math::R2Space>>;
+
+} // namespace detail
+} // namespace dynamics
+} // namespace dart
+
+#endif // DART_DYNAMICS_DETAIL_UNIVERSALJOINTASPECT_HPP_

include/dart/dynamics/dynamics.hpp

@@ -0,0 +1,14 @@
+// Automatically generated file by cmake
+
+// DEPRECATED: This header is deprecated and will be removed in the next major release.
+// Please use <dart/dynamics/all.hpp> instead of <dart/dynamics/dynamics.hpp>
+
+#ifndef DART_SUPPRESS_DEPRECATED_HEADER_WARNING
+#if defined(_MSC_VER)
+#  pragma message("Warning: dart/dynamics/dynamics.hpp is deprecated. Use dart/dynamics/all.hpp instead.")
+#elif defined(__GNUC__) || defined(__clang__)
+#  warning "dart/dynamics/dynamics.hpp is deprecated. Use dart/dynamics/all.hpp instead."
+#endif
+#endif // DART_SUPPRESS_DEPRECATED_HEADER_WARNING
+
+#include "dart/dynamics/all.hpp"