Allow specifying reference frame for TaskSpaceRegion

dartsim · Apr 3, 2021 · c22c5a1 · c22c5a1
1 parent 7e9fc47
commit c22c5a1
Show file tree

Hide file tree

Showing 4 changed files with 133 additions and 26 deletions.
diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml
@@ -18,17 +18,19 @@ env:
   DART_DEV_IMAGE: dartsim/dart-dev
   UBUNTU_BUILD_CMD: |
     docker run \
-      -v $GITHUB_WORKSPACE:$GITHUB_WORKSPACE \
+      --platform $PLATFORM \
+      --volume $GITHUB_WORKSPACE:$GITHUB_WORKSPACE \
       --env-file ./.ci/docker/env.list \
       $DART_DEV_IMAGE:$DOCKER_TAG \
       /bin/sh -c "cd $GITHUB_WORKSPACE && ./.ci/build.sh"
 
 jobs:
   # Linux distros on multiple architectures
-  bionic_gcc_release:
+  ubuntu_gcc_release:
     name: Ubuntu [GCC|Release]
     runs-on: ubuntu-20.04
     strategy:
+      fail-fast: false
       matrix:
         os:
           [
@@ -37,22 +39,20 @@ jobs:
             ubuntu-groovy,
             ubuntu-hirsute,
           ]
-        sha256: [""]
+        platform: [linux/amd64]
         include:
-          # amd64
           - os: ubuntu-bionic
-            sha256: "@sha256:bbcc3086bb13c3bb59214b67ceb520f8fde89a92745611e3b62992575fd3bc9e"
-          # arm64
+            platform: linux/amd64
           - os: ubuntu-bionic
-            sha256: "@sha256:b798f02a2e16bb3b523dd087bb9e6e35598263af18bd0e61a8781c80352bf061"
-          # ppc64le
+            platform: linux/arm64
           - os: ubuntu-bionic
-            sha256: "@sha256:f9e60cad2904c7418a48bde5803a6866d0fa5893c4204689d978541572905b87"
+            platform: linux/ppc64le
     env:
-      DOCKER_TAG: ${{ matrix.os }}-v6.10${{ matrix.sha256 }}
+      DOCKER_TAG: ${{ matrix.os }}-v6.10
+      PLATFORM: ${{ matrix.platform }}
       COMPILER: gcc
       BUILD_TYPE: Release
-      BUILD_DARTPY: OFF
+      BUILD_DARTPY: "${{ matrix.os == 'ubuntu-xenial' && 'OFF' || 'ON' }}"
     steps:
       # https://github.com/marketplace/actions/docker-setup-qemu
       - name: Set up QEMU

diff --git a/dart/dynamics/InverseKinematics.cpp b/dart/dynamics/InverseKinematics.cpp
@@ -436,8 +436,9 @@ void InverseKinematics::ErrorMethod::clearCache()
 
 //==============================================================================
 InverseKinematics::TaskSpaceRegion::UniqueProperties::UniqueProperties(
-    bool computeErrorFromCenter)
-  : mComputeErrorFromCenter(computeErrorFromCenter)
+    bool computeErrorFromCenter, SimpleFramePtr referenceFrame)
+  : mComputeErrorFromCenter(computeErrorFromCenter),
+    mReferenceFrame(std::move(referenceFrame))
 {
   // Do nothing
 }
@@ -508,11 +509,17 @@ Eigen::Vector6d InverseKinematics::TaskSpaceRegion::computeError()
   // valid constraint manifold faster than computing it from the edge of the
   // Task Space Region.
 
+  const Frame* referenceFrame = mTaskSpaceP.mReferenceFrame.get();
+  if (referenceFrame == nullptr)
+    referenceFrame = mIK->getTarget()->getParentFrame();
+  assert(referenceFrame != nullptr);
+
   // Use the target's transform with respect to its reference frame
-  const Eigen::Isometry3d& targetTf = mIK->getTarget()->getRelativeTransform();
+  const Eigen::Isometry3d targetTf
+      = mIK->getTarget()->getTransform(referenceFrame);
   // Use the actual transform with respect to the target's reference frame
-  const Eigen::Isometry3d& actualTf
-      = mIK->getNode()->getTransform(mIK->getTarget()->getParentFrame());
+  const Eigen::Isometry3d actualTf
+      = mIK->getNode()->getTransform(referenceFrame);
 
   // ^ This scheme makes it so that the bounds are expressed in the reference
   // frame of the target
@@ -521,11 +528,11 @@ Eigen::Vector6d InverseKinematics::TaskSpaceRegion::computeError()
   if (mIK->hasOffset())
     p_error += actualTf.linear() * mIK->getOffset();
 
-  Eigen::Matrix3d R_error = actualTf.linear() * targetTf.linear().transpose();
+  const Eigen::Matrix3d R_error
+      = actualTf.linear() * targetTf.linear().transpose();
 
   Eigen::Vector6d displacement;
-  displacement.head<3>() = math::matrixToEulerXYZ(R_error);
-  displacement.tail<3>() = p_error;
+  displacement << math::matrixToEulerXYZ(R_error), p_error;
 
   Eigen::Vector6d error;
   const Eigen::Vector6d& min = mErrorP.mBounds.first;
@@ -570,11 +577,10 @@ Eigen::Vector6d InverseKinematics::TaskSpaceRegion::computeError()
   if (error.norm() > mErrorP.mErrorLengthClamp)
     error = error.normalized() * mErrorP.mErrorLengthClamp;
 
-  if (!mIK->getTarget()->getParentFrame()->isWorld())
+  if (!referenceFrame->isWorld())
   {
     // Transform the error term into the world frame if it's not already
-    const Eigen::Isometry3d& R
-        = mIK->getTarget()->getParentFrame()->getWorldTransform();
+    const Eigen::Isometry3d& R = referenceFrame->getWorldTransform();
     error.head<3>() = R.linear() * error.head<3>();
     error.tail<3>() = R.linear() * error.tail<3>();
   }
@@ -595,6 +601,20 @@ bool InverseKinematics::TaskSpaceRegion::isComputingFromCenter() const
   return mTaskSpaceP.mComputeErrorFromCenter;
 }
 
+//==============================================================================
+void InverseKinematics::TaskSpaceRegion::setReferenceFrame(
+    SimpleFramePtr referenceFrame)
+{
+  mTaskSpaceP.mReferenceFrame = std::move(referenceFrame);
+}
+
+//==============================================================================
+ConstSimpleFramePtr InverseKinematics::TaskSpaceRegion::getReferenceFrame()
+    const
+{
+  return mTaskSpaceP.mReferenceFrame;
+}
+
 //==============================================================================
 InverseKinematics::TaskSpaceRegion::Properties
 InverseKinematics::TaskSpaceRegion::getTaskSpaceRegionProperties() const

diff --git a/dart/dynamics/InverseKinematics.hpp b/dart/dynamics/InverseKinematics.hpp
@@ -754,8 +754,15 @@ class InverseKinematics::TaskSpaceRegion : public ErrorMethod
     /// drop to zero, regardless of whether this flag is true or false.
     bool mComputeErrorFromCenter;
 
+    /// The reference frame that the task space region is expressed. If this
+    /// frame is set to nullptr, which is the default, then the parent frame of
+    /// the target frame is used instead.
+    SimpleFramePtr mReferenceFrame;
+
     /// Default constructor
-    UniqueProperties(bool computeErrorFromCenter = true);
+    UniqueProperties(
+        bool computeErrorFromCenter = true,
+        SimpleFramePtr referenceFrame = nullptr);
   };
 
   struct Properties : ErrorMethod::Properties, UniqueProperties
@@ -793,6 +800,13 @@ class InverseKinematics::TaskSpaceRegion : public ErrorMethod
   /// the center of the region.
   bool isComputingFromCenter() const;
 
+  /// Set the reference frame that the task space region is expressed. Pass
+  /// nullptr to use the parent frame of the target frame instead.
+  void setReferenceFrame(SimpleFramePtr referenceFrame);
+
+  /// Get the reference frame that the task space region is expressed.
+  ConstSimpleFramePtr getReferenceFrame() const;
+
   /// Get the Properties of this TaskSpaceRegion
   Properties getTaskSpaceRegionProperties() const;
 

diff --git a/python/dartpy/dynamics/InverseKinematics.cpp b/python/dartpy/dynamics/InverseKinematics.cpp
@@ -33,6 +33,7 @@
 #include <dart/dart.hpp>
 #include <pybind11/pybind11.h>
 #include <pybind11/stl.h>
+
 #include "eigen_geometry_pybind.h"
 #include "eigen_pybind.h"
 
@@ -43,6 +44,33 @@ namespace python {
 
 void InverseKinematics(py::module& m)
 {
+  ::py::class_<dart::dynamics::InverseKinematics::ErrorMethod::Properties>(
+      m, "InverseKinematicsErrorMethodProperties")
+      .def(
+          ::py::init<
+              const dart::dynamics::InverseKinematics::ErrorMethod::Bounds&,
+              double,
+              const Eigen::Vector6d&>(),
+          ::py::arg("bounds")
+          = dart::dynamics::InverseKinematics::ErrorMethod::Bounds(
+              Eigen::Vector6d::Constant(-dart::dynamics::DefaultIKTolerance),
+              Eigen::Vector6d::Constant(dart::dynamics::DefaultIKTolerance)),
+          ::py::arg("errorClamp") = dart::dynamics::DefaultIKErrorClamp,
+          ::py::arg("errorWeights") = Eigen::compose(
+              Eigen::Vector3d::Constant(dart::dynamics::DefaultIKAngularWeight),
+              Eigen::Vector3d::Constant(dart::dynamics::DefaultIKLinearWeight)))
+      .def_readwrite(
+          "mBounds",
+          &dart::dynamics::InverseKinematics::ErrorMethod::Properties::mBounds)
+      .def_readwrite(
+          "mErrorLengthClamp",
+          &dart::dynamics::InverseKinematics::ErrorMethod::Properties::
+              mErrorLengthClamp)
+      .def_readwrite(
+          "mErrorWeights",
+          &dart::dynamics::InverseKinematics::ErrorMethod::Properties::
+              mErrorWeights);
+
   ::py::class_<
       dart::dynamics::InverseKinematics::ErrorMethod,
       dart::common::Subject,
@@ -238,6 +266,50 @@ void InverseKinematics(py::module& m)
             self->clearCache();
           });
 
+  ::py::class_<
+      dart::dynamics::InverseKinematics::TaskSpaceRegion::UniqueProperties>(
+      m, "InverseKinematicsTaskSpaceRegionUniqueProperties")
+      .def(
+          ::py::init<bool, dart::dynamics::SimpleFramePtr>(),
+          ::py::arg("computeErrorFromCenter") = true,
+          ::py::arg("referenceFrame") = nullptr)
+      .def_readwrite(
+          "mComputeErrorFromCenter",
+          &dart::dynamics::InverseKinematics::TaskSpaceRegion::
+              UniqueProperties::mComputeErrorFromCenter)
+      .def_readwrite(
+          "mReferenceFrame",
+          &dart::dynamics::InverseKinematics::TaskSpaceRegion::
+              UniqueProperties::mReferenceFrame);
+
+  ::py::class_<
+      dart::dynamics::InverseKinematics::TaskSpaceRegion::Properties,
+      dart::dynamics::InverseKinematics::ErrorMethod::Properties,
+      dart::dynamics::InverseKinematics::TaskSpaceRegion::UniqueProperties>(
+      m, "InverseKinematicsTaskSpaceRegionProperties")
+      .def(
+          ::py::init<
+              const dart::dynamics::InverseKinematics::ErrorMethod::Properties&,
+              const dart::dynamics::InverseKinematics::TaskSpaceRegion::
+                  UniqueProperties&>(),
+          ::py::arg("errorProperties")
+          = dart::dynamics::InverseKinematics::ErrorMethod::Properties(),
+          ::py::arg("taskSpaceProperties") = dart::dynamics::InverseKinematics::
+              TaskSpaceRegion::UniqueProperties());
+
+  ::py::class_<
+      dart::dynamics::InverseKinematics::TaskSpaceRegion,
+      dart::dynamics::InverseKinematics::ErrorMethod,
+      std::shared_ptr<dart::dynamics::InverseKinematics::TaskSpaceRegion>>(
+      m, "InverseKinematicsTaskSpaceRegion")
+      .def(
+          ::py::init<
+              dart::dynamics::InverseKinematics*,
+              dart::dynamics::InverseKinematics::TaskSpaceRegion::Properties>(),
+          ::py::arg("ik"),
+          ::py::arg("properties")
+          = dart::dynamics::InverseKinematics::TaskSpaceRegion::Properties());
+
   ::py::class_<
       dart::dynamics::InverseKinematics,
       dart::common::Subject,
@@ -441,9 +513,10 @@ void InverseKinematics(py::module& m)
           +[](dart::dynamics::InverseKinematics* self,
               const Eigen::VectorXd& _q) { self->setPositions(_q); },
           ::py::arg("q"))
-      .def("clearCaches", +[](dart::dynamics::InverseKinematics* self) {
-        self->clearCaches();
-      });
+      .def(
+          "clearCaches", +[](dart::dynamics::InverseKinematics* self) {
+            self->clearCaches();
+          });
 }
 
 } // namespace python