From 1c1a98b19fac6717cf4ff81f5c5abbc89bf7d22e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Thu, 1 Apr 2021 21:35:20 -0700 Subject: [PATCH] Allow specifying reference frame for TaskSpaceRegion --- .github/workflows/ccpp.yml | 22 +++--- dart/dynamics/InverseKinematics.cpp | 42 +++++++++--- dart/dynamics/InverseKinematics.hpp | 16 ++++- python/dartpy/dynamics/InverseKinematics.cpp | 72 ++++++++++++++++++++ 4 files changed, 129 insertions(+), 23 deletions(-) diff --git a/.github/workflows/ccpp.yml b/.github/workflows/ccpp.yml index 3c460d57e669c..285c22aec5b25 100644 --- 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 index dd601d91e8007..9dcfb954da719 100644 --- 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 index f087c656851db..c3717a68aad26 100644 --- 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 index b5f5a6f59e157..4f2cb02906b64 100644 --- a/python/dartpy/dynamics/InverseKinematics.cpp +++ b/python/dartpy/dynamics/InverseKinematics.cpp @@ -33,6 +33,7 @@ #include #include #include + #include "eigen_geometry_pybind.h" #include "eigen_pybind.h" @@ -43,6 +44,33 @@ namespace python { void InverseKinematics(py::module& m) { + ::py::class_( + 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(), + ::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>( + 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,