diff --git a/.github/workflows/ros-build-test.yml b/.github/workflows/ros-build-test.yml index 2580bdc93..0f3ac9433 100644 --- a/.github/workflows/ros-build-test.yml +++ b/.github/workflows/ros-build-test.yml @@ -30,7 +30,7 @@ jobs: - name: System deps run: | apt-get update - apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev + apt-get install -y git ninja-build liburdfdom-dev liboctomap-dev libassimp-dev checkinstall wget rsync - uses: actions/checkout@v2 with: @@ -46,14 +46,33 @@ jobs: git clone --recurse-submodules https://github.com/leggedrobotics/pinocchio.git src/pinocchio git clone --recurse-submodules https://github.com/leggedrobotics/hpp-fcl.git src/hpp-fcl git clone https://github.com/leggedrobotics/ocs2_robotic_assets.git src/ocs2_robotic_assets + + - name: Install RaiSim + run: | + git clone --depth 1 https://github.com/raisimTech/raisimLib.git -b v1.1.01 src/raisim_tech + cd src/raisim_tech + mkdir build + cd build + cmake .. -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") + make -j4 && checkinstall + + - name: Install ONNX Runtime + run: | + wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz -P tmp/microsoft + tar xf tmp/microsoft/onnxruntime-linux-x64-1.7.0.tgz -C tmp/microsoft + rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/include/ /usr/local/include/onnxruntime + rsync -a tmp/microsoft/onnxruntime-linux-x64-1.7.0/lib/ /usr/local/lib + rsync -a src/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ /usr/local/share/onnxruntime + - name: Build (${{ matrix.build_type }}) shell: bash run: | source /opt/ros/noetic/setup.bash - catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --ignore-pkg ocs2_raisim --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} + catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --cmake-args -DCMAKE_BUILD_TYPE=${{ matrix.build_type }} - name: Test shell: bash run: | source devel_isolated/setup.bash catkin_make_isolated --use-ninja --merge --only-pkg-with-deps ocs2 --catkin-make-args run_tests + catkin_test_results diff --git a/.gitignore b/.gitignore index 15f4690f1..46f83bb33 100644 --- a/.gitignore +++ b/.gitignore @@ -25,3 +25,4 @@ ocs2_ddp/test/ddp_test_generated/ *.out *.synctex.gz .vscode/ +runs/ diff --git a/jenkins-pipeline b/jenkins-pipeline index 9c91ed1ea..dbe1a5c14 100644 --- a/jenkins-pipeline +++ b/jenkins-pipeline @@ -1,5 +1,5 @@ library 'continuous_integration_pipeline' -ciPipeline("--ros-distro noetic --publish-doxygen --recipes raisimlib\ +ciPipeline("--ros-distro noetic --publish-doxygen --recipes onnxruntime raisimlib\ --dependencies 'git@github.com:leggedrobotics/hpp-fcl.git;master;git'\ 'git@github.com:leggedrobotics/pinocchio.git;master;git'\ 'git@github.com:leggedrobotics/ocs2_robotic_assets.git;main;git'\ diff --git a/ocs2/package.xml b/ocs2/package.xml index 834b4090e..fcd5dd285 100644 --- a/ocs2/package.xml +++ b/ocs2/package.xml @@ -16,14 +16,17 @@ ocs2_oc ocs2_qp_solver ocs2_ddp + ocs2_slp ocs2_sqp ocs2_ros_interfaces ocs2_python_interface ocs2_pinocchio ocs2_robotic_tools + ocs2_perceptive ocs2_robotic_examples ocs2_thirdparty ocs2_raisim + ocs2_mpcnet diff --git a/ocs2_core/cmake/ocs2_cxx_flags.cmake b/ocs2_core/cmake/ocs2_cxx_flags.cmake index 1735d016f..7f38eb333 100644 --- a/ocs2_core/cmake/ocs2_cxx_flags.cmake +++ b/ocs2_core/cmake/ocs2_cxx_flags.cmake @@ -22,5 +22,5 @@ list(APPEND OCS2_CXX_FLAGS ) # Cpp standard version -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) diff --git a/ocs2_core/include/ocs2_core/Types.h b/ocs2_core/include/ocs2_core/Types.h index d09cc26c0..3c161783f 100644 --- a/ocs2_core/include/ocs2_core/Types.h +++ b/ocs2_core/include/ocs2_core/Types.h @@ -71,17 +71,8 @@ using matrix_array2_t = std::vector; /** Array of arrays of dynamic matrix trajectory type. */ using matrix_array3_t = std::vector; -/** Eigen scalar type. */ -using eigen_scalar_t = Eigen::Matrix; -/** Eigen scalar trajectory type. */ -using eigen_scalar_array_t = std::vector; -/** Array of eigen scalar trajectory type. */ -using eigen_scalar_array2_t = std::vector; -/** Array of arrays of eigen scalar trajectory type. */ -using eigen_scalar_array3_t = std::vector; - /** - * Defines the linear approximation + * Defines the linear approximation of a scalar function * f(x,u) = dfdx' dx + dfdu' du + f */ struct ScalarFunctionLinearApproximation { @@ -134,7 +125,7 @@ std::ostream& operator<<(std::ostream& out, const ScalarFunctionLinearApproximat * * @param[in] stateDim: Number of states. * @param[in] inputDim: Number of inputs. - * @param[in] data: Given quadratic approximation. + * @param[in] data: Given linear approximation. * @param[in] dataName: The name of the data which appears in the output error message. * @return The description of the error. If there was no error it would be empty; */ @@ -148,7 +139,7 @@ inline ScalarFunctionLinearApproximation operator*(scalar_t scalar, ScalarFuncti } /** - * Defines the quadratic approximation + * Defines the quadratic approximation of a scalar function * f(x,u) = 1/2 dx' dfdxx dx + du' dfdux dx + 1/2 du' dfduu du + dfdx' dx + dfdu' du + f */ struct ScalarFunctionQuadraticApproximation { diff --git a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h index 3bb8a0d53..79b112fce 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateConstraintCollection.h @@ -49,11 +49,14 @@ class StateConstraintCollection : public Collection { ~StateConstraintCollection() override = default; StateConstraintCollection* clone() const override; - /** Returns the number of active constraints at given time. */ - virtual size_t getNumConstraints(scalar_t time) const; + /** Returns the number of active constraints at a given time. */ + size_t getNumConstraints(scalar_t time) const; - /** Get the constraint vector value */ - virtual vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const; + /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */ + size_array_t getTermsSize(scalar_t time) const; + + /** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */ + virtual vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const; /** Get the constraint linear approximation */ virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, @@ -68,4 +71,4 @@ class StateConstraintCollection : public Collection { StateConstraintCollection(const StateConstraintCollection& other); }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h index f102c593b..aecee543f 100644 --- a/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h +++ b/ocs2_core/include/ocs2_core/constraint/StateInputConstraintCollection.h @@ -49,11 +49,14 @@ class StateInputConstraintCollection : public Collection { ~StateInputConstraintCollection() override = default; StateInputConstraintCollection* clone() const override; - /** Returns the number of active constraints at given time. */ - virtual size_t getNumConstraints(scalar_t time) const; + /** Returns the number of active constraints at a given time. */ + size_t getNumConstraints(scalar_t time) const; - /** Get the constraint vector value */ - virtual vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const; + /** Returns the number of active constraints at a given time for each term. If a term is inactive, its size is zero. */ + size_array_t getTermsSize(scalar_t time) const; + + /** Get an array of all constraints. If a term is inactive, the corresponding element is a vector of size zero. */ + virtual vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const; /** Get the constraint linear approximation */ virtual VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, @@ -68,4 +71,4 @@ class StateInputConstraintCollection : public Collection { StateInputConstraintCollection(const StateInputConstraintCollection& other); }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/control/ControllerType.h b/ocs2_core/include/ocs2_core/control/ControllerType.h index 65aa1962f..bbae196ee 100644 --- a/ocs2_core/include/ocs2_core/control/ControllerType.h +++ b/ocs2_core/include/ocs2_core/control/ControllerType.h @@ -34,6 +34,6 @@ namespace ocs2 { /** * Enum class for specifying controller type */ -enum class ControllerType { UNKNOWN, FEEDFORWARD, LINEAR, NEURAL_NETWORK }; +enum class ControllerType { UNKNOWN, FEEDFORWARD, LINEAR, ONNX, BEHAVIORAL }; } // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h b/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h index 6adb50176..37088b127 100644 --- a/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h +++ b/ocs2_core/include/ocs2_core/integration/TrapezoidalIntegration.h @@ -29,22 +29,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include #include namespace ocs2 { -template -SCALAR_T trapezoidalIntegration(const std::vector& timeTrajectory, const std::vector& valueTrajectory) { +/** + * Compute the trapezoidal integration of a trajectory of VALUE_T given the time stamp timeTrajectory and initial value initialValue. + * + * @note It requires that the VALUE_T has overwrite operator+(VALUE_T, VALUE_T) and define VALUE_T::operator*(SCALAR_T) + */ +template +VALUE_T trapezoidalIntegration(const std::vector& timeTrajectory, const std::vector& valueTrajectory, + VALUE_T initialValue) { + assert(timeTrajectory.size() == valueTrajectory.size()); + if (timeTrajectory.size() < 2) { - return 0.0; + return initialValue; } - SCALAR_T areaUnderCurve = 0.0; for (size_t k = 1; k < timeTrajectory.size(); k++) { - areaUnderCurve += 0.5 * (valueTrajectory[k] + valueTrajectory[k - 1]) * (timeTrajectory[k] - timeTrajectory[k - 1]); + auto temp = valueTrajectory[k - 1] + valueTrajectory[k]; + temp *= (0.5 * (timeTrajectory[k] - timeTrajectory[k - 1])); + initialValue += temp; } // end of k loop - return areaUnderCurve; + return initialValue; } } // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h index 0c94db368..7e96eb387 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateConstraint.h @@ -47,9 +47,10 @@ class LoopshapingStateConstraint final : public StateConstraintCollection { LoopshapingStateConstraint* clone() const override { return new LoopshapingStateConstraint(*this); } - vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + vector_array_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; diff --git a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h index 7314c1182..14830ed3c 100644 --- a/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h +++ b/ocs2_core/include/ocs2_core/loopshaping/constraint/LoopshapingStateInputConstraint.h @@ -41,7 +41,7 @@ class LoopshapingStateInputConstraint : public StateInputConstraintCollection { public: ~LoopshapingStateInputConstraint() override = default; - vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; + vector_array_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; protected: LoopshapingStateInputConstraint(const StateInputConstraintCollection& systemConstraint, diff --git a/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h b/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h index b16175052..29c23a8d4 100644 --- a/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h +++ b/ocs2_core/include/ocs2_core/misc/LinearAlgebra.h @@ -38,41 +38,18 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace LinearAlgebra { -// forward declarations -void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue); - -void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue); - -void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, matrix_t& DmDagger, matrix_t& DmDaggerTRmDmDaggerUUT, - matrix_t& RmInvConstrainedUUT); - /** * Set the eigenvalues of a triangular matrix to a minimum magnitude (maintaining the sign). */ -inline void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon()) { - for (Eigen::Index i = 0; i < Lr.rows(); ++i) { - scalar_t& eigenValue = Lr(i, i); // diagonal element is the eigenvalue - if (eigenValue < 0.0) { - eigenValue = std::min(-minEigenValue, eigenValue); - } else { - eigenValue = std::max(minEigenValue, eigenValue); - } - } -} +void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue = numeric_traits::weakEpsilon()); /** * Makes the input matrix PSD using a eigenvalue decomposition. * - * @tparam Derived type. - * @param squareMatrix: The matrix to become PSD. + * @param [in, out] squareMatrix: The matrix to become PSD. * @param [in] minEigenvalue: minimum eigenvalue. */ -template -void makePsdEigenvalue(Eigen::MatrixBase& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon()) { - matrix_t mat = squareMatrix; - makePsdEigenvalue(mat, minEigenvalue); - squareMatrix = mat; -} +void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon()); /** * Makes the input matrix PSD based on Gershgorin circle theorem. If the input matrix is positive definite and diagonally dominant, @@ -89,20 +66,10 @@ void makePsdEigenvalue(Eigen::MatrixBase& squareMatrix, scalar_t minEig * (1) Aii < minEigenvalue + Ri ==> minEigenvalue < lambda < minEigenvalue + 2 Ri * (2) Aii > minEigenvalue + Ri ==> minEigenvalue < Aii - Ri < lambda < Aii + Ri * - * @tparam Derived type. - * @param squareMatrix: The matrix to become PSD. + * @param [in, out] squareMatrix: The matrix to become PSD. * @param [in] minEigenvalue: minimum eigenvalue. */ -template -void makePsdGershgorin(Eigen::MatrixBase& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon()) { - assert(squareMatrix.rows() == squareMatrix.cols()); - squareMatrix = 0.5 * (squareMatrix + squareMatrix.transpose()).eval(); - for (size_t i = 0; i < squareMatrix.rows(); i++) { - // Gershgorin radius: since the matrix is symmetric we use column sum instead of row sum - auto Ri = squareMatrix.col(i).cwiseAbs().sum() - std::abs(squareMatrix(i, i)); - squareMatrix(i, i) = std::max(squareMatrix(i, i), Ri + minEigenvalue); - } -} +void makePsdGershgorin(matrix_t& squareMatrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon()); /** * Makes the input matrix PSD based on modified Cholesky decomposition. @@ -116,32 +83,19 @@ void makePsdGershgorin(Eigen::MatrixBase& squareMatrix, scalar_t minEig * References : C-J. Lin and J. J. Moré, Incomplete Cholesky Factorizations with Limited memory, SIAM J. Sci. Comput. * 21(1), pp. 24-45, 1999 * - * @tparam Derived type. - * @param A: The matrix to become PSD. + * @param [in, out] A: The matrix to become PSD. * @param [in] minEigenvalue: minimum eigenvalue. */ -template -void makePsdCholesky(Eigen::MatrixBase& A, scalar_t minEigenvalue = numeric_traits::limitEpsilon()) { - matrix_t mat = A; - makePsdCholesky(mat, minEigenvalue); - A = mat; -} +void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue = numeric_traits::limitEpsilon()); /** * Computes the U*U^T decomposition associated to the inverse of the input matrix, where U is an upper triangular * matrix. Note that the U*U^T decomposition is different from the Cholesky decomposition (U^T*U). * - * @tparam Derived type. * @param [in] Am: A symmetric square positive definite matrix * @param [out] AmInvUmUmT: The upper-triangular matrix associated to the UUT decomposition of inv(Am) matrix. */ -template -void computeInverseMatrixUUT(const Derived& Am, Derived& AmInvUmUmT) { - // Am = Lm Lm^T --> inv(Am) = inv(Lm^T) inv(Lm) where Lm^T is upper triangular - Eigen::LLT lltOfA(Am); - AmInvUmUmT.setIdentity(Am.rows(), Am.cols()); // for dynamic size matrices - lltOfA.matrixU().solveInPlace(AmInvUmUmT); -} +void computeInverseMatrixUUT(const matrix_t& Am, matrix_t& AmInvUmUmT); /** * Computes constraint projection for linear constraints C*x + D*u - e = 0, with the weighting inv(Rm) @@ -154,11 +108,36 @@ void computeInverseMatrixUUT(const Derived& Am, Derived& AmInvUmUmT) { * @param [out] RmInvConstrainedUUT: The VVT decomposition of (I-DmDagger*Dm) * inv(Rm) * (I-DmDagger*Dm)^T where V is of * the dimension n_u*(n_u-n_c) with n_u = Rm.rows() and n_c = Dm.rows(). */ -template -void computeConstraintProjection(const matrix_t& Dm, const DerivedInputMatrix& RmInvUmUmT, matrix_t& DmDagger, - matrix_t& DmDaggerTRmDmDaggerUUT, matrix_t& RmInvConstrainedUUT) { - computeConstraintProjection(Dm, matrix_t(RmInvUmUmT), DmDagger, DmDaggerTRmDmDaggerUUT, RmInvConstrainedUUT); -} +void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, matrix_t& DmDagger, matrix_t& DmDaggerTRmDmDaggerUUT, + matrix_t& RmInvConstrainedUUT); + +/** + * Returns the linear projection + * u = Pu * \tilde{u} + Px * x + Pe + * + * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u} + * + * Implementation based on the QR decomposition + * + * @param [in] constraint : C = dfdx, D = dfdu, e = f; + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second); + */ +std::pair qrConstraintProjection(const VectorFunctionLinearApproximation& constraint); + +/** + * Returns the linear projection + * u = Pu * \tilde{u} + Px * x + Pe + * + * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u} + * + * Implementation based on the LU decomposition + * + * @param [in] constraint : C = dfdx, D = dfdu, e = f; + * @param [in] extractPseudoInverse : If true, left pseudo-inverse of D^T is returned. If false, an empty matrix is returned; + * @return Projection terms Px = dfdx, Pu = dfdu, Pe = f (first) and left pseudo-inverse of D^T (second); + */ +std::pair luConstraintProjection(const VectorFunctionLinearApproximation& constraint, + bool extractPseudoInverse = false); /** Computes the rank of a matrix */ template diff --git a/ocs2_core/include/ocs2_core/misc/Numerics.h b/ocs2_core/include/ocs2_core/misc/Numerics.h index 5003791ce..d2e45d2f4 100644 --- a/ocs2_core/include/ocs2_core/misc/Numerics.h +++ b/ocs2_core/include/ocs2_core/misc/Numerics.h @@ -42,22 +42,39 @@ namespace numerics { * * @tparam T1: data type of x. * @tparam T2: data type of y. - * @param [in] x: a floating-point number. - * @param [in] y: a floating-point number. + * @tparam T3: data type of prec. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @param [in] prec: The comparison precision. * @return bool: true if x=y. */ -template -typename std::enable_if::type>::value || - std::is_floating_point::type>::value, - bool>::type -almost_eq(T1&& x, T2&& y) { - using TypeResult = typename std::conditional::type>::value, - typename std::remove_reference::type, typename std::remove_reference::type>::type; +template +bool almost_eq(T1&& x, T2&& y, T3&& prec) { + static_assert(std::is_floating_point::type>::value, "First argument is not floating point!"); + static_assert(std::is_floating_point::type>::value, "Second argument is not floating point!"); + static_assert(std::is_floating_point::type>::value, "prec is not floating point!"); // the machine epsilon has to be scaled to the magnitude of the values used - // and multiplied by the desired precision in ULPs (units in the last place) - return std::abs(x - y) <= std::numeric_limits::epsilon() * std::abs(x + y) - // unless the result is subnormal - || std::abs(x - y) < std::numeric_limits::min(); + // and multiplied by the desired precision unless the result is subnormal + using Type = const std::remove_reference_t; + const auto absDiff = std::abs(x - static_cast(y)); + const auto magnitude = std::min(std::abs(x), std::abs(static_cast(y))); + return absDiff <= static_cast(prec) * magnitude || absDiff < std::numeric_limits::min(); +} + +/** + * Almost equal which uses machine epsilon to compare floating-point values for equality. + * refer to: https://en.cppreference.com/w/cpp/types/numeric_limits/epsilon + * + * @tparam T1: data type of x. + * @tparam T2: data type of y. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @return bool: true if x=y. + */ +template +bool almost_eq(T1&& x, T2&& y) { + const auto prec = std::numeric_limits>::epsilon(); + return almost_eq(x, y, prec); } /** @@ -65,15 +82,28 @@ almost_eq(T1&& x, T2&& y) { * * @tparam T1: data type of x. * @tparam T2: data type of y. - * @param [in] x: a floating-point number. - * @param [in] y: a floating-point number. + * @tparam T3: data type of prec. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @param [in] prec: The comparison precision. * @return bool: true if x<=y. */ -template -typename std::enable_if::type>::value || - std::is_floating_point::type>::value, - bool>::type -almost_le(T1&& x, T2&& y) { +template +bool almost_le(T1&& x, T2&& y, T3&& prec) { + return x < y || almost_eq(x, y, prec); +} + +/** + * Almost less-equal which uses machine epsilon to compare floating-point values for equality. + * + * @tparam T1: data type of x. + * @tparam T2: data type of y. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @return bool: true if x<=y. + */ +template +bool almost_le(T1&& x, T2&& y) { return x < y || almost_eq(x, y); } @@ -82,15 +112,28 @@ almost_le(T1&& x, T2&& y) { * * @tparam T1: data type of x. * @tparam T2: data type of y. - * @param [in] x: a floating-point number. - * @param [in] y: a floating-point number. + * @tparam T3: data type of prec. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. + * @param [in] prec: The comparison precision. + * @return bool: true if x>=y. + */ +template +bool almost_ge(T1&& x, T2&& y, T3&& prec) { + return x > y || almost_eq(x, y, prec); +} + +/** + * Almost greater-equal which uses machine epsilon to compare floating-point values for equality. + * + * @tparam T1: data type of x. + * @tparam T2: data type of y. + * @param [in] x: First floating-point number. + * @param [in] y: Second floating-point number. * @return bool: true if x>=y. */ template -typename std::enable_if::type>::value || - std::is_floating_point::type>::value, - bool>::type -almost_ge(T1&& x, T2&& y) { +bool almost_ge(T1&& x, T2&& y) { return x > y || almost_eq(x, y); } diff --git a/ocs2_core/include/ocs2_core/model_data/Metrics.h b/ocs2_core/include/ocs2_core/model_data/Metrics.h index 2d205d377..dc01f54d5 100644 --- a/ocs2_core/include/ocs2_core/model_data/Metrics.h +++ b/ocs2_core/include/ocs2_core/model_data/Metrics.h @@ -53,23 +53,33 @@ struct LagrangianMetricsConstRef { }; /** - * The collection of cost, equality constraints, and LagrangianMetrics structure for all possible constraint terms (handled by - * Lagrangian method) in a particular time point. - * cost : The total cost in a particular time point. - * stateEqConstraint : A vector of all active state equality constraints. - * stateInputEqConstraint : A vector of all active state-input equality constraints. - * stateEqLagrangian : An array of state equality constraint terms handled by Lagrangian method. - * stateIneqLagrangian : An array of state inequality constraint terms handled by Lagrangian method. - * stateInputEqLagrangian : An array of state-input equality constraint terms handled by Lagrangian method. - * stateInputIneqLagrangian : An array of state-input inequality constraint terms handled by Lagrangian method. + * The collection of cost, dynamics violation, constraints, and LagrangianMetrics structure for all possible constraint + * terms (handled by Lagrangian method) in a point of time. + * cost : The total cost in a particular time point. + * dynamicsViolation : The vector of dynamics violation. + * stateEqConstraint : An array of all state equality constraints. + * stateInputEqConstraint : An array of all state-input equality constraints. + * stateIneqConstraint : An array of all state inequality constraints. + * stateInputIneqConstraint : An array of all state-input inequality constraints. + * stateEqLagrangian : An array of state equality constraint terms handled by Lagrangian method. + * stateIneqLagrangian : An array of state inequality constraint terms handled by Lagrangian method. + * stateInputEqLagrangian : An array of state-input equality constraint terms handled by Lagrangian method. + * stateInputIneqLagrangian : An array of state-input inequality constraint terms handled by Lagrangian method. */ -struct MetricsCollection { +struct Metrics { // Cost scalar_t cost; + // Dynamics violation + vector_t dynamicsViolation; + // Equality constraints - vector_t stateEqConstraint; - vector_t stateInputEqConstraint; + vector_array_t stateEqConstraint; + vector_array_t stateInputEqConstraint; + + // Inequality constraints + vector_array_t stateIneqConstraint; + vector_array_t stateInputIneqConstraint; // Lagrangians std::vector stateEqLagrangian; @@ -77,49 +87,62 @@ struct MetricsCollection { std::vector stateInputEqLagrangian; std::vector stateInputIneqLagrangian; - /** Exchanges the values of MetricsCollection */ - void swap(MetricsCollection& other) { - // Cost - std::swap(cost, other.cost); - // Equality constraints - stateEqConstraint.swap(other.stateEqConstraint); - stateInputEqConstraint.swap(other.stateInputEqConstraint); - // Lagrangians - stateEqLagrangian.swap(other.stateEqLagrangian); - stateIneqLagrangian.swap(other.stateIneqLagrangian); - stateInputEqLagrangian.swap(other.stateInputEqLagrangian); - stateInputIneqLagrangian.swap(other.stateInputIneqLagrangian); - } + /** Exchanges the values of Metrics */ + void swap(Metrics& other); - /** Clears the value of the MetricsCollection */ - void clear() { - // Cost - cost = 0.0; - // Equality constraints - stateEqConstraint = vector_t(); - stateInputEqConstraint = vector_t(); - // Lagrangians - stateEqLagrangian.clear(); - stateIneqLagrangian.clear(); - stateInputEqLagrangian.clear(); - stateInputIneqLagrangian.clear(); - } + /** Clears the value of the Metrics */ + void clear(); + + /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ + bool isApprox(const Metrics& other, scalar_t prec = 1e-8) const; }; -/** Sums penalties of an array of LagrangianMetrics */ +/** Sums penalties of an array of LagrangianMetrics. */ inline scalar_t sumPenalties(const std::vector& metricsArray) { scalar_t s = 0.0; std::for_each(metricsArray.begin(), metricsArray.end(), [&s](const LagrangianMetrics& m) { s += m.penalty; }); return s; } -/** Computes the sum of squared norm of constraints of an array of LagrangianMetrics */ +/** Computes the sum of squared norm of constraints of an array of LagrangianMetrics. */ inline scalar_t constraintsSquaredNorm(const std::vector& metricsArray) { scalar_t s = 0.0; std::for_each(metricsArray.begin(), metricsArray.end(), [&s](const LagrangianMetrics& m) { s += m.constraint.squaredNorm(); }); return s; } +/** Computes the sum of squared norm of a vector of equality constraints violation. */ +inline scalar_t getEqConstraintsSSE(const vector_t& eqConstraint) { + if (eqConstraint.size() == 0) { + return 0.0; + } else { + return eqConstraint.squaredNorm(); + } +} + +/** Computes the sum of squared norm of a vector of inequality constraints violation. */ +inline scalar_t getIneqConstraintsSSE(const vector_t& ineqConstraint) { + if (ineqConstraint.size() == 0) { + return 0.0; + } else { + return ineqConstraint.cwiseMin(0.0).squaredNorm(); + } +} + +/** Computes the sum of squared norm of an array of equality constraints violation. */ +inline scalar_t getEqConstraintsSSE(const vector_array_t& eqConstraint) { + scalar_t s = 0.0; + std::for_each(eqConstraint.begin(), eqConstraint.end(), [&s](const vector_t& v) { s += getEqConstraintsSSE(v); }); + return s; +} + +/** Computes the sum of squared norm of an array of inequality constraints violation. */ +inline scalar_t getIneqConstraintsSSE(const vector_array_t& ineqConstraint) { + scalar_t s = 0.0; + std::for_each(ineqConstraint.begin(), ineqConstraint.end(), [&s](const vector_t& v) { s += getIneqConstraintsSSE(v); }); + return s; +} + /** * Serializes an array of LagrangianMetrics structures associated to an array of constraint terms. * @@ -128,14 +151,40 @@ inline scalar_t constraintsSquaredNorm(const std::vector& met */ vector_t toVector(const std::vector& termsLagrangianMetrics); +/** + * Serializes an array of constraint terms. + * + * @ param [in] constraintArray : An array of constraint terms. + * @return Serialized vector of the format : (..., constraintArray[i], ...). + */ +vector_t toVector(const vector_array_t& constraintArray); + /** * Gets the size of constraint terms. * + * @ param [in] constraintArray : An array of constraint terms. + * @return An array of constraint terms size. It has the same size as the input array. + */ +size_array_t getSizes(const vector_array_t& constraintArray); + +/** + * Gets the size of constraint Lagrangian terms. + * * @ param [in] termsLagrangianMetrics : LagrangianMetrics associated to an array of constraint terms. * @return An array of constraint terms size. It has the same size as the input array. */ size_array_t getSizes(const std::vector& termsLagrangianMetrics); +/** + * Deserializes the vector to an array of constraint terms. + * + * @param [in] termsSize : An array of constraint terms size. It as the same size as the output array. + * @param [in] vec : Serialized array of constraint terms of the format : + * (..., constraintArray[i], ...) + * @return An array of constraint terms. + */ +vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec); + /** * Deserializes the vector to an array of LagrangianMetrics structures based on size of constraint terms. * @@ -161,13 +210,13 @@ namespace LinearInterpolation { LagrangianMetrics interpolate(const index_alpha_t& indexAlpha, const std::vector& dataArray); /** - * Linearly interpolates a trajectory of MetricsCollection. + * Linearly interpolates a trajectory of Metrics. * * @param [in] indexAlpha : index and interpolation coefficient (alpha) pair. - * @param [in] dataArray : A trajectory of MetricsCollection. - * @return The interpolated MetricsCollection at indexAlpha. + * @param [in] dataArray : A trajectory of Metrics. + * @return The interpolated Metrics at indexAlpha. */ -MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector& dataArray); +Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector& dataArray); } // namespace LinearInterpolation } // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h b/ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h index 239e20b53..a5bb74645 100644 --- a/ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h +++ b/ocs2_core/include/ocs2_core/penalties/augmented/ModifiedRelaxedBarrierPenalty.h @@ -76,7 +76,7 @@ class ModifiedRelaxedBarrierPenalty final : public AugmentedPenaltyBase { /** Factory function */ static std::unique_ptr create(Config config) { - return std::unique_ptr(new ModifiedRelaxedBarrierPenalty(std::move(config))); + return std::make_unique(std::move(config)); } ~ModifiedRelaxedBarrierPenalty() override = default; diff --git a/ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h b/ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h index 317516ab5..7a8b0578c 100644 --- a/ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h +++ b/ocs2_core/include/ocs2_core/penalties/augmented/QuadraticPenalty.h @@ -72,9 +72,7 @@ class QuadraticPenalty final : public AugmentedPenaltyBase { explicit QuadraticPenalty(Config config) : config_(std::move(config)) {} /** Factory function */ - static std::unique_ptr create(Config config) { - return std::unique_ptr(new QuadraticPenalty(std::move(config))); - } + static std::unique_ptr create(Config config) { return std::make_unique(std::move(config)); } ~QuadraticPenalty() override = default; QuadraticPenalty* clone() const override { return new QuadraticPenalty(*this); } diff --git a/ocs2_core/include/ocs2_core/penalties/augmented/SlacknessSquaredHingePenalty.h b/ocs2_core/include/ocs2_core/penalties/augmented/SlacknessSquaredHingePenalty.h index f4ca9da26..09cadba0f 100644 --- a/ocs2_core/include/ocs2_core/penalties/augmented/SlacknessSquaredHingePenalty.h +++ b/ocs2_core/include/ocs2_core/penalties/augmented/SlacknessSquaredHingePenalty.h @@ -75,7 +75,7 @@ class SlacknessSquaredHingePenalty final : public AugmentedPenaltyBase { /** Factory function */ static std::unique_ptr create(Config config) { - return std::unique_ptr(new SlacknessSquaredHingePenalty(std::move(config))); + return std::make_unique(std::move(config)); } ~SlacknessSquaredHingePenalty() override = default; diff --git a/ocs2_core/include/ocs2_core/penalties/augmented/SmoothAbsolutePenalty.h b/ocs2_core/include/ocs2_core/penalties/augmented/SmoothAbsolutePenalty.h index 836ba28b5..d6f1e6b0f 100644 --- a/ocs2_core/include/ocs2_core/penalties/augmented/SmoothAbsolutePenalty.h +++ b/ocs2_core/include/ocs2_core/penalties/augmented/SmoothAbsolutePenalty.h @@ -76,9 +76,7 @@ class SmoothAbsolutePenalty final : public AugmentedPenaltyBase { explicit SmoothAbsolutePenalty(Config config) : config_(std::move(config)) {} /** Factory function */ - static std::unique_ptr create(Config config) { - return std::unique_ptr(new SmoothAbsolutePenalty(std::move(config))); - } + static std::unique_ptr create(Config config) { return std::make_unique(std::move(config)); } ~SmoothAbsolutePenalty() override = default; SmoothAbsolutePenalty* clone() const override { return new SmoothAbsolutePenalty(*this); } diff --git a/ocs2_core/include/ocs2_core/thread_support/BufferedValue.h b/ocs2_core/include/ocs2_core/thread_support/BufferedValue.h index 9fc28dca7..8c1464b78 100644 --- a/ocs2_core/include/ocs2_core/thread_support/BufferedValue.h +++ b/ocs2_core/include/ocs2_core/thread_support/BufferedValue.h @@ -58,10 +58,10 @@ class BufferedValue { T& get() { return activeValue_; } /** Copy a new value into the buffer. */ - void setBuffer(const T& value) { buffer_.reset(std::unique_ptr(new T(value))); } + void setBuffer(const T& value) { buffer_.reset(std::make_unique(value)); } /** Move a new value into the buffer. */ - void setBuffer(T&& value) { buffer_.reset(std::unique_ptr(new T(std::move(value)))); } + void setBuffer(T&& value) { buffer_.reset(std::make_unique(std::move(value))); } /** * Replaces the active value with the value in the buffer. @@ -87,4 +87,4 @@ class BufferedValue { Synchronized buffer_; }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/include/ocs2_core/thread_support/ThreadPool.h b/ocs2_core/include/ocs2_core/thread_support/ThreadPool.h index 1ae4324dd..106e9fc21 100644 --- a/ocs2_core/include/ocs2_core/thread_support/ThreadPool.h +++ b/ocs2_core/include/ocs2_core/thread_support/ThreadPool.h @@ -141,7 +141,7 @@ struct ThreadPool::Task final : public ThreadPool::TaskBase { /**************************************************************************************************/ template std::future::type> ThreadPool::run(Functor taskFunction) { - std::unique_ptr> taskPtr(new Task(std::move(taskFunction))); + auto taskPtr = std::make_unique>(std::move(taskFunction)); auto future = taskPtr->packagedTask.get_future(); if (workerThreads_.empty()) { diff --git a/ocs2_core/src/Types.cpp b/ocs2_core/src/Types.cpp index c6d4e4583..3645fa031 100644 --- a/ocs2_core/src/Types.cpp +++ b/ocs2_core/src/Types.cpp @@ -446,6 +446,8 @@ VectorFunctionQuadraticApproximation& VectorFunctionQuadraticApproximation::setZ } } else { dfdu = matrix_t(); + dfdux.resize(nv); + dfduu.resize(nv); for (size_t i = 0; i < nv; i++) { dfdux[i] = matrix_t(); dfduu[i] = matrix_t(); diff --git a/ocs2_core/src/augmented_lagrangian/AugmentedLagrangian.cpp b/ocs2_core/src/augmented_lagrangian/AugmentedLagrangian.cpp index 01db3586e..d24cb8746 100644 --- a/ocs2_core/src/augmented_lagrangian/AugmentedLagrangian.cpp +++ b/ocs2_core/src/augmented_lagrangian/AugmentedLagrangian.cpp @@ -36,7 +36,7 @@ namespace ocs2 { /******************************************************************************************************/ std::unique_ptr create(std::unique_ptr constraintPtr, std::vector> penaltyPtrArray) { - return std::unique_ptr(new StateAugmentedLagrangian(std::move(constraintPtr), std::move(penaltyPtrArray))); + return std::make_unique(std::move(constraintPtr), std::move(penaltyPtrArray)); } /******************************************************************************************************/ @@ -44,7 +44,7 @@ std::unique_ptr create(std::unique_ptr create(std::unique_ptr constraintPtr, std::unique_ptr penaltyPtr) { - return std::unique_ptr(new StateAugmentedLagrangian(std::move(constraintPtr), std::move(penaltyPtr))); + return std::make_unique(std::move(constraintPtr), std::move(penaltyPtr)); } /******************************************************************************************************/ @@ -52,8 +52,7 @@ std::unique_ptr create(std::unique_ptr create(std::unique_ptr constraintPtr, std::vector> penaltyPtrArray) { - return std::unique_ptr( - new StateInputAugmentedLagrangian(std::move(constraintPtr), std::move(penaltyPtrArray))); + return std::make_unique(std::move(constraintPtr), std::move(penaltyPtrArray)); } /******************************************************************************************************/ @@ -61,7 +60,7 @@ std::unique_ptr create(std::unique_ptr create(std::unique_ptr constraintPtr, std::unique_ptr penaltyPtr) { - return std::unique_ptr(new StateInputAugmentedLagrangian(std::move(constraintPtr), std::move(penaltyPtr))); + return std::make_unique(std::move(constraintPtr), std::move(penaltyPtr)); } } // namespace ocs2 diff --git a/ocs2_core/src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp b/ocs2_core/src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp index 4189cffcf..b002109f9 100644 --- a/ocs2_core/src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp +++ b/ocs2_core/src/augmented_lagrangian/StateAugmentedLagrangianCollection.cpp @@ -90,10 +90,18 @@ ScalarFunctionQuadraticApproximation StateAugmentedLagrangianCollection::getQuad // accumulate terms for (size_t i = firstActiveInd + 1; i < terms_.size(); i++) { if (terms_[i]->isActive(time)) { - penalty += terms_[i]->getQuadraticApproximation(time, state, termsMultiplier[i], preComp); + const auto termPenalty = terms_[i]->getQuadraticApproximation(time, state, termsMultiplier[i], preComp); + penalty.f += termPenalty.f; + penalty.dfdx += termPenalty.dfdx; + penalty.dfdxx += termPenalty.dfdxx; } } + // make sure that input derivatives are empty + penalty.dfdu = vector_t(); + penalty.dfduu = matrix_t(); + penalty.dfdux = matrix_t(); + return penalty; } diff --git a/ocs2_core/src/constraint/StateConstraintCollection.cpp b/ocs2_core/src/constraint/StateConstraintCollection.cpp index 77086ea07..94dc0634a 100644 --- a/ocs2_core/src/constraint/StateConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateConstraintCollection.cpp @@ -48,34 +48,37 @@ StateConstraintCollection* StateConstraintCollection::clone() const { /******************************************************************************************************/ size_t StateConstraintCollection::getNumConstraints(scalar_t time) const { size_t numConstraints = 0; - - // accumulate number of constraints for each constraintTerm for (const auto& constraintTerm : this->terms_) { if (constraintTerm->isActive(time)) { numConstraints += constraintTerm->getNumConstraints(time); } } - return numConstraints; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_t StateConstraintCollection::getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const { - vector_t constraintValues; - constraintValues.resize(getNumConstraints(time)); - - // append vectors of constraint values from each constraintTerm - size_t i = 0; - for (const auto& constraintTerm : this->terms_) { - if (constraintTerm->isActive(time)) { - const auto constraintTermValues = constraintTerm->getValue(time, state, preComp); - constraintValues.segment(i, constraintTermValues.rows()) = constraintTermValues; - i += constraintTermValues.rows(); +size_array_t StateConstraintCollection::getTermsSize(scalar_t time) const { + size_array_t termsSize(this->terms_.size(), 0); + for (size_t i = 0; i < this->terms_.size(); ++i) { + if (this->terms_[i]->isActive(time)) { + termsSize[i] = this->terms_[i]->getNumConstraints(time); } } + return termsSize; +} +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_array_t StateConstraintCollection::getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const { + vector_array_t constraintValues(this->terms_.size()); + for (size_t i = 0; i < this->terms_.size(); ++i) { + if (this->terms_[i]->isActive(time)) { + constraintValues[i] = this->terms_[i]->getValue(time, state, preComp); + } + } // end of i loop return constraintValues; } diff --git a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp index 44264a06b..ae3daa870 100644 --- a/ocs2_core/src/constraint/StateInputConstraintCollection.cpp +++ b/ocs2_core/src/constraint/StateInputConstraintCollection.cpp @@ -49,35 +49,38 @@ StateInputConstraintCollection* StateInputConstraintCollection::clone() const { /******************************************************************************************************/ size_t StateInputConstraintCollection::getNumConstraints(scalar_t time) const { size_t numConstraints = 0; - - // accumulate number of constraints for each constraintTerm for (const auto& constraintTerm : this->terms_) { if (constraintTerm->isActive(time)) { numConstraints += constraintTerm->getNumConstraints(time); } } - return numConstraints; } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_t StateInputConstraintCollection::getValue(scalar_t time, const vector_t& state, const vector_t& input, - const PreComputation& preComp) const { - vector_t constraintValues; - constraintValues.resize(getNumConstraints(time)); - - // append vectors of constraint values from each constraintTerm - size_t i = 0; - for (const auto& constraintTerm : this->terms_) { - if (constraintTerm->isActive(time)) { - const auto constraintTermValues = constraintTerm->getValue(time, state, input, preComp); - constraintValues.segment(i, constraintTermValues.rows()) = constraintTermValues; - i += constraintTermValues.rows(); +size_array_t StateInputConstraintCollection::getTermsSize(scalar_t time) const { + size_array_t termsSize(this->terms_.size(), 0); + for (size_t i = 0; i < this->terms_.size(); ++i) { + if (this->terms_[i]->isActive(time)) { + termsSize[i] = this->terms_[i]->getNumConstraints(time); } } + return termsSize; +} +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_array_t StateInputConstraintCollection::getValue(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const { + vector_array_t constraintValues(this->terms_.size()); + for (size_t i = 0; i < this->terms_.size(); ++i) { + if (this->terms_[i]->isActive(time)) { + constraintValues[i] = this->terms_[i]->getValue(time, state, input, preComp); + } + } // end of i loop return constraintValues; } diff --git a/ocs2_core/src/integration/Integrator.cpp b/ocs2_core/src/integration/Integrator.cpp index 01efb48d3..e7f1ab696 100644 --- a/ocs2_core/src/integration/Integrator.cpp +++ b/ocs2_core/src/integration/Integrator.cpp @@ -80,24 +80,24 @@ IntegratorType fromString(const std::string& name) { std::unique_ptr newIntegrator(IntegratorType integratorType, const std::shared_ptr& eventHandlerPtr) { switch (integratorType) { case (IntegratorType::EULER): - return std::unique_ptr(new IntegratorEuler(eventHandlerPtr)); + return std::make_unique(eventHandlerPtr); case (IntegratorType::ODE45): - return std::unique_ptr(new ODE45(eventHandlerPtr)); + return std::make_unique(eventHandlerPtr); case (IntegratorType::ODE45_OCS2): - return std::unique_ptr(new RungeKuttaDormandPrince5(eventHandlerPtr)); + return std::make_unique(eventHandlerPtr); case (IntegratorType::ADAMS_BASHFORTH): - return std::unique_ptr(new IntegratorAdamsBashforth<1>(eventHandlerPtr)); + return std::make_unique>(eventHandlerPtr); case (IntegratorType::BULIRSCH_STOER): - return std::unique_ptr(new IntegratorBulirschStoer(eventHandlerPtr)); + return std::make_unique(eventHandlerPtr); case (IntegratorType::MODIFIED_MIDPOINT): - return std::unique_ptr(new IntegratorModifiedMidpoint(eventHandlerPtr)); + return std::make_unique(eventHandlerPtr); case (IntegratorType::RK4): - return std::unique_ptr(new IntegratorRK4(eventHandlerPtr)); + return std::make_unique(eventHandlerPtr); case (IntegratorType::RK5_VARIABLE): - return std::unique_ptr(new IntegratorRK5Variable(eventHandlerPtr)); + return std::make_unique(eventHandlerPtr); #if (BOOST_VERSION / 100000 == 1 && BOOST_VERSION / 100 % 1000 > 55) case (IntegratorType::ADAMS_BASHFORTH_MOULTON): - return std::unique_ptr(new IntegratorAdamsBashforthMoulton<1>(eventHandlerPtr)); + return std::make_unique>(eventHandlerPtr); #endif default: throw std::runtime_error("Integrator of type " + integrator_type::toString(integratorType) + " not supported."); diff --git a/ocs2_core/src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp b/ocs2_core/src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp index 223f8d7c3..9ec9286f5 100644 --- a/ocs2_core/src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp +++ b/ocs2_core/src/loopshaping/augmented_lagrangian/LoopshapingAugmentedLagrangian.cpp @@ -41,8 +41,7 @@ namespace LoopshapingAugmentedLagrangian { /******************************************************************************************************/ std::unique_ptr create(const StateAugmentedLagrangianCollection& systemAugmentedLagrangian, std::shared_ptr loopshapingDefinition) { - return std::unique_ptr( - new LoopshapingStateAugmentedLagrangian(systemAugmentedLagrangian, std::move(loopshapingDefinition))); + return std::make_unique(systemAugmentedLagrangian, std::move(loopshapingDefinition)); } /******************************************************************************************************/ @@ -52,11 +51,9 @@ std::unique_ptr create(const StateInput std::shared_ptr loopshapingDefinition) { switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr( - new LoopshapingAugmentedLagrangianOutputPattern(systemAugmentedLagrangian, std::move(loopshapingDefinition))); + return std::make_unique(systemAugmentedLagrangian, std::move(loopshapingDefinition)); case LoopshapingType::eliminatepattern: - return std::unique_ptr( - new LoopshapingAugmentedLagrangianEliminatePattern(systemAugmentedLagrangian, std::move(loopshapingDefinition))); + return std::make_unique(systemAugmentedLagrangian, std::move(loopshapingDefinition)); default: throw std::runtime_error("[LoopshapingAugmentedLagrangian::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp index 09dc48011..e99e8dc48 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingConstraint.cpp @@ -41,7 +41,7 @@ namespace LoopshapingConstraint { /******************************************************************************************************/ std::unique_ptr create(const StateConstraintCollection& systemConstraint, std::shared_ptr loopshapingDefinition) { - return std::unique_ptr(new LoopshapingStateConstraint(systemConstraint, loopshapingDefinition)); + return std::make_unique(systemConstraint, std::move(loopshapingDefinition)); } /******************************************************************************************************/ @@ -51,11 +51,9 @@ std::unique_ptr create(const StateInputConstrain std::shared_ptr loopshapingDefinition) { switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr( - new LoopshapingConstraintOutputPattern(systemConstraint, std::move(loopshapingDefinition))); + return std::make_unique(systemConstraint, std::move(loopshapingDefinition)); case LoopshapingType::eliminatepattern: - return std::unique_ptr( - new LoopshapingConstraintEliminatePattern(systemConstraint, std::move(loopshapingDefinition))); + return std::make_unique(systemConstraint, std::move(loopshapingDefinition)); default: throw std::runtime_error("[LoopshapingConstraint::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp index 1df9ded24..af67c3761 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingStateConstraint.cpp @@ -38,9 +38,9 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_t LoopshapingStateConstraint::getValue(scalar_t t, const vector_t& x, const PreComputation& preComp) const { +vector_array_t LoopshapingStateConstraint::getValue(scalar_t t, const vector_t& x, const PreComputation& preComp) const { if (this->empty()) { - return vector_t::Zero(0); + return vector_array_t(); } const LoopshapingPreComputation& preCompLS = cast(preComp); diff --git a/ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp b/ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp index 14125e7f2..9ef11c9b8 100644 --- a/ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp +++ b/ocs2_core/src/loopshaping/constraint/LoopshapingStateInputConstraint.cpp @@ -38,9 +38,10 @@ namespace ocs2 { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -vector_t LoopshapingStateInputConstraint::getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation& preComp) const { +vector_array_t LoopshapingStateInputConstraint::getValue(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const { if (this->empty()) { - return vector_t::Zero(0); + return vector_array_t(); } const LoopshapingPreComputation& preCompLS = cast(preComp); diff --git a/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp b/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp index 44a10dc89..df5e301b7 100644 --- a/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp +++ b/ocs2_core/src/loopshaping/cost/LoopshapingCost.cpp @@ -41,7 +41,7 @@ namespace LoopshapingCost { /******************************************************************************************************/ std::unique_ptr create(const StateCostCollection& systemCost, std::shared_ptr loopshapingDefinition) { - return std::unique_ptr(new LoopshapingStateCost(systemCost, loopshapingDefinition)); + return std::make_unique(systemCost, std::move(loopshapingDefinition)); } /******************************************************************************************************/ @@ -51,9 +51,9 @@ std::unique_ptr create(const StateInputCostCollection& std::shared_ptr loopshapingDefinition) { switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr(new LoopshapingCostOutputPattern(systemCost, std::move(loopshapingDefinition))); + return std::make_unique(systemCost, std::move(loopshapingDefinition)); case LoopshapingType::eliminatepattern: - return std::unique_ptr(new LoopshapingCostEliminatePattern(systemCost, std::move(loopshapingDefinition))); + return std::make_unique(systemCost, std::move(loopshapingDefinition)); default: throw std::runtime_error("[LoopshapingStateInputCost::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/loopshaping/dynamics/LoopshapingDynamics.cpp b/ocs2_core/src/loopshaping/dynamics/LoopshapingDynamics.cpp index d57cc1fe4..3ebb53890 100644 --- a/ocs2_core/src/loopshaping/dynamics/LoopshapingDynamics.cpp +++ b/ocs2_core/src/loopshaping/dynamics/LoopshapingDynamics.cpp @@ -125,11 +125,9 @@ std::unique_ptr LoopshapingDynamics::create(const SystemDyn switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr( - new LoopshapingDynamicsOutputPattern(systemDynamics, std::move(loopshapingDefinition), preComputation)); + return std::make_unique(systemDynamics, std::move(loopshapingDefinition), preComputation); case LoopshapingType::eliminatepattern: - return std::unique_ptr( - new LoopshapingDynamicsEliminatePattern(systemDynamics, std::move(loopshapingDefinition), preComputation)); + return std::make_unique(systemDynamics, std::move(loopshapingDefinition), preComputation); default: throw std::runtime_error("[LoopshapingDynamics::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp b/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp index 31d97b7ca..6755ad0f0 100644 --- a/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp +++ b/ocs2_core/src/loopshaping/soft_constraint/LoopshapingSoftConstraint.cpp @@ -42,7 +42,7 @@ namespace LoopshapingSoftConstraint { std::unique_ptr create(const StateCostCollection& systemSoftConstraint, std::shared_ptr loopshapingDefinition) { // State-only soft constraint wrapper is identical to LoopshapingStateCost wrapper. - return std::unique_ptr(new LoopshapingStateCost(systemSoftConstraint, loopshapingDefinition)); + return std::make_unique(systemSoftConstraint, std::move(loopshapingDefinition)); } /******************************************************************************************************/ @@ -52,11 +52,9 @@ std::unique_ptr create(const StateInputCostCollection& std::shared_ptr loopshapingDefinition) { switch (loopshapingDefinition->getType()) { case LoopshapingType::outputpattern: - return std::unique_ptr( - new LoopshapingSoftConstraintOutputPattern(systemSoftConstraint, std::move(loopshapingDefinition))); + return std::make_unique(systemSoftConstraint, std::move(loopshapingDefinition)); case LoopshapingType::eliminatepattern: - return std::unique_ptr( - new LoopshapingSoftConstraintEliminatePattern(systemSoftConstraint, std::move(loopshapingDefinition))); + return std::make_unique(systemSoftConstraint, std::move(loopshapingDefinition)); default: throw std::runtime_error("[LoopshapingStateInputSoftConstraint::create] invalid loopshaping type"); } diff --git a/ocs2_core/src/misc/LinearAlgebra.cpp b/ocs2_core/src/misc/LinearAlgebra.cpp index 40a2fb6b0..a51a9031d 100644 --- a/ocs2_core/src/misc/LinearAlgebra.cpp +++ b/ocs2_core/src/misc/LinearAlgebra.cpp @@ -32,6 +32,20 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace LinearAlgebra { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void setTriangularMinimumEigenvalues(matrix_t& Lr, scalar_t minEigenValue) { + for (Eigen::Index i = 0; i < Lr.rows(); ++i) { + scalar_t& eigenValue = Lr(i, i); // diagonal element is the eigenvalue + if (eigenValue < 0.0) { + eigenValue = std::min(-minEigenValue, eigenValue); + } else { + eigenValue = std::max(minEigenValue, eigenValue); + } + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -57,6 +71,19 @@ void makePsdEigenvalue(matrix_t& squareMatrix, scalar_t minEigenvalue) { } } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void makePsdGershgorin(matrix_t& squareMatrix, scalar_t minEigenvalue) { + assert(squareMatrix.rows() == squareMatrix.cols()); + squareMatrix = 0.5 * (squareMatrix + squareMatrix.transpose()).eval(); + for (size_t i = 0; i < squareMatrix.rows(); i++) { + // Gershgorin radius: since the matrix is symmetric we use column sum instead of row sum + auto Ri = squareMatrix.col(i).cwiseAbs().sum() - std::abs(squareMatrix(i, i)); + squareMatrix(i, i) = std::max(squareMatrix(i, i), Ri + minEigenvalue); + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -86,6 +113,16 @@ void makePsdCholesky(matrix_t& A, scalar_t minEigenvalue) { A.diagonal().array() += minEigenvalue; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void computeInverseMatrixUUT(const matrix_t& Am, matrix_t& AmInvUmUmT) { + // Am = Lm Lm^T --> inv(Am) = inv(Lm^T) inv(Lm) where Lm^T is upper triangular + Eigen::LLT lltOfA(Am); + AmInvUmUmT.setIdentity(Am.rows(), Am.cols()); // for dynamic size matrices + lltOfA.matrixU().solveInPlace(AmInvUmUmT); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -117,6 +154,50 @@ void computeConstraintProjection(const matrix_t& Dm, const matrix_t& RmInvUmUmT, RmInvConstrainedUUT.noalias() = RmInvUmUmT * QRof_RmInvUmUmTT_DmT_Qu; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::pair qrConstraintProjection(const VectorFunctionLinearApproximation& constraint) { + // Constraint Projectors are based on the QR decomposition + const auto numConstraints = constraint.dfdu.rows(); + const auto numInputs = constraint.dfdu.cols(); + const Eigen::HouseholderQR QRof_DT(constraint.dfdu.transpose()); + + const matrix_t Q = QRof_DT.householderQ(); + const auto Q1 = Q.leftCols(numConstraints); + + const auto R = QRof_DT.matrixQR().topRows(numConstraints).triangularView(); + const matrix_t pseudoInverse = R.solve(Q1.transpose()); // left pseudo-inverse of D^T + + VectorFunctionLinearApproximation projectionTerms; + projectionTerms.dfdu = Q.rightCols(numInputs - numConstraints); + projectionTerms.dfdx.noalias() = -pseudoInverse.transpose() * constraint.dfdx; + projectionTerms.f.noalias() = -pseudoInverse.transpose() * constraint.f; + + return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::pair luConstraintProjection(const VectorFunctionLinearApproximation& constraint, + bool extractPseudoInverse) { + // Constraint Projectors are based on the LU decomposition + const Eigen::FullPivLU lu(constraint.dfdu); + + VectorFunctionLinearApproximation projectionTerms; + projectionTerms.dfdu = lu.kernel(); + projectionTerms.dfdx.noalias() = -lu.solve(constraint.dfdx); + projectionTerms.f.noalias() = -lu.solve(constraint.f); + + matrix_t pseudoInverse; + if (extractPseudoInverse) { + pseudoInverse = lu.solve(matrix_t::Identity(constraint.f.size(), constraint.f.size())).transpose(); // left pseudo-inverse of D^T + } + + return std::make_pair(std::move(projectionTerms), std::move(pseudoInverse)); +} + // Explicit instantiations for dynamic sized matrices template int rank(const matrix_t& A); template Eigen::VectorXcd eigenvalues(const matrix_t& A); diff --git a/ocs2_core/src/model_data/Metrics.cpp b/ocs2_core/src/model_data/Metrics.cpp index 8d1066934..39e440aa7 100644 --- a/ocs2_core/src/model_data/Metrics.cpp +++ b/ocs2_core/src/model_data/Metrics.cpp @@ -29,8 +29,67 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_core/model_data/Metrics.h" +#include + namespace ocs2 { +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void Metrics::swap(Metrics& other) { + // Cost + std::swap(cost, other.cost); + // Dynamics violation + dynamicsViolation.swap(other.dynamicsViolation); + // Equality constraints + stateEqConstraint.swap(other.stateEqConstraint); + stateInputEqConstraint.swap(other.stateInputEqConstraint); + // Inequality constraints + stateIneqConstraint.swap(other.stateIneqConstraint); + stateInputIneqConstraint.swap(other.stateInputIneqConstraint); + // Lagrangians + stateEqLagrangian.swap(other.stateEqLagrangian); + stateIneqLagrangian.swap(other.stateIneqLagrangian); + stateInputEqLagrangian.swap(other.stateInputEqLagrangian); + stateInputIneqLagrangian.swap(other.stateInputIneqLagrangian); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void Metrics::clear() { + // Cost + cost = 0.0; + // Dynamics violation + dynamicsViolation = vector_t(); + // Equality constraints + stateEqConstraint.clear(); + stateInputEqConstraint.clear(); + // Inequality constraints + stateIneqConstraint.clear(); + stateInputIneqConstraint.clear(); + // Lagrangians + stateEqLagrangian.clear(); + stateIneqLagrangian.clear(); + stateInputEqLagrangian.clear(); + stateInputIneqLagrangian.clear(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool Metrics::isApprox(const Metrics& other, scalar_t prec) const { + return numerics::almost_eq(this->cost, other.cost, prec) && this->dynamicsViolation.isApprox(other.dynamicsViolation, prec) && + toVector(this->stateEqConstraint).isApprox(toVector(other.stateEqConstraint), prec) && + toVector(this->stateInputEqConstraint).isApprox(toVector(other.stateInputEqConstraint), prec) && + toVector(this->stateIneqConstraint).isApprox(toVector(other.stateIneqConstraint), prec) && + toVector(this->stateInputIneqConstraint).isApprox(toVector(other.stateInputIneqConstraint), prec) && + toVector(this->stateEqLagrangian).isApprox(toVector(other.stateEqLagrangian), prec) && + toVector(this->stateIneqLagrangian).isApprox(toVector(other.stateIneqLagrangian), prec) && + toVector(this->stateInputEqLagrangian).isApprox(toVector(other.stateInputEqLagrangian), prec) && + toVector(this->stateInputIneqLagrangian).isApprox(toVector(other.stateInputIneqLagrangian), prec); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -50,6 +109,39 @@ vector_t toVector(const std::vector& termsLagrangianMetrics) return vec; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t toVector(const vector_array_t& constraintArray) { + size_t n = 0; + std::for_each(constraintArray.begin(), constraintArray.end(), [&n](const vector_t& v) { n += v.size(); }); + + vector_t vec(n); + size_t head = 0; + for (const auto& v : constraintArray) { + vec.segment(head, v.size()) = v; + head += v.size(); + } // end of i loop + + return vec; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_array_t toConstraintArray(const size_array_t& termsSize, const vector_t& vec) { + vector_array_t constraintArray; + constraintArray.reserve(termsSize.size()); + + size_t head = 0; + for (const auto& l : termsSize) { + constraintArray.emplace_back(vec.segment(head, l)); + head += l; + } // end of i loop + + return constraintArray; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -66,6 +158,16 @@ std::vector toLagrangianMetrics(const size_array_t& termsSize return lagrangianMetrics; } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +size_array_t getSizes(const vector_array_t& constraintArray) { + size_array_t s(constraintArray.size()); + std::transform(constraintArray.begin(), constraintArray.end(), s.begin(), + [](const vector_t& v) { return static_cast(v.size()); }); + return s; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -99,71 +201,103 @@ LagrangianMetrics interpolate(const index_alpha_t& indexAlpha, const std::vector /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MetricsCollection interpolate(const index_alpha_t& indexAlpha, const std::vector& dataArray) { +Metrics interpolate(const index_alpha_t& indexAlpha, const std::vector& dataArray) { // number of terms const auto ind = indexAlpha.second > 0.5 ? indexAlpha.first : indexAlpha.first + 1; - const size_t mumStateEq = dataArray[ind].stateEqLagrangian.size(); - const size_t mumStateIneq = dataArray[ind].stateIneqLagrangian.size(); - const size_t mumStateInputEq = dataArray[ind].stateInputEqLagrangian.size(); - const size_t mumStateInputIneq = dataArray[ind].stateInputIneqLagrangian.size(); + const size_t numStateEqConst = dataArray[ind].stateEqConstraint.size(); + const size_t numStateInputEqCost = dataArray[ind].stateInputEqConstraint.size(); + const size_t numStateIneqConst = dataArray[ind].stateIneqConstraint.size(); + const size_t numStateInputIneqCost = dataArray[ind].stateInputIneqConstraint.size(); + const size_t numStateEqLag = dataArray[ind].stateEqLagrangian.size(); + const size_t numStateIneqLag = dataArray[ind].stateIneqLagrangian.size(); + const size_t numStateInputEqLag = dataArray[ind].stateInputEqLagrangian.size(); + const size_t numStateInputIneqLag = dataArray[ind].stateInputIneqLagrangian.size(); - MetricsCollection out; + Metrics out; // cost - out.cost = interpolate(indexAlpha, dataArray, - [](const std::vector& array, size_t t) -> const scalar_t& { return array[t].cost; }); - - // constraints - out.stateEqConstraint = interpolate(indexAlpha, dataArray, [](const std::vector& array, size_t t) -> const vector_t& { - return array[t].stateEqConstraint; - }); - out.stateInputEqConstraint = - interpolate(indexAlpha, dataArray, - [](const std::vector& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); + out.cost = + interpolate(indexAlpha, dataArray, [](const std::vector& array, size_t t) -> const scalar_t& { return array[t].cost; }); + + // dynamics violation + out.dynamicsViolation = interpolate( + indexAlpha, dataArray, [](const std::vector& array, size_t t) -> const vector_t& { return array[t].dynamicsViolation; }); + + // equality constraints + out.stateEqConstraint.reserve(numStateEqConst); + for (size_t i = 0; i < numStateEqConst; ++i) { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { + return array[t].stateEqConstraint[i]; + }); + out.stateEqConstraint.emplace_back(std::move(constraint)); + } + out.stateInputEqConstraint.reserve(numStateInputEqCost); + for (size_t i = 0; i < numStateInputEqCost; ++i) { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { + return array[t].stateInputEqConstraint[i]; + }); + out.stateInputEqConstraint.emplace_back(std::move(constraint)); + } + + // inequality constraints + out.stateIneqConstraint.reserve(numStateIneqConst); + for (size_t i = 0; i < numStateIneqConst; ++i) { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { + return array[t].stateIneqConstraint[i]; + }); + out.stateIneqConstraint.emplace_back(std::move(constraint)); + } + out.stateInputIneqConstraint.reserve(numStateInputIneqCost); + for (size_t i = 0; i < numStateInputIneqCost; ++i) { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { + return array[t].stateInputIneqConstraint[i]; + }); + out.stateInputIneqConstraint.emplace_back(std::move(constraint)); + } // state equality Lagrangian - out.stateEqLagrangian.reserve(mumStateEq); - for (size_t i = 0; i < mumStateEq; i++) { - auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const scalar_t& { + out.stateEqLagrangian.reserve(numStateEqLag); + for (size_t i = 0; i < numStateEqLag; ++i) { + auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const scalar_t& { return array[t].stateEqLagrangian[i].penalty; }); - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { return array[t].stateEqLagrangian[i].constraint; }); out.stateEqLagrangian.emplace_back(penalty, std::move(constraint)); } // end of i loop // state inequality Lagrangian - out.stateIneqLagrangian.reserve(mumStateIneq); - for (size_t i = 0; i < mumStateIneq; i++) { - auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const scalar_t& { + out.stateIneqLagrangian.reserve(numStateIneqLag); + for (size_t i = 0; i < numStateIneqLag; ++i) { + auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const scalar_t& { return array[t].stateIneqLagrangian[i].penalty; }); - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { return array[t].stateIneqLagrangian[i].constraint; }); out.stateIneqLagrangian.emplace_back(penalty, std::move(constraint)); } // end of i loop // state-input equality Lagrangian - out.stateInputEqLagrangian.reserve(mumStateInputEq); - for (size_t i = 0; i < mumStateInputEq; i++) { - auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const scalar_t& { + out.stateInputEqLagrangian.reserve(numStateInputEqLag); + for (size_t i = 0; i < numStateInputEqLag; ++i) { + auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const scalar_t& { return array[t].stateInputEqLagrangian[i].penalty; }); - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { return array[t].stateInputEqLagrangian[i].constraint; }); out.stateInputEqLagrangian.emplace_back(penalty, std::move(constraint)); } // end of i loop // state-input inequality Lagrangian - out.stateInputIneqLagrangian.reserve(mumStateInputIneq); - for (size_t i = 0; i < mumStateInputIneq; i++) { - auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const scalar_t& { + out.stateInputIneqLagrangian.reserve(numStateInputIneqLag); + for (size_t i = 0; i < numStateInputIneqLag; ++i) { + auto penalty = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const scalar_t& { return array[t].stateInputIneqLagrangian[i].penalty; }); - auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { + auto constraint = interpolate(indexAlpha, dataArray, [i](const std::vector& array, size_t t) -> const vector_t& { return array[t].stateInputIneqLagrangian[i].constraint; }); out.stateInputIneqLagrangian.emplace_back(penalty, std::move(constraint)); diff --git a/ocs2_core/src/penalties/MultidimensionalPenalty.cpp b/ocs2_core/src/penalties/MultidimensionalPenalty.cpp index b15175abc..1eaf51b3c 100644 --- a/ocs2_core/src/penalties/MultidimensionalPenalty.cpp +++ b/ocs2_core/src/penalties/MultidimensionalPenalty.cpp @@ -63,7 +63,7 @@ class PenaltyBaseWrapper final : public augmented::AugmentedPenaltyBase { }; std::unique_ptr createWrapper(std::unique_ptr penaltyPtr) { - return std::unique_ptr(new PenaltyBaseWrapper(std::move(penaltyPtr))); + return std::make_unique(std::move(penaltyPtr)); } scalar_t getMultiplier(const vector_t* l, size_t ind) { diff --git a/ocs2_core/test/constraint/testConstraintCollection.cpp b/ocs2_core/test/constraint/testConstraintCollection.cpp index 418275b98..aef7c3fe5 100644 --- a/ocs2_core/test/constraint/testConstraintCollection.cpp +++ b/ocs2_core/test/constraint/testConstraintCollection.cpp @@ -27,6 +27,8 @@ 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 + #include #include @@ -37,7 +39,7 @@ TEST(TestConstraintCollection, add) { ocs2::StateInputConstraintCollection constraintCollection; // Add after construction - std::unique_ptr constraintTerm(new TestEmptyConstraint()); + auto constraintTerm = std::make_unique(); ASSERT_NO_THROW({ constraintCollection.add("Constraint1", std::move(constraintTerm)); }); } @@ -48,7 +50,7 @@ TEST(TestConstraintCollection, numberOfConstraints) { EXPECT_EQ(constraintCollection.getNumConstraints(0.0), 0); // Add Linear inequality constraint term, which has 2 constraints - std::unique_ptr constraintTerm(new TestDummyConstraint()); + auto constraintTerm = std::make_unique(); const size_t addedConstraints = constraintTerm->getNumConstraints(0.0); constraintCollection.add("Constraint1", std::move(constraintTerm)); @@ -56,6 +58,37 @@ TEST(TestConstraintCollection, numberOfConstraints) { EXPECT_EQ(constraintCollection.getNumConstraints(0.0), addedConstraints); } +TEST(TestConstraintCollection, termsSize) { + ocs2::StateInputConstraintCollection constraintCollection; + + // Initially we have zero constraints for all types + auto termsSize = constraintCollection.getTermsSize(0.0); + EXPECT_EQ(termsSize.size(), 0); + EXPECT_EQ(std::accumulate(termsSize.begin(), termsSize.end(), 0), 0); + + // Add 2 Linear inequality constraint term, which has 2 constraints + constraintCollection.add("Constraint1", std::make_unique()); + constraintCollection.add("Constraint2", std::make_unique()); + constraintCollection.add("Constraint3", std::make_unique()); + auto& constraint1 = constraintCollection.get("Constraint1"); + const size_t constraint1Size = constraint1.getNumConstraints(0.0); + + // Check the right constraint size + termsSize = constraintCollection.getTermsSize(0.0); + EXPECT_EQ(termsSize.size(), 3); + if (termsSize.size() == 3) { + EXPECT_EQ(termsSize[0], constraint1Size); + EXPECT_EQ(termsSize[1], constraint1Size); + EXPECT_EQ(termsSize[2], constraint1Size); + } + + // Deactivate constraint1 + constraint1.setActivity(false); + termsSize = constraintCollection.getTermsSize(0.0); + EXPECT_EQ(termsSize.size(), 3); + EXPECT_EQ(std::accumulate(termsSize.begin(), termsSize.end(), 0), 2 * constraint1Size); +} + TEST(TestConstraintCollection, activatingConstraints) { ocs2::StateInputConstraintCollection constraintCollection; @@ -63,7 +96,7 @@ TEST(TestConstraintCollection, activatingConstraints) { EXPECT_EQ(constraintCollection.getNumConstraints(0.0), 0); // Add Linear inequality constraint term, which has 2 constraints - std::unique_ptr constraintTerm(new TestDummyConstraint()); + auto constraintTerm = std::make_unique(); const size_t addedConstraints = constraintTerm->getNumConstraints(0.0); constraintCollection.add("Constraint1", std::move(constraintTerm)); @@ -78,7 +111,7 @@ TEST(TestConstraintCollection, activatingConstraints) { TEST(TestConstraintCollection, clone) { ocs2::StateInputConstraintCollection constraintCollection; - std::unique_ptr constraintTerm(new TestDummyConstraint()); + auto constraintTerm = std::make_unique(); const size_t addedConstraints = constraintTerm->getNumConstraints(0.0); constraintCollection.add("Constraint1", std::move(constraintTerm)); @@ -103,17 +136,16 @@ TEST(TestConstraintCollection, getValue) { EXPECT_EQ(constraintValues.size(), 0); // Add Linear inequality constraint term, which has 2 constraints, twice - std::unique_ptr constraintTerm1(new TestDummyConstraint()); - std::unique_ptr constraintTerm2(new TestDummyConstraint()); + auto constraintTerm1 = std::make_unique(); + auto constraintTerm2 = std::make_unique(); constraintCollection.add("Constraint1", std::move(constraintTerm1)); constraintCollection.add("Constraint2", std::move(constraintTerm2)); - + const ocs2::vector_t expectedValue = (ocs2::vector_t(2) << 1.0, 2.0).finished(); constraintValues = constraintCollection.getValue(t, x, u, ocs2::PreComputation()); - ASSERT_EQ(constraintValues.rows(), 4); - EXPECT_EQ(constraintValues[0], 1.0); - EXPECT_EQ(constraintValues[1], 2.0); - EXPECT_EQ(constraintValues[2], 1.0); - EXPECT_EQ(constraintValues[3], 2.0); + + ASSERT_EQ(constraintValues.size(), 2); + EXPECT_TRUE(constraintValues[0].isApprox(expectedValue)); + EXPECT_TRUE(constraintValues[1].isApprox(expectedValue)); } TEST(TestConstraintCollection, getLinearApproximation) { @@ -128,8 +160,8 @@ TEST(TestConstraintCollection, getLinearApproximation) { x.setZero(); // Add Linear inequality constraint term, which has 2 constraints, twice - std::unique_ptr constraintTerm1(new TestDummyConstraint()); - std::unique_ptr constraintTerm2(new TestDummyConstraint()); + auto constraintTerm1 = std::make_unique(); + auto constraintTerm2 = std::make_unique(); constraintCollection.add("Constraint1", std::move(constraintTerm1)); constraintCollection.add("Constraint2", std::move(constraintTerm2)); @@ -161,8 +193,8 @@ TEST(TestConstraintCollection, getQuadraticApproximation) { x.setZero(); // Add Linear inequality constraint term, which has 2 constraints, twice - std::unique_ptr constraintTerm1(new TestDummyConstraint()); - std::unique_ptr constraintTerm2(new TestDummyConstraint()); + auto constraintTerm1 = std::make_unique(); + auto constraintTerm2 = std::make_unique(); constraintCollection.add("Constraint1", std::move(constraintTerm1)); constraintCollection.add("Constraint2", std::move(constraintTerm2)); diff --git a/ocs2_core/test/cost/testCostCollection.cpp b/ocs2_core/test/cost/testCostCollection.cpp index 67661d598..76cf3702e 100644 --- a/ocs2_core/test/cost/testCostCollection.cpp +++ b/ocs2_core/test/cost/testCostCollection.cpp @@ -82,8 +82,8 @@ class StateInputCost_TestFixture : public ::testing::Test { u = ocs2::vector_t::Random(INPUT_DIM); t = 0.0; - auto cost1 = std::unique_ptr(new SimpleQuadraticCost(Q, R)); - auto cost2 = std::unique_ptr(new SimpleQuadraticCost(0.5 * Q, 2.0 * R)); + auto cost1 = std::make_unique(Q, R); + auto cost2 = std::make_unique(0.5 * Q, 2.0 * R); expectedCost = cost1->getValue(t, x, u, targetTrajectories, {}) + cost2->getValue(t, x, u, targetTrajectories, {}); expectedCostApproximation = cost1->getQuadraticApproximation(t, x, u, targetTrajectories, {}); expectedCostApproximation += cost2->getQuadraticApproximation(t, x, u, targetTrajectories, {}); @@ -193,7 +193,7 @@ class StateCost_TestFixture : public ::testing::Test { x = ocs2::vector_t::Random(STATE_DIM); t = 0.0; - auto cost = std::unique_ptr(new SimpleQuadraticFinalCost(std::move(Q))); + auto cost = std::make_unique(std::move(Q)); expectedCost = cost->getValue(t, x, targetTrajectories, {}); expectedCostApproximation = cost->getQuadraticApproximation(t, x, targetTrajectories, {}); diff --git a/ocs2_core/test/integration/IntegrationTest.cpp b/ocs2_core/test/integration/IntegrationTest.cpp index 354c29fb4..9a111e927 100644 --- a/ocs2_core/test/integration/IntegrationTest.cpp +++ b/ocs2_core/test/integration/IntegrationTest.cpp @@ -37,19 +37,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. using namespace ocs2; -std::unique_ptr getSystem() { +std::unique_ptr getSystem(LinearController& controller) { matrix_t A(2, 2); A << -2, -1, // clang-format off 1, 0; // clang-format on matrix_t B(2, 1); B << 1, 0; - auto sys = std::unique_ptr(new LinearSystemDynamics(A, B)); - scalar_array_t cntTimeStamp{0, 10}; - vector_array_t uff(2, vector_t::Ones(1)); - matrix_array_t k(2, matrix_t::Zero(1, 2)); - static std::shared_ptr controller = std::make_shared(cntTimeStamp, uff, k); - sys->setController(controller.get()); + auto sys = std::make_unique(std::move(A), std::move(B)); + sys->setController(&controller); return std::move(sys); } @@ -60,7 +56,11 @@ void testSecondOrderSystem(IntegratorType integrator_type) { const scalar_t dt = 0.05; const vector_t x0 = vector_t::Zero(2); - auto sys = getSystem(); + const scalar_array_t cntTimeStamp{0, 10}; + const vector_array_t uff(2, vector_t::Ones(1)); + const matrix_array_t k(2, matrix_t::Zero(1, 2)); + LinearController controller(cntTimeStamp, uff, k); + auto sys = getSystem(controller); std::unique_ptr integrator = newIntegrator(integrator_type); diff --git a/ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp b/ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp index dcd69bd98..37f86baba 100644 --- a/ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp +++ b/ocs2_core/test/integration/TrapezoidalIntegrationTest.cpp @@ -46,7 +46,7 @@ TEST(TrapezoidalIntegrationTest, Rectangle) { ocs2::scalar_array_t yTrj(numIntervals + 1, height); - const auto area = ocs2::trapezoidalIntegration(xTrj, yTrj); + const auto area = ocs2::trapezoidalIntegration(xTrj, yTrj, 0.0); ASSERT_NEAR(area, width * height, 1e-12); } @@ -65,6 +65,6 @@ TEST(TrapezoidalIntegrationTest, RightTriangle) { auto y = -dy; std::generate(yTrj.begin(), yTrj.end(), [&y, dy]() { return (y += dy); }); - const auto area = ocs2::trapezoidalIntegration(xTrj, yTrj); + const auto area = ocs2::trapezoidalIntegration(xTrj, yTrj, 0.0); ASSERT_NEAR(area, width * height / 2.0, 1e-12); } diff --git a/ocs2_core/test/integration/testSensitivityIntegrator.cpp b/ocs2_core/test/integration/testSensitivityIntegrator.cpp index a1280bf08..974b663d0 100644 --- a/ocs2_core/test/integration/testSensitivityIntegrator.cpp +++ b/ocs2_core/test/integration/testSensitivityIntegrator.cpp @@ -40,10 +40,10 @@ namespace { std::unique_ptr getSystem() { ocs2::matrix_t A(2, 2); A << -2, -1, // clang-format off - 1, 0; // clang-format on + 1, 0; // clang-format on ocs2::matrix_t B(2, 1); B << 1, 0; - return std::unique_ptr(new ocs2::LinearSystemDynamics(A, B)); + return std::make_unique(std::move(A), std::move(B)); } } // namespace @@ -209,4 +209,4 @@ TEST(test_sensitivity_integrator, vsBoostRK4) { // Check ASSERT_TRUE(rk4ForwardDynamics.isApprox(boostRk4ForwardDynamics)); -} \ No newline at end of file +} diff --git a/ocs2_core/test/loopshaping/testLoopshapingConstraint.h b/ocs2_core/test/loopshaping/testLoopshapingConstraint.h index babfd868c..6750ab652 100644 --- a/ocs2_core/test/loopshaping/testLoopshapingConstraint.h +++ b/ocs2_core/test/loopshaping/testLoopshapingConstraint.h @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_core/loopshaping/LoopshapingPreComputation.h" #include "ocs2_core/loopshaping/LoopshapingPropertyTree.h" #include "ocs2_core/loopshaping/constraint/LoopshapingConstraint.h" +#include "ocs2_core/model_data/Metrics.h" #include "testLoopshapingConfigurations.h" #include "testQuadraticConstraint.h" @@ -62,14 +63,15 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { void testStateInputConstraintEvaluation() const { // Evaluate system - vector_t g_system = systemConstraint->getValue(t, x_sys_, u_sys_, PreComputation()); + const auto g_system = systemConstraint->getValue(t, x_sys_, u_sys_, PreComputation()); // Evaluate loopshaping system preComp_->request(Request::Constraint, t, x_, u_); - vector_t g = loopshapingConstraint->getValue(t, x_, u_, *preComp_); + const auto g = toVector(loopshapingConstraint->getValue(t, x_, u_, *preComp_)); // The constraint should stay the same - EXPECT_LE((g_system - g).array().abs().maxCoeff(), tol); + EXPECT_TRUE(g_system.size() == g.size()); + EXPECT_TRUE(g_system.isApprox(g, tol)); } void testStateInputConstraintLinearApproximation() const { @@ -90,7 +92,7 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { // Reevaluate at disturbed state preComp_->request(Request::Constraint, t, x_ + x_disturbance_, u_ + u_disturbance_); - vector_t h_disturbance = loopshapingConstraint->getValue(t, x_ + x_disturbance_, u_ + u_disturbance_, *preComp_); + const auto h_disturbance = toVector(loopshapingConstraint->getValue(t, x_ + x_disturbance_, u_ + u_disturbance_, *preComp_)); // Evaluate approximation for (size_t i = 0; i < h.f.rows(); i++) { @@ -98,20 +100,20 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { 0.5 * x_disturbance_.transpose() * h.dfdxx[i] * x_disturbance_ + 0.5 * u_disturbance_.transpose() * h.dfduu[i] * u_disturbance_ + u_disturbance_.transpose() * h.dfdux[i] * x_disturbance_; - EXPECT_LE(std::abs(h_disturbance[i] - h_approximation), tol); + EXPECT_NEAR(h_disturbance[i], h_approximation, tol); } } void testStateOnlyConstraintEvaluation() const { // Evaluate system - vector_t g_system = systemStateConstraint->getValue(t, x_sys_, PreComputation()); + const auto g_system = systemStateConstraint->getValue(t, x_sys_, PreComputation()); // Evaluate loopshaping system preComp_->requestFinal(Request::Constraint, t, x_); - vector_t g = loopshapingStateConstraint->getValue(t, x_, *preComp_); + const auto g = toVector(loopshapingStateConstraint->getValue(t, x_, *preComp_)); // System part of the constraints should stay the same - EXPECT_LE((g_system - g).array().abs().maxCoeff(), tol); + EXPECT_TRUE(g_system.isApprox(g, tol)); } void testStateOnlyConstraintLinearApproximation() const { @@ -131,12 +133,12 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { // Reevaluate at disturbed state preComp_->requestFinal(Request::Constraint, t, x_ + x_disturbance_); - vector_t h_disturbance = loopshapingStateConstraint->getValue(t, x_ + x_disturbance_, *preComp_); + const auto h_disturbance = toVector(loopshapingStateConstraint->getValue(t, x_ + x_disturbance_, *preComp_)); // Evaluate approximation for (size_t i = 0; i < h.f.rows(); i++) { - scalar_t h_approximation = h.f(i) + h.dfdx.row(i) * x_disturbance_ + 0.5 * x_disturbance_.transpose() * h.dfdxx[i] * x_disturbance_; - EXPECT_LE(std::abs(h_disturbance[i] - h_approximation), tol); + const auto h_approximation = h.f(i) + h.dfdx.row(i) * x_disturbance_ + 0.5 * x_disturbance_.transpose() * h.dfdxx[i] * x_disturbance_; + EXPECT_NEAR(h_disturbance[i], h_approximation, tol); } } @@ -147,4 +149,4 @@ class TestFixtureLoopShapingConstraint : LoopshapingTestConfiguration { std::unique_ptr loopshapingStateConstraint; }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/test/loopshaping/testLoopshapingFilterDynamics.cpp b/ocs2_core/test/loopshaping/testLoopshapingFilterDynamics.cpp index 0c2b58756..8ba8c2210 100644 --- a/ocs2_core/test/loopshaping/testLoopshapingFilterDynamics.cpp +++ b/ocs2_core/test/loopshaping/testLoopshapingFilterDynamics.cpp @@ -43,7 +43,7 @@ TEST(testLoopshapingFilterDynamics, Integration) { C << 0.0, 0.0, 0.0; D << 1.0; Filter filter(A, B, C, D); - auto loopshapingDefinition = std::shared_ptr(new LoopshapingDefinition(LoopshapingType::outputpattern, filter)); + auto loopshapingDefinition = std::make_shared(LoopshapingType::outputpattern, filter); using TestLoopshapingFilterDynamics = LoopshapingFilterDynamics; TestLoopshapingFilterDynamics loopshapingFilterDynamics(loopshapingDefinition); diff --git a/ocs2_core/test/loopshaping/testQuadraticConstraint.h b/ocs2_core/test/loopshaping/testQuadraticConstraint.h index 94f81249d..b5c7237b7 100644 --- a/ocs2_core/test/loopshaping/testQuadraticConstraint.h +++ b/ocs2_core/test/loopshaping/testQuadraticConstraint.h @@ -44,7 +44,7 @@ class TestQuadraticStateInputConstraint : public StateInputConstraint { P.setRandom(inputDim, stateDim); Q = (0.5 * Q.transpose() + 0.5 * Q).eval(); R = (0.5 * R.transpose() + 0.5 * R).eval(); - return std::unique_ptr(new TestQuadraticStateInputConstraint(Q, R, P)); + return std::make_unique(std::move(Q), std::move(R), std::move(P)); } ~TestQuadraticStateInputConstraint() override = default; @@ -94,7 +94,7 @@ class TestQuadraticStateConstraint : public StateConstraint { matrix_t Q; Q.setRandom(stateDim, stateDim); Q = (0.5 * Q.transpose() + 0.5 * Q).eval(); - return std::unique_ptr(new TestQuadraticStateConstraint(Q)); + return std::make_unique(std::move(Q)); } ~TestQuadraticStateConstraint() override = default; @@ -128,4 +128,4 @@ class TestQuadraticStateConstraint : public StateConstraint { matrix_t Q_; }; -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_core/test/misc/testLinearAlgebra.cpp b/ocs2_core/test/misc/testLinearAlgebra.cpp index 4aee1de9a..83057856a 100644 --- a/ocs2_core/test/misc/testLinearAlgebra.cpp +++ b/ocs2_core/test/misc/testLinearAlgebra.cpp @@ -1,86 +1,187 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 #include #include #include -using namespace ocs2; -using namespace LinearAlgebra; +TEST(test_projection, testProjectionQR) { + constexpr ocs2::scalar_t nx = 30; + constexpr ocs2::scalar_t nu = 20; + constexpr ocs2::scalar_t nc = 10; + const auto constraint = [&]() { + ocs2::VectorFunctionLinearApproximation approx; + approx.dfdx = ocs2::matrix_t::Random(nc, nx); + approx.dfdu = ocs2::matrix_t::Random(nc, nu); + approx.f = ocs2::vector_t::Random(nc); + return approx; + }(); + + auto result = ocs2::LinearAlgebra::qrConstraintProjection(constraint); + const auto projection = std::move(result.first); + const auto pseudoInverse = std::move(result.second); + + // range of Pu is in null-space of D + ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); + + // D * Px cancels the C term + ASSERT_TRUE((constraint.dfdx + constraint.dfdu * projection.dfdx).isZero()); + + // D * Pe cancels the e term + ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); + + ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); + ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); + + ASSERT_TRUE((pseudoInverse * constraint.dfdu.transpose()).isIdentity()); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.dfdx).isApprox(-projection.dfdx)); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.f).isApprox(-projection.f)); +} + +TEST(test_projection, testProjectionLU) { + constexpr ocs2::scalar_t nx = 30; + constexpr ocs2::scalar_t nu = 20; + constexpr ocs2::scalar_t nc = 10; + const auto constraint = [&]() { + ocs2::VectorFunctionLinearApproximation approx; + approx.dfdx = ocs2::matrix_t::Random(nc, nx); + approx.dfdu = ocs2::matrix_t::Random(nc, nu); + approx.f = ocs2::vector_t::Random(nc); + return approx; + }(); + + auto result = ocs2::LinearAlgebra::luConstraintProjection(constraint); + ASSERT_EQ(result.second.rows(), 0); + ASSERT_EQ(result.second.cols(), 0); + + const auto projection = std::move(result.first); + + // range of Pu is in null-space of D + ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); + + // D * Px cancels the C term + ASSERT_TRUE((constraint.dfdx + constraint.dfdu * projection.dfdx).isZero()); + + // D * Pe cancels the e term + ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); + + auto resultWithPseudoInverse = ocs2::LinearAlgebra::luConstraintProjection(constraint, true); + const auto projectionWithPseudoInverse = std::move(resultWithPseudoInverse.first); + ASSERT_TRUE(projection.f.isApprox(projectionWithPseudoInverse.f)); + ASSERT_TRUE(projection.dfdx.isApprox(projectionWithPseudoInverse.dfdx)); + ASSERT_TRUE(projection.dfdu.isApprox(projectionWithPseudoInverse.dfdu)); + + const auto pseudoInverse = std::move(resultWithPseudoInverse.second); + ASSERT_EQ(pseudoInverse.rows(), constraint.dfdu.rows()); + ASSERT_EQ(pseudoInverse.cols(), constraint.dfdu.cols()); + + ASSERT_TRUE((pseudoInverse * constraint.dfdu.transpose()).isIdentity()); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.dfdx).isApprox(-projection.dfdx)); + ASSERT_TRUE((pseudoInverse.transpose() * constraint.f).isApprox(-projection.f)); +} TEST(LLTofInverse, checkAgainstFullInverse) { - const size_t n = 10; // matrix size - const scalar_t tol = 1e-9; // Coefficient-wise tolerance + constexpr size_t n = 10; // matrix size + constexpr ocs2::scalar_t tol = 1e-9; // Coefficient-wise tolerance // Some random symmetric positive definite matrix - matrix_t A = generateSPDmatrix(n); - matrix_t AmInvUmUmT; + ocs2::matrix_t A = ocs2::LinearAlgebra::generateSPDmatrix(n); + ocs2::matrix_t AmInvUmUmT; - computeInverseMatrixUUT(A, AmInvUmUmT); + ocs2::LinearAlgebra::computeInverseMatrixUUT(A, AmInvUmUmT); - matrix_t Ainv = A.inverse(); - matrix_t Ainv_constructed = AmInvUmUmT * AmInvUmUmT.transpose(); + ocs2::matrix_t Ainv = A.inverse(); + ocs2::matrix_t Ainv_constructed = AmInvUmUmT * AmInvUmUmT.transpose(); ASSERT_LT((Ainv - Ainv_constructed).array().abs().maxCoeff(), tol); } TEST(constraintProjection, checkAgainstFullComputations) { - const size_t m = 4; // num constraints - const size_t n = 15; // num inputs - const scalar_t tol = 1e-9; // Coefficient-wise tolerance + constexpr size_t m = 4; // num constraints + constexpr size_t n = 15; // num inputs + constexpr ocs2::scalar_t tol = 1e-9; // Coefficient-wise tolerance // Some random constraint matrix - matrix_t D = generateFullRowRankmatrix(m, n); + ocs2::matrix_t D = ocs2::LinearAlgebra::generateFullRowRankmatrix(m, n); // Random cost matrix - matrix_t R = generateSPDmatrix(n); + ocs2::matrix_t R = ocs2::LinearAlgebra::generateSPDmatrix(n); // Inverse of R - matrix_t RmInvUmUmT; - computeInverseMatrixUUT(R, RmInvUmUmT); - matrix_t Rinv = RmInvUmUmT * RmInvUmUmT.transpose(); + ocs2::matrix_t RmInvUmUmT; + ocs2::LinearAlgebra::computeInverseMatrixUUT(R, RmInvUmUmT); + ocs2::matrix_t Rinv = RmInvUmUmT * RmInvUmUmT.transpose(); // Compute constraint projection terms, this is what we are testing in this unit test - matrix_t Ddagger, DdaggerT_R_Ddagger_Chol, RinvConstrainedChol; - computeConstraintProjection(D, RmInvUmUmT, Ddagger, DdaggerT_R_Ddagger_Chol, RinvConstrainedChol); + ocs2::matrix_t Ddagger, DdaggerT_R_Ddagger_Chol, RinvConstrainedChol; + ocs2::LinearAlgebra::computeConstraintProjection(D, RmInvUmUmT, Ddagger, DdaggerT_R_Ddagger_Chol, RinvConstrainedChol); // Reconstruct full matrices to compare - matrix_t RinvConstrained = RinvConstrainedChol * RinvConstrainedChol.transpose(); - matrix_t DdaggerT_R_Ddagger = DdaggerT_R_Ddagger_Chol * DdaggerT_R_Ddagger_Chol.transpose(); + ocs2::matrix_t RinvConstrained = RinvConstrainedChol * RinvConstrainedChol.transpose(); + ocs2::matrix_t DdaggerT_R_Ddagger = DdaggerT_R_Ddagger_Chol * DdaggerT_R_Ddagger_Chol.transpose(); // Alternative computation following Farshidian - An Efficient Optimal Planning and Control Framework For Quadrupedal Locomotion // Check Ddagger - matrix_t RmProjected = (D * Rinv * D.transpose()).ldlt().solve(matrix_t::Identity(m, m)); - matrix_t Ddagger_check = Rinv * D.transpose() * RmProjected; + ocs2::matrix_t RmProjected = (D * Rinv * D.transpose()).ldlt().solve(ocs2::matrix_t::Identity(m, m)); + ocs2::matrix_t Ddagger_check = Rinv * D.transpose() * RmProjected; ASSERT_LT((Ddagger - Ddagger_check).array().abs().maxCoeff(), tol); // Check Ddagger^T * R * Ddagger - matrix_t DdaggerT_R_Ddagger_check = Ddagger_check.transpose() * R * Ddagger_check; + ocs2::matrix_t DdaggerT_R_Ddagger_check = Ddagger_check.transpose() * R * Ddagger_check; ASSERT_LT((DdaggerT_R_Ddagger - DdaggerT_R_Ddagger_check).array().abs().maxCoeff(), tol); // Check Constrained cost matrix defined as defined in the paper - matrix_t nullspaceProjection = matrix_t::Identity(n, n) - Ddagger_check * D; - matrix_t RinvConstrained_check = Rinv.transpose() * nullspaceProjection.transpose() * R * nullspaceProjection * Rinv; + ocs2::matrix_t nullspaceProjection = ocs2::matrix_t::Identity(n, n) - Ddagger_check * D; + ocs2::matrix_t RinvConstrained_check = Rinv.transpose() * nullspaceProjection.transpose() * R * nullspaceProjection * Rinv; ASSERT_LT((RinvConstrained - RinvConstrained_check).array().abs().maxCoeff(), tol); } TEST(makePsdGershgorin, makePsdGershgorin) { - const size_t n = 10; // matrix size - const scalar_t tol = 1e-9; // Coefficient-wise tolerance + constexpr size_t n = 10; // matrix size + constexpr ocs2::scalar_t tol = 1e-9; // Coefficient-wise tolerance // a random, symmetric, and diagonally dominant matrix - matrix_t ddMat = generateSPDmatrix(n); - matrix_t ddMatCorr = ddMat; - makePsdGershgorin(ddMatCorr); + ocs2::matrix_t ddMat = ocs2::LinearAlgebra::generateSPDmatrix(n); + ocs2::matrix_t ddMatCorr = ddMat; + ocs2::LinearAlgebra::makePsdGershgorin(ddMatCorr); ASSERT_TRUE(ddMat.isApprox(ddMatCorr, tol)); // non-definite matrix auto lambdaMin = ocs2::LinearAlgebra::symmetricEigenvalues(ddMat).minCoeff(); - matrix_t ndMat = ddMat - (lambdaMin + 1e-2) * matrix_t::Identity(n, n); - matrix_t ndMatCorr = ndMat; - const scalar_t minDesiredEigenvalue = 1e-3; - makePsdGershgorin(ndMatCorr, minDesiredEigenvalue); - vector_t lambda = ocs2::LinearAlgebra::symmetricEigenvalues(ndMat); - vector_t lambdaCorr = ocs2::LinearAlgebra::symmetricEigenvalues(ndMatCorr); + ocs2::matrix_t ndMat = ddMat - (lambdaMin + 1e-2) * ocs2::matrix_t::Identity(n, n); + ocs2::matrix_t ndMatCorr = ndMat; + const ocs2::scalar_t minDesiredEigenvalue = 1e-3; + ocs2::LinearAlgebra::makePsdGershgorin(ndMatCorr, minDesiredEigenvalue); + ocs2::vector_t lambda = ocs2::LinearAlgebra::symmetricEigenvalues(ndMat); + ocs2::vector_t lambdaCorr = ocs2::LinearAlgebra::symmetricEigenvalues(ndMatCorr); std::cerr << "MakePSD Gershgorin:" << std::endl; std::cerr << "eigenvalues " << lambda.transpose() << std::endl; std::cerr << "eigenvalues corrected: " << lambdaCorr.transpose() << std::endl; @@ -88,43 +189,43 @@ TEST(makePsdGershgorin, makePsdGershgorin) { } TEST(makePsdCholesky, makePsdCholesky) { - const size_t n = 10; // matrix size - const scalar_t tol = 1e-9; // Coefficient-wise tolerance + constexpr size_t n = 10; // matrix size + constexpr ocs2::scalar_t tol = 1e-9; // Coefficient-wise tolerance // PSD matrix check // some random symmetric matrix - matrix_t psdMat = generateSPDmatrix(n); - matrix_t psdMatCorr = psdMat; - makePsdCholesky(psdMatCorr, 0.0); + ocs2::matrix_t psdMat = ocs2::LinearAlgebra::generateSPDmatrix(n); + ocs2::matrix_t psdMatCorr = psdMat; + ocs2::LinearAlgebra::makePsdCholesky(psdMatCorr, 0.0); ASSERT_TRUE(psdMat.isApprox(psdMatCorr, tol)); // non-definite matrix auto lambdaMin = ocs2::LinearAlgebra::symmetricEigenvalues(psdMat).minCoeff(); - matrix_t ndMat = psdMat - (lambdaMin + 1e-2) * matrix_t::Identity(n, n); - matrix_t ndMatCorr = ndMat; - const scalar_t minDesiredEigenvalue = 1e-1; - makePsdCholesky(ndMatCorr, minDesiredEigenvalue); - vector_t lambda = ocs2::LinearAlgebra::symmetricEigenvalues(ndMat); - vector_t lambdaCorr = ocs2::LinearAlgebra::symmetricEigenvalues(ndMatCorr); + ocs2::matrix_t ndMat = psdMat - (lambdaMin + 1e-2) * ocs2::matrix_t::Identity(n, n); + ocs2::matrix_t ndMatCorr = ndMat; + constexpr ocs2::scalar_t minDesiredEigenvalue = 1e-1; + ocs2::LinearAlgebra::makePsdCholesky(ndMatCorr, minDesiredEigenvalue); + ocs2::vector_t lambda = ocs2::LinearAlgebra::symmetricEigenvalues(ndMat); + ocs2::vector_t lambdaCorr = ocs2::LinearAlgebra::symmetricEigenvalues(ndMatCorr); std::cerr << "MakePSD Cholesky: " << std::endl; std::cerr << "eigenvalues " << lambda.transpose() << std::endl; std::cerr << "eigenvalues corrected: " << lambdaCorr.transpose() << std::endl; ASSERT_GE(lambdaCorr.minCoeff(), minDesiredEigenvalue); // zero matrix - matrix_t zeroMat = matrix_t::Zero(n, n); - makePsdCholesky(zeroMat, minDesiredEigenvalue); - vector_t lambdaZeroMat = ocs2::LinearAlgebra::symmetricEigenvalues(zeroMat); + ocs2::matrix_t zeroMat = ocs2::matrix_t::Zero(n, n); + ocs2::LinearAlgebra::makePsdCholesky(zeroMat, minDesiredEigenvalue); + ocs2::vector_t lambdaZeroMat = ocs2::LinearAlgebra::symmetricEigenvalues(zeroMat); ASSERT_GE(lambdaZeroMat.minCoeff(), minDesiredEigenvalue); // sparse matrix Eigen::VectorXd vec = Eigen::VectorXd::Random(n); - vec = vec.unaryExpr([](scalar_t x) { return std::max(x, 0.0); }); + vec = vec.unaryExpr([](ocs2::scalar_t x) { return std::max(x, 0.0); }); std::cerr << "Sparse vec:\n" << vec << std::endl; - matrix_t sparseMat = vec * vec.transpose() - minDesiredEigenvalue * matrix_t::Identity(n, n); - vector_t lambdaSparseMat = ocs2::LinearAlgebra::symmetricEigenvalues(sparseMat); - makePsdCholesky(sparseMat, minDesiredEigenvalue); - vector_t lambdaSparseMatCorr = ocs2::LinearAlgebra::symmetricEigenvalues(sparseMat); + ocs2::matrix_t sparseMat = vec * vec.transpose() - minDesiredEigenvalue * ocs2::matrix_t::Identity(n, n); + ocs2::vector_t lambdaSparseMat = ocs2::LinearAlgebra::symmetricEigenvalues(sparseMat); + ocs2::LinearAlgebra::makePsdCholesky(sparseMat, minDesiredEigenvalue); + ocs2::vector_t lambdaSparseMatCorr = ocs2::LinearAlgebra::symmetricEigenvalues(sparseMat); std::cerr << "MakePSD Cholesky Sparse Matrix: " << std::endl; std::cerr << "Sparse Matrix:\n" << sparseMat << std::endl; diff --git a/ocs2_core/test/model_data/testMetrics.cpp b/ocs2_core/test/model_data/testMetrics.cpp index 40866541c..052f31f9e 100644 --- a/ocs2_core/test/model_data/testMetrics.cpp +++ b/ocs2_core/test/model_data/testMetrics.cpp @@ -39,14 +39,14 @@ namespace ocs2 { namespace { /** - * Linearly interpolates a trajectory of MetricsCollections. + * Linearly interpolates a trajectory of Metricss. * * @param [in] indexAlpha : index and interpolation coefficient (alpha) pair. - * @param [in] dataArray : A trajectory of MetricsCollections. - * @return The interpolated MetricsCollection at indexAlpha. + * @param [in] dataArray : A trajectory of Metricss. + * @return The interpolated Metrics at indexAlpha. */ -inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t& indexAlpha, - const std::vector& dataArray) { +inline Metrics interpolateNew(const LinearInterpolation::index_alpha_t& indexAlpha, + const std::vector& dataArray) { assert(dataArray.size() > 0); if (dataArray.size() > 1) { // Normal interpolation case @@ -54,41 +54,60 @@ inline MetricsCollection interpolateNew(const LinearInterpolation::index_alpha_t const scalar_t alpha = indexAlpha.second; bool areSameSize = true; - const auto lhs_stateEq = toVector(dataArray[index].stateEqLagrangian); - const auto rhs_stateEq = toVector(dataArray[index + 1].stateEqLagrangian); - areSameSize = areSameSize && (lhs_stateEq.size() == rhs_stateEq.size()); + const auto lhs_stateEqConst = toVector(dataArray[index].stateEqConstraint); + const auto rhs_stateEqConst = toVector(dataArray[index + 1].stateEqConstraint); + areSameSize = areSameSize && (lhs_stateEqConst.size() == rhs_stateEqConst.size()); - const auto lhs_stateIneq = toVector(dataArray[index].stateIneqLagrangian); - const auto rhs_stateIneq = toVector(dataArray[index + 1].stateIneqLagrangian); - areSameSize = areSameSize && (lhs_stateIneq.size() == rhs_stateIneq.size()); + const auto lhs_stateInputEqConst = toVector(dataArray[index].stateInputEqConstraint); + const auto rhs_stateInputEqConst = toVector(dataArray[index + 1].stateInputEqConstraint); + areSameSize = areSameSize && (lhs_stateInputEqConst.size() == rhs_stateInputEqConst.size()); - const auto lhs_stateInputEq = toVector(dataArray[index].stateInputEqLagrangian); - const auto rhs_stateInputEq = toVector(dataArray[index + 1].stateInputEqLagrangian); - areSameSize = areSameSize && (lhs_stateInputEq.size() == rhs_stateInputEq.size()); + const auto lhs_stateIneqConst = toVector(dataArray[index].stateIneqConstraint); + const auto rhs_stateIneqConst = toVector(dataArray[index + 1].stateIneqConstraint); + areSameSize = areSameSize && (lhs_stateIneqConst.size() == rhs_stateIneqConst.size()); - const auto lhs_stateInputIneq = toVector(dataArray[index].stateInputIneqLagrangian); - const auto rhs_stateInputIneq = toVector(dataArray[index + 1].stateInputIneqLagrangian); - areSameSize = areSameSize && (lhs_stateInputIneq.size() == rhs_stateInputIneq.size()); + const auto lhs_stateInputIneqConst = toVector(dataArray[index].stateInputIneqConstraint); + const auto rhs_stateInputIneqConst = toVector(dataArray[index + 1].stateInputIneqConstraint); + areSameSize = areSameSize && (lhs_stateInputIneqConst.size() == rhs_stateInputIneqConst.size()); + + const auto lhs_stateEqLag = toVector(dataArray[index].stateEqLagrangian); + const auto rhs_stateEqLag = toVector(dataArray[index + 1].stateEqLagrangian); + areSameSize = areSameSize && (lhs_stateEqLag.size() == rhs_stateEqLag.size()); + + const auto lhs_stateIneqLag = toVector(dataArray[index].stateIneqLagrangian); + const auto rhs_stateIneqLag = toVector(dataArray[index + 1].stateIneqLagrangian); + areSameSize = areSameSize && (lhs_stateIneqLag.size() == rhs_stateIneqLag.size()); + + const auto lhs_stateInputEqLag = toVector(dataArray[index].stateInputEqLagrangian); + const auto rhs_stateInputEqLag = toVector(dataArray[index + 1].stateInputEqLagrangian); + areSameSize = areSameSize && (lhs_stateInputEqLag.size() == rhs_stateInputEqLag.size()); + + const auto lhs_stateInputIneqLag = toVector(dataArray[index].stateInputIneqLagrangian); + const auto rhs_stateInputIneqLag = toVector(dataArray[index + 1].stateInputIneqLagrangian); + areSameSize = areSameSize && (lhs_stateInputIneqLag.size() == rhs_stateInputIneqLag.size()); if (areSameSize) { const auto f = [alpha](const vector_t& lhs, const vector_t& rhs) -> vector_t { return alpha * lhs + (scalar_t(1.0) - alpha) * rhs; }; - MetricsCollection out; + Metrics out; // cost out.cost = LinearInterpolation::interpolate( - indexAlpha, dataArray, [](const std::vector& array, size_t t) -> const scalar_t& { return array[t].cost; }); - // constraints - out.stateEqConstraint = LinearInterpolation::interpolate( - indexAlpha, dataArray, - [](const std::vector& array, size_t t) -> const vector_t& { return array[t].stateEqConstraint; }); - out.stateInputEqConstraint = LinearInterpolation::interpolate( - indexAlpha, dataArray, - [](const std::vector& array, size_t t) -> const vector_t& { return array[t].stateInputEqConstraint; }); - out.stateEqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateEqLagrangian), f(lhs_stateEq, rhs_stateEq)); - out.stateIneqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateIneqLagrangian), f(lhs_stateIneq, rhs_stateIneq)); + indexAlpha, dataArray, [](const std::vector& array, size_t t) -> const scalar_t& { return array[t].cost; }); + // dynamics violation + out.dynamicsViolation = LinearInterpolation::interpolate( + indexAlpha, dataArray, [](const std::vector& array, size_t t) -> const vector_t& { return array[t].dynamicsViolation; }); + // equality constraints + out.stateEqConstraint = toConstraintArray(getSizes(dataArray[index].stateEqConstraint), f(lhs_stateEqConst, rhs_stateEqConst)); + out.stateInputEqConstraint = toConstraintArray(getSizes(dataArray[index].stateInputEqConstraint), f(lhs_stateInputEqConst, rhs_stateInputEqConst)); + // inequality constraints + out.stateIneqConstraint = toConstraintArray(getSizes(dataArray[index].stateIneqConstraint), f(lhs_stateIneqConst, rhs_stateIneqConst)); + out.stateInputIneqConstraint = toConstraintArray(getSizes(dataArray[index].stateInputIneqConstraint), f(lhs_stateInputIneqConst, rhs_stateInputIneqConst)); + // lagrangian + out.stateEqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateEqLagrangian), f(lhs_stateEqLag, rhs_stateEqLag)); + out.stateIneqLagrangian = toLagrangianMetrics(getSizes(dataArray[index].stateIneqLagrangian), f(lhs_stateIneqLag, rhs_stateIneqLag)); out.stateInputEqLagrangian = - toLagrangianMetrics(getSizes(dataArray[index].stateInputEqLagrangian), f(lhs_stateInputEq, rhs_stateInputEq)); + toLagrangianMetrics(getSizes(dataArray[index].stateInputEqLagrangian), f(lhs_stateInputEqLag, rhs_stateInputEqLag)); out.stateInputIneqLagrangian = - toLagrangianMetrics(getSizes(dataArray[index].stateInputIneqLagrangian), f(lhs_stateInputIneq, rhs_stateInputIneq)); + toLagrangianMetrics(getSizes(dataArray[index].stateInputIneqLagrangian), f(lhs_stateInputIneqLag, rhs_stateInputIneqLag)); return out; } else { @@ -106,15 +125,10 @@ void random(const size_array_t& termsSize, std::vector& lagra lagrangianMetrics = toLagrangianMetrics(termsSize, serializedLagrangianMetrics); } -bool isApprox(const MetricsCollection& lhs, const MetricsCollection& rhs, scalar_t prec) { - bool flag = std::abs(lhs.cost - rhs.cost) < prec; - flag = flag && lhs.stateEqConstraint.isApprox(rhs.stateEqConstraint, prec); - flag = flag && lhs.stateInputEqConstraint.isApprox(rhs.stateInputEqConstraint, prec); - flag = flag && toVector(lhs.stateEqLagrangian).isApprox(toVector(rhs.stateEqLagrangian), prec); - flag = flag && toVector(lhs.stateIneqLagrangian).isApprox(toVector(rhs.stateIneqLagrangian), prec); - flag = flag && toVector(lhs.stateInputEqLagrangian).isApprox(toVector(rhs.stateInputEqLagrangian), prec); - flag = flag && toVector(lhs.stateInputIneqLagrangian).isApprox(toVector(rhs.stateInputIneqLagrangian), prec); - return flag; +void random(const size_array_t& termsSize, vector_array_t& constraintArray) { + const size_t length = std::accumulate(termsSize.begin(), termsSize.end(), termsSize.size()); + const vector_t serializedConstraintArray = vector_t::Random(length); + constraintArray = toConstraintArray(termsSize, serializedConstraintArray); } } // unnamed namespace @@ -126,34 +140,37 @@ TEST(TestMetrics, testSerialization) { const size_t length = std::accumulate(termsSize.begin(), termsSize.end(), numConstraint); const ocs2::vector_t randomVec = ocs2::vector_t::Random(length); - const std::vector termsMetricsCollection = ocs2::toLagrangianMetrics(termsSize, randomVec); - const ocs2::vector_t serializedMetricsCollection = ocs2::toVector(termsMetricsCollection); + const std::vector l = ocs2::toLagrangianMetrics(termsSize, randomVec); + const ocs2::vector_t serialized_l = ocs2::toVector(l); - EXPECT_TRUE(serializedMetricsCollection == randomVec); - EXPECT_TRUE(serializedMetricsCollection.size() == length); - EXPECT_TRUE(getSizes(termsMetricsCollection) == termsSize); + EXPECT_TRUE(serialized_l == randomVec); + EXPECT_TRUE(serialized_l.size() == length); + EXPECT_TRUE(getSizes(l) == termsSize); } TEST(TestMetrics, testSwap) { const ocs2::size_array_t termsSize{0, 2, 0, 0, 3, 5}; - ocs2::MetricsCollection termsMetricsCollection; - termsMetricsCollection.cost = ocs2::vector_t::Random(1)(0); - termsMetricsCollection.stateEqConstraint = ocs2::vector_t::Random(2); - termsMetricsCollection.stateInputEqConstraint = ocs2::vector_t::Random(3); - ocs2::random(termsSize, termsMetricsCollection.stateEqLagrangian); - ocs2::random(termsSize, termsMetricsCollection.stateIneqLagrangian); - ocs2::random(termsSize, termsMetricsCollection.stateInputEqLagrangian); - ocs2::random(termsSize, termsMetricsCollection.stateInputIneqLagrangian); - - auto termsMetricsCollectionRef = termsMetricsCollection; - ocs2::MetricsCollection termsMultiplierNew; - termsMetricsCollection.swap(termsMultiplierNew); - - EXPECT_TRUE(ocs2::isApprox(termsMultiplierNew, termsMetricsCollectionRef, 1e-10)); - - termsMetricsCollectionRef.clear(); - EXPECT_TRUE(ocs2::isApprox(termsMetricsCollection, termsMetricsCollectionRef, 1e-10)); + ocs2::Metrics termsMetrics; + termsMetrics.cost = ocs2::vector_t::Random(1)(0); + termsMetrics.dynamicsViolation.setRandom(2); + ocs2::random(termsSize, termsMetrics.stateEqConstraint); + ocs2::random(termsSize, termsMetrics.stateInputEqConstraint); + ocs2::random(termsSize, termsMetrics.stateIneqConstraint); + ocs2::random(termsSize, termsMetrics.stateInputIneqConstraint); + ocs2::random(termsSize, termsMetrics.stateEqLagrangian); + ocs2::random(termsSize, termsMetrics.stateIneqLagrangian); + ocs2::random(termsSize, termsMetrics.stateInputEqLagrangian); + ocs2::random(termsSize, termsMetrics.stateInputIneqLagrangian); + + auto termsMetricsRef = termsMetrics; + ocs2::Metrics termsMetricsNew; + termsMetrics.swap(termsMetricsNew); + + EXPECT_TRUE(termsMetricsNew.isApprox(termsMetricsRef, 1e-10)); + + termsMetricsRef.clear(); + EXPECT_TRUE(termsMetrics.isApprox(termsMetricsRef, 1e-10)); } TEST(TestMetrics, testInterpolation) { @@ -170,16 +187,19 @@ TEST(TestMetrics, testInterpolation) { ocs2::vector_array_t stateIneqTrajectory(N); ocs2::vector_array_t stateInputEqTrajectory(N); ocs2::vector_array_t stateInputIneqTrajectory(N); - std::vector metricsCollectionTrajectory(N); + std::vector metricsTrajectory(N); for (size_t i = 0; i < N; i++) { timeTrajectory[i] = i * 0.1; - metricsCollectionTrajectory[i].cost = ocs2::vector_t::Random(1)(0); - metricsCollectionTrajectory[i].stateEqConstraint = ocs2::vector_t::Random(2); - metricsCollectionTrajectory[i].stateInputEqConstraint = ocs2::vector_t::Random(3); - ocs2::random(stateEqTermsSize, metricsCollectionTrajectory[i].stateEqLagrangian); - ocs2::random(stateIneqTermsSize, metricsCollectionTrajectory[i].stateIneqLagrangian); - ocs2::random(stateInputEqTermsSize, metricsCollectionTrajectory[i].stateInputEqLagrangian); - ocs2::random(stateInputIneqTermsSize, metricsCollectionTrajectory[i].stateInputIneqLagrangian); + metricsTrajectory[i].cost = ocs2::vector_t::Random(1)(0); + metricsTrajectory[i].dynamicsViolation.setRandom(2); + ocs2::random(stateEqTermsSize, metricsTrajectory[i].stateEqConstraint); + ocs2::random(stateInputEqTermsSize, metricsTrajectory[i].stateInputEqConstraint); + ocs2::random(stateIneqTermsSize, metricsTrajectory[i].stateIneqConstraint); + ocs2::random(stateInputIneqTermsSize, metricsTrajectory[i].stateInputIneqConstraint); + ocs2::random(stateEqTermsSize, metricsTrajectory[i].stateEqLagrangian); + ocs2::random(stateIneqTermsSize, metricsTrajectory[i].stateIneqLagrangian); + ocs2::random(stateInputEqTermsSize, metricsTrajectory[i].stateInputEqLagrangian); + ocs2::random(stateInputIneqTermsSize, metricsTrajectory[i].stateInputIneqLagrangian); } // end of i loop constexpr ocs2::scalar_t prec = 1e-8; @@ -187,9 +207,9 @@ TEST(TestMetrics, testInterpolation) { for (size_t i = 0; i < timeTrajectoryTest.size(); i++) { const auto indexAlpha = ocs2::LinearInterpolation::timeSegment(timeTrajectoryTest[i], timeTrajectory); - const auto metricsCollection = ocs2::LinearInterpolation::interpolate(indexAlpha, metricsCollectionTrajectory); - const auto metricsCollectionNew = ocs2::interpolateNew(indexAlpha, metricsCollectionTrajectory); + const auto metrics = ocs2::LinearInterpolation::interpolate(indexAlpha, metricsTrajectory); + const auto metricsNew = ocs2::interpolateNew(indexAlpha, metricsTrajectory); - EXPECT_TRUE(ocs2::isApprox(metricsCollection, metricsCollectionNew, prec)); + EXPECT_TRUE(metrics.isApprox(metricsNew, prec)); } // end of i loop } diff --git a/ocs2_core/test/soft_constraint/testSoftConstraint.cpp b/ocs2_core/test/soft_constraint/testSoftConstraint.cpp index daef4dac8..699e67388 100644 --- a/ocs2_core/test/soft_constraint/testSoftConstraint.cpp +++ b/ocs2_core/test/soft_constraint/testSoftConstraint.cpp @@ -36,7 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -class TestStateConstraint : public ocs2::StateConstraint { +class TestStateConstraint final : public ocs2::StateConstraint { public: TestStateConstraint(ocs2::ConstraintOrder constraintOrder, size_t numConstraints) : StateConstraint(constraintOrder), numConstraints_(numConstraints) {} @@ -96,19 +96,19 @@ template std::unique_ptr softConstraintFactory(size_t numConstraints, ocs2::ConstraintOrder constraintOrder, bool useSimilarPenalty) { // constraint - std::unique_ptr constraintPtr(new Constraint(constraintOrder, numConstraints)); + auto constraintPtr = std::make_unique(constraintOrder, numConstraints); if (useSimilarPenalty) { // penalty function - std::unique_ptr penaltyFunctionPtr(new ocs2::RelaxedBarrierPenalty({10.0, 1.0})); - return std::unique_ptr(new SoftConstraint(std::move(constraintPtr), std::move(penaltyFunctionPtr))); + auto penaltyFunctionPtr = std::make_unique(ocs2::RelaxedBarrierPenalty::Config{10.0, 1.0}); + return std::make_unique(std::move(constraintPtr), std::move(penaltyFunctionPtr)); } else { std::vector> penaltyFunctionPtrArry; for (size_t i = 0; i < numConstraints; i++) { penaltyFunctionPtrArry.emplace_back(new ocs2::RelaxedBarrierPenalty({10.0, 1.0})); } // end of i loop - return std::unique_ptr(new SoftConstraint(std::move(constraintPtr), std::move(penaltyFunctionPtrArry))); + return std::make_unique(std::move(constraintPtr), std::move(penaltyFunctionPtrArry)); } } @@ -184,9 +184,9 @@ class ActivityTestStateConstraint : public ocs2::StateConstraint { TEST(testSoftConstraint, checkActivityStateConstraint) { constexpr size_t numConstraints = 10; - std::unique_ptr constraint(new ActivityTestStateConstraint); - std::unique_ptr penalty(new ocs2::RelaxedBarrierPenalty({10.0, 1.0})); - std::unique_ptr softConstraint(new ocs2::StateSoftConstraint(std::move(constraint), std::move(penalty))); + auto constraint = std::make_unique(); + auto penalty = std::make_unique(ocs2::RelaxedBarrierPenalty::Config{10.0, 1.0}); + auto softConstraint = std::make_unique(std::move(constraint), std::move(penalty)); softConstraint->get().setActivity(true); EXPECT_TRUE(std::unique_ptr(softConstraint->clone())->isActive(0.0)); @@ -216,10 +216,9 @@ class ActivityTestStateInputConstraint : public ocs2::StateInputConstraint { TEST(testSoftConstraint, checkActivityStateInputConstraint) { constexpr size_t numConstraints = 10; - std::unique_ptr constraint(new ActivityTestStateInputConstraint); - std::unique_ptr penalty(new ocs2::RelaxedBarrierPenalty({10.0, 1.0})); - std::unique_ptr softConstraint( - new ocs2::StateInputSoftConstraint(std::move(constraint), std::move(penalty))); + auto constraint = std::make_unique(); + auto penalty = std::make_unique(ocs2::RelaxedBarrierPenalty::Config{10.0, 1.0}); + auto softConstraint = std::make_unique(std::move(constraint), std::move(penalty)); softConstraint->get().setActivity(true); EXPECT_TRUE(std::unique_ptr(softConstraint->clone())->isActive(0.0)); diff --git a/ocs2_core/test/testTypes.cpp b/ocs2_core/test/testTypes.cpp index 947ca7c3f..b926efee5 100644 --- a/ocs2_core/test/testTypes.cpp +++ b/ocs2_core/test/testTypes.cpp @@ -2,30 +2,30 @@ Copyright (c) 2020, Farbod Farshidian. All rights reserved. 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. - - * Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - - 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. - ******************************************************************************/ +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 diff --git a/ocs2_core/test/thread_support/testSynchronized.cpp b/ocs2_core/test/thread_support/testSynchronized.cpp index 13425b934..ec3164ce9 100644 --- a/ocs2_core/test/thread_support/testSynchronized.cpp +++ b/ocs2_core/test/thread_support/testSynchronized.cpp @@ -35,7 +35,7 @@ TEST(testLockable, construction) { ASSERT_FALSE(defaultObj.lock()); // from pointer - std::unique_ptr doublePtr(new double(1.0)); + auto doublePtr = std::make_unique(1.0); ocs2::Synchronized nonDefaultObj(std::move(doublePtr)); ASSERT_TRUE(nonDefaultObj.lock()); } @@ -176,4 +176,4 @@ TEST(testLockable, lockMultiple) { synchronizedA.getMutex().unlock(); synchronizedB.getMutex().unlock(); synchronizedDouble.getMutex().unlock(); -} \ No newline at end of file +} diff --git a/ocs2_ddp/CMakeLists.txt b/ocs2_ddp/CMakeLists.txt index 5b133f45d..97606f278 100644 --- a/ocs2_ddp/CMakeLists.txt +++ b/ocs2_ddp/CMakeLists.txt @@ -82,7 +82,7 @@ if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) add_clang_tooling( TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR ) @@ -201,3 +201,13 @@ target_link_libraries(testDdpHelperFunction ${PROJECT_NAME} gtest_main ) + +catkin_add_gtest(testReachingTask + test/testReachingTask.cpp +) +target_link_libraries(testReachingTask + ${Boost_LIBRARIES} + ${catkin_LIBRARIES} + ${PROJECT_NAME} + gtest_main +) diff --git a/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h b/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h index d7fecaa0a..2adca453e 100644 --- a/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h +++ b/ocs2_ddp/include/ocs2_ddp/DDP_HelperFunctions.h @@ -34,8 +34,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include -#include #include #include "ocs2_ddp/DDP_Data.h" @@ -80,6 +80,17 @@ PerformanceIndex computeRolloutPerformanceIndex(const scalar_array_t& timeTrajec scalar_t rolloutTrajectory(RolloutBase& rollout, scalar_t initTime, const vector_t& initState, scalar_t finalTime, PrimalSolution& primalSolution); +/** + * Projects the unconstrained LQ coefficients to constrained ones. + * + * @param [in] modelData: The model data. + * @param [in] constraintRangeProjector: The projection matrix to the constrained subspace. + * @param [in] constraintNullProjector: The projection matrix to the null space of constrained. + * @param [out] projectedModelData: The projected model data. + */ +void projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, const matrix_t& constraintNullProjector, + ModelData& projectedModelData); + /** * Extract a primal solution for the range [initTime, finalTime] from a given primal solution. It assumes that the * given range is within the solution time of input primal solution. @@ -94,6 +105,14 @@ scalar_t rolloutTrajectory(RolloutBase& rollout, scalar_t initTime, const vector void extractPrimalSolution(const std::pair& timePeriod, const PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution); +/** + * Calculates max feedforward update norm of the controller. + * + * @param [in] controller: Control policy + * @return max feedforward update norm. + */ +scalar_t maxControllerUpdateNorm(const LinearController& controller); + /** * Computes the integral of the squared (IS) norm of the controller update. * @@ -135,6 +154,26 @@ void retrieveActiveNormalizedTime(const std::pair& partitionInterval, const size_array_t& postEventIndices, scalar_array_t& normalizedTimeTrajectory, size_array_t& normalizedPostEventIndices); +/** + * Get the Partition Intervals From Time Trajectory. Intervals are defined as [start, end). + * + * Pay attention, the rightmost index of the end partition is (..., timeArray.size() - 1) , as the last value function is filled manually. + * The reason is though we don’t write to the end index, we do have to read it. Adding the last index to the final partition will + * cause a segmentation fault. There is no trivial method to distinguish the final partition from other partitions because, by design, + * partitions should be treated equally. + * + * Every time point that is equal or larger to the desiredPartitionPoint should be included in that partition. This logic here is the same + * as the event times. + * + * The last time of desiredPartitionPoints is filled manually. There is no round-off error involved. So it is safe to use == for + * floating-point numbers. The last time point is naturally included by using std::lower_bound. + * + * @param [in] timeTrajectory: time trajectory that will be divided + * @param [in] numWorkers: number of worker i.e. number of partitions + * @return array of index pairs indicating the start and end of each partition + */ +std::vector> computePartitionIntervals(const scalar_array_t& timeTrajectory, int numWorkers); + /** * Gets a reference to the linear controller from the given primal solution. */ diff --git a/ocs2_ddp/include/ocs2_ddp/DDP_Settings.h b/ocs2_ddp/include/ocs2_ddp/DDP_Settings.h index 9cb6c76d6..f6b41fd68 100644 --- a/ocs2_ddp/include/ocs2_ddp/DDP_Settings.h +++ b/ocs2_ddp/include/ocs2_ddp/DDP_Settings.h @@ -96,13 +96,12 @@ struct Settings { /** The backward pass integrator type: SLQ uses it for solving Riccati equation and ILQR uses it for discretizing LQ approximation. */ IntegratorType backwardPassIntegratorType_ = IntegratorType::ODE45; - /** The initial coefficient of the quadratic penalty function in augmented Lagrangian method. It should be greater than one. */ + /** The initial coefficient of the quadratic penalty function in the merit function. It should be greater than one. */ scalar_t constraintPenaltyInitialValue_ = 2.0; - /** The rate that the coefficient of the quadratic penalty function in augmented Lagrangian method grows. It should be greater than - * one. */ + /** The rate that the coefficient of the quadratic penalty function in the merit function grows. It should be greater than one. */ scalar_t constraintPenaltyIncreaseRate_ = 2.0; - /** If true, terms of the Riccati equation will be precomputed before interpolation in the flow-map */ + /** If true, terms of the Riccati equation will be pre-computed before interpolation in the flow-map */ bool preComputeRiccatiTerms_ = true; /** Use either the optimized control policy (true) or the optimized state-input trajectory (false). */ diff --git a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h index b24f50c26..54af07364 100644 --- a/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h +++ b/ocs2_ddp/include/ocs2_ddp/GaussNewtonDDP.h @@ -89,7 +89,7 @@ class GaussNewtonDDP : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const final; - const DualSolution& getDualSolution() const override { return optimizedDualSolution_; } + const DualSolution* getDualSolution() const override { return &optimizedDualSolution_; } const ProblemMetrics& getSolutionMetrics() const override { return optimizedProblemMetrics_; } @@ -121,7 +121,9 @@ class GaussNewtonDDP : public SolverBase { * @param [in] taskFunction: task function * @param [in] N: number of times to run taskFunction, if N = 1 it is run in the main thread */ - void runParallel(std::function taskFunction, size_t N); + void runParallel(std::function taskFunction, size_t N) { + threadPool_.runParallel([&](int) { taskFunction(); }, N); + } /** * Takes the following steps: (1) Computes the Hessian of the Hamiltonian (i.e., Hm) (2) Based on Hm, it calculates @@ -168,7 +170,7 @@ class GaussNewtonDDP : public SolverBase { /** * Solves Riccati equations. * - * @param [in] finalValueFunction The final Sm(dfdxx), Sv(dfdx), s(f), for Riccati equation. + * @param [in] finalValueFunction: The final value of Sm (dfdxx), Sv (dfdx), s (f), for Riccati equation. * @return average time step */ virtual scalar_t solveSequentialRiccatiEquations(const ScalarFunctionQuadraticApproximation& finalValueFunction) = 0; @@ -230,35 +232,33 @@ class GaussNewtonDDP : public SolverBase { } /** - * Get the Partition Intervals From Time Trajectory. Intervals are defined as [start, end). - * - * Pay attention, the rightmost index of the end partition is (..., timeArray.size() - 1) , as the last value function is filled manually. - * The reason is though we don’t write to the end index, we do have to read it. Adding the last index to the final partition will - * cause a segmentation fault. There is no trivial method to distinguish the final partition from other partitions because, by design, - * partitions should be treated equally. - * - * Every time point that is equal or larger to the desiredPartitionPoint should be included in that partition. This logic here is the same - * as the event times. + * Forward integrate the system dynamics with the controller in inputPrimalSolution. In general, it uses the given + * control policies and the initial state, to integrate the system dynamics in the time period [initTime, finalTime]. + * However, if inputPrimalSolution's controller does not cover the period [initTime, finalTime], it will use the + * controller till the final time of the controller * - * The last time of desiredPartitionPoints is filled manually. There is no round-off error involved. So it is safe to use == for - * floating-point numbers. The last time point is naturally included by using std::lower_bound. - * - * @param [in] timeTrajectory: time trajectory that will be divided - * @param [in] numWorkers: number of worker i.e. number of partitions - * @return array of index pairs indicating the start and end of each partition + * @param [in] inputPrimalSolution: Its controller will be used for rollout. + * @param [out] outputPrimalSolution: The resulting PrimalSolution. + * @return True if the rollout was successful. */ - std::vector> getPartitionIntervalsFromTimeTrajectory(const scalar_array_t& timeTrajectory, int numWorkers); + bool rolloutInitialController(PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution); /** - * Forward integrate the system dynamics with given controller in primalSolution and operating trajectories. In general, it uses - * the given control policies and initial state, to integrate the system dynamics in the time period [initTime, finalTime]. - * However, if the provided controller does not cover the period [initTime, finalTime], it will use the controller till the - * final time of the controller and after it uses the operating trajectories. + * Extracts the PrimalSolution trajectories from inputPrimalSolution. In general, it will try to extract in time period + * [initTime, finalTime]. However, if inputPrimalSolution's timeTrajectory does not cover the period [initTime, finalTime], + * it will extract the solution until the last time of the timeTrajectory * - * @param [in, out] primalSolution: The resulting state-input trajectory. The primal solution is initialized with the controller - * and the modeSchedule. However, for StateTriggered Rollout the modeSchedule can be overwritten. + * @param [in] inputPrimalSolution: Its controller will be used for rollout. + * @param [out] outputPrimalSolution: The resulting PrimalSolution. + * @return True if the extraction was successful. + */ + bool extractInitialTrajectories(PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution); + + /** + * It will check the content of the primalSolution, and if its final time is smaller than the current solver finalTime_, + * it will concatenate it with the result of Initializer. */ - void rolloutInitialTrajectory(PrimalSolution& primalSolution); + void rolloutInitializer(PrimalSolution& primalSolution); /** * Calculates the controller. This method uses the following variables. The method modifies unoptimizedController_. @@ -278,21 +278,6 @@ class GaussNewtonDDP : public SolverBase { */ scalar_t calculateRolloutMerit(const PerformanceIndex& performanceIndex) const; - /** - * Calculates max feedforward update norm and max type-1 error update norm. - * - * @param maxDeltaUffNorm: max feedforward update norm. - * @param maxDeltaUeeNorm: max type-1 error update norm. - */ - - /** - * Calculates max feedforward update norm of the controller. - * - * @param [in] controller: Control policy - * @return max feedforward update norm. - */ - scalar_t maxControllerUpdateNorm(const LinearController& controller) const; - /** * Approximates the nonlinear problem as a linear-quadratic problem around the * nominal state and control trajectories. This method updates the following @@ -316,20 +301,7 @@ class GaussNewtonDDP : public SolverBase { void computeProjections(const matrix_t& Hm, const matrix_t& Dm, matrix_t& constraintRangeProjector, matrix_t& constraintNullProjector) const; - /** - * Projects the unconstrained LQ coefficients to constrained ones. - * - * @param [in] modelData: The model data. - * @param [in] constraintRangeProjector: The projection matrix to the constrained subspace. - * @param [in] constraintNullProjector: The projection matrix to the null space of constrained. - * @param [out] projectedModelData: The projected model data. - */ - void projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, const matrix_t& constraintNullProjector, - ModelData& projectedModelData) const; - - /** - * Initialize the constraint penalty coefficients. - */ + /** Initialize the constraint penalty coefficients. */ void initializeConstraintPenalties(); /** @@ -338,16 +310,19 @@ class GaussNewtonDDP : public SolverBase { */ void updateConstraintPenalties(scalar_t equalityConstraintsSSE); - /** - * Runs the initialization method for Gauss-Newton DDP. + /** Initializes the nominal primal based on the optimized ones. + * @return True if the rollout is not purely from the Initializer. */ - void runInit(); + bool initializePrimalSolution(); /** - * Runs a single iteration of Gauss-Newton DDP. - * @param [in] lqModelExpectedCost: The expected cost based on the LQ model optimization. + * Initializes the nominal dual solutions based on the optimized ones and nominal primal solution. + * Moreover, it updates ProblemMetrics. */ - void runIteration(scalar_t lqModelExpectedCost); + void initializeDualSolutionAndMetrics(); + + /** Based on the current LQ solution updates the optimized primal and dual solutions. */ + void takePrimalDualStep(scalar_t lqModelExpectedCost); /** * Checks convergence of the main loop of DDP. @@ -364,13 +339,7 @@ class GaussNewtonDDP : public SolverBase { void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const ControllerBase* externalControllerPtr) override; - void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) override { - if (primalSolution.controllerPtr_ == nullptr) { - std::cerr - << "[GaussNewtonDDP] DDP cannot be warm started without a controller in the primal solution. Will revert to a cold start.\n"; - } - runImpl(initTime, initState, finalTime, primalSolution.controllerPtr_.get()); - } + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) override; protected: // nominal data diff --git a/ocs2_ddp/include/ocs2_ddp/HessianCorrection.h b/ocs2_ddp/include/ocs2_ddp/HessianCorrection.h index 26ea4753a..deb3765b2 100644 --- a/ocs2_ddp/include/ocs2_ddp/HessianCorrection.h +++ b/ocs2_ddp/include/ocs2_ddp/HessianCorrection.h @@ -58,34 +58,11 @@ Strategy fromString(const std::string& name); /** * Shifts the Hessian based on the strategy defined by Line_Search::hessianCorrectionStrategy_. * - * @tparam Derived type. * @param [in] strategy: Hessian matrix correction strategy. - * @param matrix: The Hessian matrix. + * @param [in, out] matrix: The Hessian matrix. * @param [in] minEigenvalue: The minimum expected eigenvalue after correction. */ -template -void shiftHessian(Strategy strategy, Eigen::MatrixBase& matrix, - scalar_t minEigenvalue = numeric_traits::limitEpsilon()) { - assert(matrix.rows() == matrix.cols()); - switch (strategy) { - case Strategy::DIAGONAL_SHIFT: { - matrix.diagonal().array() += minEigenvalue; - break; - } - case Strategy::CHOLESKY_MODIFICATION: { - LinearAlgebra::makePsdCholesky(matrix, minEigenvalue); - break; - } - case Strategy::EIGENVALUE_MODIFICATION: { - LinearAlgebra::makePsdEigenvalue(matrix, minEigenvalue); - break; - } - case Strategy::GERSHGORIN_MODIFICATION: { - LinearAlgebra::makePsdGershgorin(matrix, minEigenvalue); - break; - } - } -} +void shiftHessian(Strategy strategy, matrix_t& matrix, scalar_t minEigenvalue = numeric_traits::limitEpsilon()); } // namespace hessian_correction } // namespace ocs2 diff --git a/ocs2_ddp/include/ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h b/ocs2_ddp/include/ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h index 01221f3ce..f48c909fd 100644 --- a/ocs2_ddp/include/ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h +++ b/ocs2_ddp/include/ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h @@ -83,16 +83,26 @@ class LevenbergMarquardtStrategy final : public SearchStrategyBase { matrix_t augmentHamiltonianHessian(const ModelData& modelData, const matrix_t& Hm) const override; private: + /** computes the ratio between actual reduction and predicted reduction */ + scalar_t reductionToPredictedReduction(const scalar_t actualReduction, const scalar_t expectedReduction) const { + if (std::abs(actualReduction) < baseSettings_.minRelCost || expectedReduction <= baseSettings_.minRelCost) { + return 1.0; + } else if (actualReduction < 0.0) { + return 0.0; + } else { + return actualReduction / expectedReduction; + } + } + // Levenberg-Marquardt struct LevenbergMarquardtModule { - scalar_t pho = 1.0; // the ratio between actual reduction and predicted reduction scalar_t riccatiMultiple = 0.0; // the Riccati multiple for Tikhonov regularization. scalar_t riccatiMultipleAdaptiveRatio = 1.0; // the adaptive ratio of geometric progression for Riccati multiple. size_t numSuccessiveRejections = 0; // the number of successive rejections of solution. }; const levenberg_marquardt::Settings settings_; - LevenbergMarquardtModule levenbergMarquardtModule_; + LevenbergMarquardtModule lmModule_; RolloutBase& rolloutRef_; OptimalControlProblem& optimalControlProblemRef_; diff --git a/ocs2_ddp/include/ocs2_ddp/search_strategy/SearchStrategyBase.h b/ocs2_ddp/include/ocs2_ddp/search_strategy/SearchStrategyBase.h index 7192b3864..8d57eca51 100644 --- a/ocs2_ddp/include/ocs2_ddp/search_strategy/SearchStrategyBase.h +++ b/ocs2_ddp/include/ocs2_ddp/search_strategy/SearchStrategyBase.h @@ -38,11 +38,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include - #include +#include #include #include -#include #include "ocs2_ddp/search_strategy/StrategySettings.h" diff --git a/ocs2_ddp/include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h b/ocs2_ddp/include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h index 73085e948..df32f839e 100644 --- a/ocs2_ddp/include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h +++ b/ocs2_ddp/include/ocs2_ddp/unsupported/SLQ_Hamiltonian.h @@ -72,9 +72,6 @@ class SLQ_Hamiltonian : public DDP_BASE { using scalar_array_t = typename BASE::scalar_array_t; using scalar_array2_t = typename BASE::scalar_array2_t; using scalar_array3_t = typename BASE::scalar_array3_t; - using eigen_scalar_t = typename BASE::eigen_scalar_t; - using eigen_scalar_array_t = typename BASE::eigen_scalar_array_t; - using eigen_scalar_array2_t = typename BASE::eigen_scalar_array2_t; using state_vector_t = typename BASE::state_vector_t; using state_vector_array_t = typename BASE::state_vector_array_t; using state_vector_array2_t = typename BASE::state_vector_array2_t; @@ -275,7 +272,7 @@ class SLQ_Hamiltonian : public DDP_BASE { * @param [in] sFinal: The final s for Riccati equation. */ void solveRiccatiEquationsWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, const eigen_scalar_t& sFinal); + const state_vector_t& SvFinal, const scalar_t sFinal); /** * Solves a set of Riccati equations for the partition in the given index for nominal time trajectory stamp. @@ -288,7 +285,7 @@ class SLQ_Hamiltonian : public DDP_BASE { * @param [in] sFinal: The final s for the current Riccati equation. */ void solveRiccatiEquationsForNominalTimeWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, const eigen_scalar_t& sFinal); + const state_vector_t& SvFinal, const scalar_t sFinal); /** * Type_1 constraints error correction compensation which solves a set of error Riccati equations for the partition in the given index. @@ -310,7 +307,7 @@ class SLQ_Hamiltonian : public DDP_BASE { * @param [in] SveFinal: The final Sve for the current Riccati equation. */ void solveSlqRiccatiEquationsWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, const eigen_scalar_t& sFinal, const state_vector_t& SveFinal); + const state_vector_t& SvFinal, const scalar_t sFinal, const state_vector_t& SveFinal); /** * Full Backward Sweep method uses exponential method instead of ODE to solve Riccati equations. @@ -324,7 +321,7 @@ class SLQ_Hamiltonian : public DDP_BASE { * @param [in] constraintStepSize: type-1 constraint step-size */ void fullRiccatiBackwardSweepWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, const state_vector_t& SveFinal, const eigen_scalar_t& sFinal, + const state_vector_t& SvFinal, const state_vector_t& SveFinal, const scalar_t sFinal, const scalar_t& constraintStepSize); template diff --git a/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/BVPEquations.h b/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/BVPEquations.h index 077623fb1..13f96a84b 100644 --- a/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/BVPEquations.h +++ b/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/BVPEquations.h @@ -60,7 +60,6 @@ class BVPEquations : public OdeBase { using full_ode_vector_t = Eigen::Matrix; using full_ode_vector_array_t = std::vector >; - using eigen_scalar_t = Eigen::Matrix; using state_vector_t = Eigen::Matrix; using input_vector_t = Eigen::Matrix; using state_state_matrix_t = Eigen::Matrix; @@ -69,7 +68,6 @@ class BVPEquations : public OdeBase { using state_input_matrix_t = Eigen::Matrix; using scalar_array_t = std::vector; - using eigen_scalar_array_t = std::vector >; using state_vector_array_t = std::vector >; using input_vector_array_t = std::vector >; using state_state_matrix_array_t = std::vector >; diff --git a/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/SolveBVP.h b/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/SolveBVP.h index e52de8be5..1eed6f968 100644 --- a/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/SolveBVP.h +++ b/ocs2_ddp/include/ocs2_ddp/unsupported/bvp_solver/SolveBVP.h @@ -58,7 +58,6 @@ class SolveBVP { using full_ode_vector_t = typename bvp_equations_t::full_ode_vector_t; using full_ode_vector_array_t = typename bvp_equations_t::full_ode_vector_array_t; - using eigen_scalar_t = typename bvp_equations_t::eigen_scalar_t; using state_vector_t = typename bvp_equations_t::state_vector_t; using input_vector_t = typename bvp_equations_t::input_vector_t; using state_state_matrix_t = typename bvp_equations_t::state_state_matrix_t; @@ -68,7 +67,6 @@ class SolveBVP { using scalar_t = typename bvp_equations_t::scalar_t; using scalar_array_t = typename bvp_equations_t::scalar_array_t; - using eigen_scalar_array_t = typename bvp_equations_t::eigen_scalar_array_t; using state_vector_array_t = typename bvp_equations_t::state_vector_array_t; using input_vector_array_t = typename bvp_equations_t::input_vector_array_t; using state_state_matrix_array_t = typename bvp_equations_t::state_state_matrix_array_t; diff --git a/ocs2_ddp/include/ocs2_ddp/unsupported/implementation/SLQ_Hamiltonian.h b/ocs2_ddp/include/ocs2_ddp/unsupported/implementation/SLQ_Hamiltonian.h index afaa6384f..9a06a4771 100644 --- a/ocs2_ddp/include/ocs2_ddp/unsupported/implementation/SLQ_Hamiltonian.h +++ b/ocs2_ddp/include/ocs2_ddp/unsupported/implementation/SLQ_Hamiltonian.h @@ -403,7 +403,7 @@ template void SLQ_Hamiltonian::solveRiccatiEquationsWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, const state_vector_t& SvFinal, - const eigen_scalar_t& sFinal) { + const scalar_t sFinal) { // set data for Riccati equations riccatiEquationsPtrStock_[workerIndex]->resetNumFunctionCalls(); riccatiEquationsPtrStock_[workerIndex]->setData( @@ -537,11 +537,8 @@ void SLQ_Hamiltonian::solveRiccatiEquations /******************************************************************************************************/ /******************************************************************************************************/ template -void SLQ_Hamiltonian::solveRiccatiEquationsForNominalTimeWorker(size_t workerIndex, - const size_t& partitionIndex, - const state_matrix_t& SmFinal, - const state_vector_t& SvFinal, - const eigen_scalar_t& sFinal) { +void SLQ_Hamiltonian::solveRiccatiEquationsForNominalTimeWorker( + size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, const state_vector_t& SvFinal, const scalar_t sFinal) { // set data for Riccati equations riccatiEquationsPtrStock_[workerIndex]->resetNumFunctionCalls(); riccatiEquationsPtrStock_[workerIndex]->setData( @@ -813,7 +810,7 @@ template void SLQ_Hamiltonian::solveSlqRiccatiEquationsWorker(size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, const state_vector_t& SvFinal, - const eigen_scalar_t& sFinal, + const scalar_t sFinal, const state_vector_t& SveFinal) { if (settings_.useNominalTimeForBackwardPass_) { solveRiccatiEquationsForNominalTimeWorker(workerIndex, partitionIndex, SmFinal, SvFinal, sFinal); @@ -921,7 +918,7 @@ SLQ_Hamiltonian::integrateIncrement(size_t template void SLQ_Hamiltonian::fullRiccatiBackwardSweepWorker( size_t workerIndex, const size_t& partitionIndex, const state_matrix_t& SmFinal, const state_vector_t& SvFinal, - const state_vector_t& SveFinal, const eigen_scalar_t& sFinal, const scalar_t& constraintStepSize) { + const state_vector_t& SveFinal, const scalar_t sFinal, const scalar_t& constraintStepSize) { const size_t N = BASE::nominalTimeTrajectoriesStock_[partitionIndex].size(); // Riccati parameters diff --git a/ocs2_ddp/src/DDP_HelperFunctions.cpp b/ocs2_ddp/src/DDP_HelperFunctions.cpp index 7ce907a54..8e0290bf3 100644 --- a/ocs2_ddp/src/DDP_HelperFunctions.cpp +++ b/ocs2_ddp/src/DDP_HelperFunctions.cpp @@ -35,6 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include namespace ocs2 { @@ -72,7 +73,7 @@ void computeRolloutMetrics(OptimalControlProblem& problem, const PrimalSolution& problemMetrics.intermediates.reserve(tTrajectory.size()); auto nextPostEventIndexItr = postEventIndices.begin(); - const auto request = Request::Cost + Request::Constraint + Request::SoftConstraint; + constexpr auto request = Request::Cost + Request::Constraint + Request::SoftConstraint; for (size_t k = 0; k < tTrajectory.size(); k++) { // intermediate time cost and constraints problem.preComputationPtr->request(request, tTrajectory[k], xTrajectory[k], uTrajectory[k]); @@ -101,69 +102,21 @@ void computeRolloutMetrics(OptimalControlProblem& problem, const PrimalSolution& PerformanceIndex computeRolloutPerformanceIndex(const scalar_array_t& timeTrajectory, const ProblemMetrics& problemMetrics) { assert(timeTrajectory.size() == problemMetrics.intermediates.size()); - PerformanceIndex performanceIndex; - - // Total cost: - // - Final: state cost, state soft-constraints - // - PreJumps: state cost, state soft-constraints - // - Intermediates: state/state-input cost, state/state-input soft-constraints - performanceIndex.cost = problemMetrics.final.cost; - - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), - [&](const MetricsCollection& m) { performanceIndex.cost += m.cost; }); - - scalar_array_t costTrajectory(timeTrajectory.size()); - std::transform(problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), costTrajectory.begin(), - [](const MetricsCollection& m) { return m.cost; }); - performanceIndex.cost += trapezoidalIntegration(timeTrajectory, costTrajectory); - - // Dynamics violation: - performanceIndex.dynamicsViolationSSE = 0.0; - - // Equality constraints' SSE: - // - Final: state equality constraints - // - PreJumps: state equality constraints - // - Intermediates: state/state-input equality constraints - performanceIndex.equalityConstraintsSSE = problemMetrics.final.stateEqConstraint.squaredNorm(); - - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), - [&](const MetricsCollection& m) { performanceIndex.equalityConstraintsSSE += m.stateEqConstraint.squaredNorm(); }); - - scalar_array_t equalityNorm2Trajectory(timeTrajectory.size()); - std::transform(problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), equalityNorm2Trajectory.begin(), - [](const MetricsCollection& m) { return m.stateEqConstraint.squaredNorm() + m.stateInputEqConstraint.squaredNorm(); }); - performanceIndex.equalityConstraintsSSE += trapezoidalIntegration(timeTrajectory, equalityNorm2Trajectory); - - // Equality Lagrangians penalty - // - Final: state equality Lagrangians - // - PreJumps: state equality Lagrangians - // - Intermediates: state/state-input equality Lagrangians - performanceIndex.equalityLagrangian = sumPenalties(problemMetrics.final.stateEqLagrangian); - - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), - [&](const MetricsCollection& m) { performanceIndex.equalityLagrangian += sumPenalties(m.stateEqLagrangian); }); - - scalar_array_t equalityPenaltyTrajectory(timeTrajectory.size()); - std::transform(problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), equalityPenaltyTrajectory.begin(), - [&](const MetricsCollection& m) { return sumPenalties(m.stateEqLagrangian) + sumPenalties(m.stateInputEqLagrangian); }); - performanceIndex.equalityLagrangian += trapezoidalIntegration(timeTrajectory, equalityPenaltyTrajectory); - - // Inequality Lagrangians penalty - // - Final: state inequality Lagrangians - // - PreJumps: state inequality Lagrangians - // - Intermediates: state/state-input inequality Lagrangians - performanceIndex.inequalityLagrangian = sumPenalties(problemMetrics.final.stateIneqLagrangian); - - std::for_each(problemMetrics.preJumps.begin(), problemMetrics.preJumps.end(), - [&](const MetricsCollection& m) { performanceIndex.inequalityLagrangian += sumPenalties(m.stateIneqLagrangian); }); - - scalar_array_t inequalityPenaltyTrajectory(timeTrajectory.size()); - std::transform( - problemMetrics.intermediates.begin(), problemMetrics.intermediates.end(), inequalityPenaltyTrajectory.begin(), - [&](const MetricsCollection& m) { return sumPenalties(m.stateIneqLagrangian) + sumPenalties(m.stateInputIneqLagrangian); }); - performanceIndex.inequalityLagrangian += trapezoidalIntegration(timeTrajectory, inequalityPenaltyTrajectory); - - return performanceIndex; + // Final + PerformanceIndex performanceIndex = toPerformanceIndex(problemMetrics.final); + + // PreJumps + std::for_each(problemMetrics.preJumps.cbegin(), problemMetrics.preJumps.cend(), + [&performanceIndex](const Metrics& m) { performanceIndex += toPerformanceIndex(m); }); + + // Intermediates + std::vector performanceIndexTrajectory; + performanceIndexTrajectory.reserve(problemMetrics.intermediates.size()); + std::for_each(problemMetrics.intermediates.cbegin(), problemMetrics.intermediates.cend(), + [&performanceIndexTrajectory](const Metrics& m) { performanceIndexTrajectory.push_back(toPerformanceIndex(m)); }); + + // Intermediates + return trapezoidalIntegration(timeTrajectory, performanceIndexTrajectory, performanceIndex); } /******************************************************************************************************/ @@ -184,6 +137,69 @@ scalar_t rolloutTrajectory(RolloutBase& rollout, scalar_t initTime, const vector return (finalTime - initTime) / static_cast(primalSolution.timeTrajectory_.size()); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, const matrix_t& constraintNullProjector, + ModelData& projectedModelData) { + // dimensions and time + projectedModelData.time = modelData.time; + projectedModelData.stateDim = modelData.stateDim; + projectedModelData.inputDim = modelData.inputDim - modelData.stateInputEqConstraint.f.rows(); + + // unhandled constraints + projectedModelData.stateEqConstraint.f = vector_t(); + + if (modelData.stateInputEqConstraint.f.rows() == 0) { + // Change of variables u = Pu * tilde{u} + // Pu = constraintNullProjector; + + // projected state-input equality constraints + projectedModelData.stateInputEqConstraint.f.setZero(projectedModelData.inputDim); + projectedModelData.stateInputEqConstraint.dfdx.setZero(projectedModelData.inputDim, projectedModelData.stateDim); + projectedModelData.stateInputEqConstraint.dfdu.setZero(modelData.inputDim, modelData.inputDim); + + // dynamics + projectedModelData.dynamics = modelData.dynamics; + changeOfInputVariables(projectedModelData.dynamics, constraintNullProjector); + + // dynamics bias + projectedModelData.dynamicsBias = modelData.dynamicsBias; + + // cost + projectedModelData.cost = modelData.cost; + changeOfInputVariables(projectedModelData.cost, constraintNullProjector); + + } else { + // Change of variables u = Pu * tilde{u} + Px * x + u0 + // Pu = constraintNullProjector; + // Px (= -CmProjected) = -constraintRangeProjector * C + // u0 (= -EvProjected) = -constraintRangeProjector * e + + /* projected state-input equality constraints */ + projectedModelData.stateInputEqConstraint.f.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.f; + projectedModelData.stateInputEqConstraint.dfdx.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.dfdx; + projectedModelData.stateInputEqConstraint.dfdu.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.dfdu; + + // Change of variable matrices + const auto& Pu = constraintNullProjector; + const matrix_t Px = -projectedModelData.stateInputEqConstraint.dfdx; + const matrix_t u0 = -projectedModelData.stateInputEqConstraint.f; + + // dynamics + projectedModelData.dynamics = modelData.dynamics; + changeOfInputVariables(projectedModelData.dynamics, Pu, Px, u0); + + // dynamics bias + projectedModelData.dynamicsBias = modelData.dynamicsBias; + projectedModelData.dynamicsBias.noalias() += modelData.dynamics.dfdu * u0; + + // cost + projectedModelData.cost = modelData.cost; + changeOfInputVariables(projectedModelData.cost, Pu, Px, u0); + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -252,6 +268,17 @@ void extractPrimalSolution(const std::pair& timePeriod, cons } } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +scalar_t maxControllerUpdateNorm(const LinearController& controller) { + scalar_t maxDeltaUffNorm = 0.0; + for (const auto& deltaBias : controller.deltaBiasArray_) { + maxDeltaUffNorm = std::max(maxDeltaUffNorm, deltaBias.norm()); + } + return maxDeltaUffNorm; +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -260,7 +287,7 @@ scalar_t computeControllerUpdateIS(const LinearController& controller) { std::transform(controller.deltaBiasArray_.begin(), controller.deltaBiasArray_.end(), biasArraySquaredNorm.begin(), [](const vector_t& b) { return b.squaredNorm(); }); // integrates using the trapezoidal approximation method - return trapezoidalIntegration(controller.timeStamp_, biasArraySquaredNorm); + return trapezoidalIntegration(controller.timeStamp_, biasArraySquaredNorm, 0.0); } /******************************************************************************************************/ @@ -300,4 +327,33 @@ void retrieveActiveNormalizedTime(const std::pair& partitionInterval, [N, &partitionInterval](size_t i) -> size_t { return N - i + partitionInterval.first; }); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::vector> computePartitionIntervals(const scalar_array_t& timeTrajectory, int numWorkers) { + const scalar_t increment = (timeTrajectory.back() - timeTrajectory.front()) / static_cast(numWorkers); + + scalar_array_t desiredPartitionPoints(numWorkers + 1); + desiredPartitionPoints.front() = timeTrajectory.front(); + for (size_t i = 1u; i < desiredPartitionPoints.size() - 1; i++) { + desiredPartitionPoints[i] = desiredPartitionPoints[i - 1] + increment; + } + desiredPartitionPoints.back() = timeTrajectory.back(); + + std::vector> partitionIntervals; + partitionIntervals.reserve(desiredPartitionPoints.size()); + + int endPos, startPos = 0; + for (size_t i = 1u; i < desiredPartitionPoints.size(); i++) { + const auto itr = std::upper_bound(timeTrajectory.begin(), timeTrajectory.end(), desiredPartitionPoints[i]); + endPos = (itr != timeTrajectory.end()) ? std::distance(timeTrajectory.begin(), itr) : (timeTrajectory.size() - 1); + if (endPos != startPos) { + partitionIntervals.emplace_back(startPos, endPos); + startPos = endPos; + } + } + + return partitionIntervals; +} + } // namespace ocs2 diff --git a/ocs2_ddp/src/GaussNewtonDDP.cpp b/ocs2_ddp/src/GaussNewtonDDP.cpp index 9443b07cf..65ae1e8b6 100644 --- a/ocs2_ddp/src/GaussNewtonDDP.cpp +++ b/ocs2_ddp/src/GaussNewtonDDP.cpp @@ -36,7 +36,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #include #include #include @@ -55,6 +54,9 @@ namespace ocs2 { GaussNewtonDDP::GaussNewtonDDP(ddp::Settings ddpSettings, const RolloutBase& rollout, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) : ddpSettings_(std::move(ddpSettings)), threadPool_(std::max(ddpSettings_.nThreads_, size_t(1)) - 1, ddpSettings_.threadPriority_) { + Eigen::setNbThreads(1); // no multithreading within Eigen. + Eigen::initParallel(); + // check OCP if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { throw std::runtime_error( @@ -372,81 +374,84 @@ vector_t GaussNewtonDDP::getStateInputEqualityConstraintLagrangianImpl(scalar_t /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -std::vector> GaussNewtonDDP::getPartitionIntervalsFromTimeTrajectory(const scalar_array_t& timeTrajectory, - int numWorkers) { - scalar_array_t desiredPartitionPoints(numWorkers + 1); - desiredPartitionPoints.front() = timeTrajectory.front(); +bool GaussNewtonDDP::rolloutInitialController(PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution) { + if (inputPrimalSolution.controllerPtr_->empty()) { + return false; + } - const scalar_t increment = (timeTrajectory.back() - timeTrajectory.front()) / static_cast(numWorkers); - for (size_t i = 1u; i < desiredPartitionPoints.size() - 1; i++) { - desiredPartitionPoints[i] = desiredPartitionPoints[i - 1] + increment; + // cast to linear the controller + auto& inputLinearController = getLinearController(inputPrimalSolution); + + // adjust in-place the controller + std::ignore = trajectorySpread(inputPrimalSolution.modeSchedule_, getReferenceManager().getModeSchedule(), inputLinearController); + // after adjustment it might become empty + if (inputLinearController.empty()) { + return false; } - desiredPartitionPoints.back() = timeTrajectory.back(); - - std::vector> partitionIntervals; - partitionIntervals.reserve(desiredPartitionPoints.size()); - - int endPos, startPos = 0; - for (size_t i = 1u; i < desiredPartitionPoints.size(); i++) { - const scalar_t& time = desiredPartitionPoints[i]; - endPos = std::distance(timeTrajectory.begin(), std::lower_bound(timeTrajectory.begin(), timeTrajectory.end(), time)); - if (endPos != startPos) { - partitionIntervals.emplace_back(startPos, endPos); - startPos = endPos; + + const auto finalTime = std::max(initTime_, std::min(inputLinearController.timeStamp_.back(), finalTime_)); + + if (initTime_ < finalTime) { + if (ddpSettings_.debugPrintRollout_) { + std::cerr << "[GaussNewtonDDP::rolloutInitialController] for t = [" << initTime_ << ", " << finalTime_ << "]\n"; + std::cerr << "\twill use controller for t = [" << initTime_ << ", " << finalTime << "]\n"; } - } + outputPrimalSolution.controllerPtr_.swap(inputPrimalSolution.controllerPtr_); + std::ignore = rolloutTrajectory(*dynamicsForwardRolloutPtrStock_[0], initTime_, initState_, finalTime, outputPrimalSolution); + return true; - return partitionIntervals; + } else { + return false; + } } + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::runParallel(std::function taskFunction, size_t N) { - threadPool_.runParallel([&](int) { taskFunction(); }, N); +bool GaussNewtonDDP::extractInitialTrajectories(PrimalSolution& inputPrimalSolution, PrimalSolution& outputPrimalSolution) { + if (inputPrimalSolution.timeTrajectory_.empty()) { + return false; + } + + // adjust in-place the primalSolution + std::ignore = trajectorySpread(inputPrimalSolution.modeSchedule_, getReferenceManager().getModeSchedule(), inputPrimalSolution); + // after adjustment it might become empty + if (inputPrimalSolution.timeTrajectory_.empty()) { + return false; + } + + const auto finalTime = std::max(initTime_, std::min(inputPrimalSolution.timeTrajectory_.back(), finalTime_)); + + if (initTime_ < finalTime) { + if (ddpSettings_.debugPrintRollout_) { + std::cerr << "[GaussNewtonDDP::extractInitialTrajectories] for t = [" << initTime_ << ", " << finalTime_ << "]\n"; + std::cerr << "\twill use PrimalSolution trajectory for t = [" << initTime_ << ", " << finalTime << "]\n"; + } + extractPrimalSolution({initTime_, finalTime}, inputPrimalSolution, outputPrimalSolution); + return true; + + } else { + return false; + } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::rolloutInitialTrajectory(PrimalSolution& primalSolution) { +void GaussNewtonDDP::rolloutInitializer(PrimalSolution& primalSolution) { // create alias - auto* controllerPtr = primalSolution.controllerPtr_.get(); auto& modeSchedule = primalSolution.modeSchedule_; auto& timeTrajectory = primalSolution.timeTrajectory_; auto& stateTrajectory = primalSolution.stateTrajectory_; auto& inputTrajectory = primalSolution.inputTrajectory_; auto& postEventIndices = primalSolution.postEventIndices_; - // divide the rollout segment in controller rollout and operating points - const auto controllerAvailableTill = - controllerPtr->empty() ? initTime_ : static_cast(controllerPtr)->timeStamp_.back(); - const auto controllerRolloutFromTo = std::make_pair(initTime_, std::max(initTime_, std::min(controllerAvailableTill, finalTime_))); - const auto operatingPointsFromTo = std::make_pair(controllerRolloutFromTo.second, finalTime_); - - // display - if (ddpSettings_.debugPrintRollout_) { - std::cerr << "[GaussNewtonDDP::rolloutInitialTrajectory] for t = [" << initTime_ << ", " << finalTime_ << "]\n"; - std::cerr << "\tcontroller available till t = " << controllerAvailableTill << "\n"; - if (controllerRolloutFromTo.first < controllerRolloutFromTo.second) { - std::cerr << "\twill use controller for t = [" << controllerRolloutFromTo.first << ", " << controllerRolloutFromTo.second << "]\n"; - } - if (operatingPointsFromTo.first < operatingPointsFromTo.second) { - std::cerr << "\twill use operating points for t = [" << operatingPointsFromTo.first << ", " << operatingPointsFromTo.second << "]\n"; - } - } - - // rollout with controller - vector_t xCurrent = initState_; - if (controllerRolloutFromTo.first < controllerRolloutFromTo.second) { - constexpr size_t workerIndex = 0; - xCurrent = dynamicsForwardRolloutPtrStock_[workerIndex]->run(controllerRolloutFromTo.first, initState_, controllerRolloutFromTo.second, - controllerPtr, modeSchedule, timeTrajectory, postEventIndices, - stateTrajectory, inputTrajectory); - } + // finish rollout with Initializer + if (timeTrajectory.empty() || timeTrajectory.back() < finalTime_) { + scalar_t initTime = timeTrajectory.empty() ? initTime_ : timeTrajectory.back(); + const vector_t initState = stateTrajectory.empty() ? initState_ : stateTrajectory.back(); - // finish rollout with operating points - if (operatingPointsFromTo.first < operatingPointsFromTo.second) { - // Remove last point of the controller rollout if it is directly past an event. Here where we want to use the initializer + // Remove last point of the rollout if it is directly past an event. Here where we want to use the Initializer // instead. However, we do start the integration at the state after the event. i.e. the jump map remains applied. if (!postEventIndices.empty() && postEventIndices.back() == (timeTrajectory.size() - 1)) { // Start new integration at the time point after the event @@ -454,38 +459,30 @@ void GaussNewtonDDP::rolloutInitialTrajectory(PrimalSolution& primalSolution) { stateTrajectory.pop_back(); inputTrajectory.pop_back(); // eventsPastTheEndIndeces is not removed because we need to mark the start of the operatingPointTrajectory as being after an event. + + // adjusting the start time to correct for subsystem recognition + constexpr auto eps = numeric_traits::weakEpsilon(); + initTime = std::min(initTime + eps, finalTime_); } scalar_array_t timeTrajectoryTail; - size_array_t eventsPastTheEndIndecesTail; + size_array_t postEventIndicesTail; vector_array_t stateTrajectoryTail; vector_array_t inputTrajectoryTail; - xCurrent = initializerRolloutPtr_->run(operatingPointsFromTo.first, xCurrent, operatingPointsFromTo.second, nullptr, modeSchedule, - timeTrajectoryTail, eventsPastTheEndIndecesTail, stateTrajectoryTail, inputTrajectoryTail); + std::ignore = initializerRolloutPtr_->run(initTime, initState, finalTime_, nullptr, modeSchedule, timeTrajectoryTail, + postEventIndicesTail, stateTrajectoryTail, inputTrajectoryTail); // Add controller rollout length to event past the indeces - for (auto& eventIndex : eventsPastTheEndIndecesTail) { - eventIndex += stateTrajectory.size(); // This size of this trajectory part was missing when counting events in the tail + for (auto& eventIndex : postEventIndicesTail) { + eventIndex += stateTrajectory.size(); // the size of the old trajectory is missing when counting event's index } - // Concatenate the operating points to the rollout + // Concatenate timeTrajectory.insert(timeTrajectory.end(), timeTrajectoryTail.begin(), timeTrajectoryTail.end()); - postEventIndices.insert(postEventIndices.end(), eventsPastTheEndIndecesTail.begin(), eventsPastTheEndIndecesTail.end()); + postEventIndices.insert(postEventIndices.end(), postEventIndicesTail.begin(), postEventIndicesTail.end()); stateTrajectory.insert(stateTrajectory.end(), stateTrajectoryTail.begin(), stateTrajectoryTail.end()); inputTrajectory.insert(inputTrajectory.end(), inputTrajectoryTail.begin(), inputTrajectoryTail.end()); } - - if (!xCurrent.allFinite()) { - throw std::runtime_error("[GaussNewtonDDP::rolloutInitialTrajectory] System became unstable during the initial rollout!"); - } - - // debug print - if (ddpSettings_.debugPrintRollout_) { - std::cerr << "\n++++++++++++++++++++++++++++++++++++++++\n"; - std::cerr << "[GaussNewtonDDP::rolloutInitialTrajectory] for t = [" << timeTrajectory.front() << ", " << timeTrajectory.back() << "]"; - std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++\n"; - RolloutBase::display(timeTrajectory, postEventIndices, stateTrajectory, &inputTrajectory); - } } /******************************************************************************************************/ @@ -534,8 +531,7 @@ scalar_t GaussNewtonDDP::solveSequentialRiccatiEquationsImpl(const ScalarFunctio riccatiEquationsWorker(0, partitionInterval, finalValueFunction); } else { // solve it in parallel // do equal-time partitions based on available thread resource - std::vector> partitionIntervals = - getPartitionIntervalsFromTimeTrajectory(nominalPrimalData_.primalSolution.timeTrajectory_, ddpSettings_.nThreads_); + const auto partitionIntervals = computePartitionIntervals(nominalPrimalData_.primalSolution.timeTrajectory_, ddpSettings_.nThreads_); // hold the final value function of each partition std::vector finalValueFunctionOfEachPartition(partitionIntervals.size()); @@ -559,7 +555,14 @@ scalar_t GaussNewtonDDP::solveSequentialRiccatiEquationsImpl(const ScalarFunctio if (ddpSettings_.checkNumericalStability_) { const int N = nominalPrimalData_.primalSolution.timeTrajectory_.size(); for (int k = N - 1; k >= 0; k--) { - const auto errorDescription = checkBeingPSD(nominalDualData_.valueFunctionTrajectory[k], "ValueFunction"); + // check size + auto errorDescription = checkSize(nominalPrimalData_.primalSolution.stateTrajectory_[k].size(), 0, + nominalDualData_.valueFunctionTrajectory[k], "ValueFunction"); + if (!errorDescription.empty()) { + throw std::runtime_error(errorDescription); + } + // check PSD + errorDescription = checkBeingPSD(nominalDualData_.valueFunctionTrajectory[k], "ValueFunction"); if (!errorDescription.empty()) { std::stringstream throwMsg; throwMsg << "at time " << nominalPrimalData_.primalSolution.timeTrajectory_[k] << ":\n"; @@ -631,17 +634,11 @@ void GaussNewtonDDP::calculateController() { } } } -} -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -scalar_t GaussNewtonDDP::maxControllerUpdateNorm(const LinearController& controller) const { - scalar_t maxDeltaUffNorm = 0.0; - for (const auto& deltaBias : controller.deltaBiasArray_) { - maxDeltaUffNorm = std::max(maxDeltaUffNorm, deltaBias.norm()); + // display + if (ddpSettings_.displayInfo_) { + std::cerr << "max feedforward norm: " << maxControllerUpdateNorm(unoptimizedController_) << "\n"; } - return maxDeltaUffNorm; } /******************************************************************************************************/ @@ -784,69 +781,6 @@ void GaussNewtonDDP::computeProjections(const matrix_t& Hm, const matrix_t& Dm, } } -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void GaussNewtonDDP::projectLQ(const ModelData& modelData, const matrix_t& constraintRangeProjector, - const matrix_t& constraintNullProjector, ModelData& projectedModelData) const { - // dimensions and time - projectedModelData.time = modelData.time; - projectedModelData.stateDim = modelData.stateDim; - projectedModelData.inputDim = modelData.inputDim - modelData.stateInputEqConstraint.f.rows(); - - // unhandled constraints - projectedModelData.stateEqConstraint.f = vector_t(); - - if (modelData.stateInputEqConstraint.f.rows() == 0) { - // Change of variables u = Pu * tilde{u} - // Pu = constraintNullProjector; - - // projected state-input equality constraints - projectedModelData.stateInputEqConstraint.f.setZero(projectedModelData.inputDim); - projectedModelData.stateInputEqConstraint.dfdx.setZero(projectedModelData.inputDim, projectedModelData.stateDim); - projectedModelData.stateInputEqConstraint.dfdu.setZero(modelData.inputDim, modelData.inputDim); - - // dynamics - projectedModelData.dynamics = modelData.dynamics; - changeOfInputVariables(projectedModelData.dynamics, constraintNullProjector); - - // dynamics bias - projectedModelData.dynamicsBias = modelData.dynamicsBias; - - // cost - projectedModelData.cost = modelData.cost; - changeOfInputVariables(projectedModelData.cost, constraintNullProjector); - - } else { - // Change of variables u = Pu * tilde{u} + Px * x + u0 - // Pu = constraintNullProjector; - // Px (= -CmProjected) = -constraintRangeProjector * C - // u0 (= -EvProjected) = -constraintRangeProjector * e - - /* projected state-input equality constraints */ - projectedModelData.stateInputEqConstraint.f.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.f; - projectedModelData.stateInputEqConstraint.dfdx.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.dfdx; - projectedModelData.stateInputEqConstraint.dfdu.noalias() = constraintRangeProjector * modelData.stateInputEqConstraint.dfdu; - - // Change of variable matrices - const auto& Pu = constraintNullProjector; - const matrix_t Px = -projectedModelData.stateInputEqConstraint.dfdx; - const matrix_t u0 = -projectedModelData.stateInputEqConstraint.f; - - // dynamics - projectedModelData.dynamics = modelData.dynamics; - changeOfInputVariables(projectedModelData.dynamics, Pu, Px, u0); - - // dynamics bias - projectedModelData.dynamicsBias = modelData.dynamicsBias; - projectedModelData.dynamicsBias.noalias() += modelData.dynamics.dfdu * u0; - - // cost - projectedModelData.cost = modelData.cost; - changeOfInputVariables(projectedModelData.cost, Pu, Px, u0); - } -} - /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -856,6 +790,15 @@ void GaussNewtonDDP::initializeConstraintPenalties() { constraintPenaltyCoefficients_.penaltyCoeff = ddpSettings_.constraintPenaltyInitialValue_; constraintPenaltyCoefficients_.penaltyTol = 1.0 / std::pow(constraintPenaltyCoefficients_.penaltyCoeff, 0.1); + + // display + if (ddpSettings_.displayInfo_) { + std::string displayText = "Initial equality Constraints Penalty Parameters:\n"; + displayText += " Penalty Tolerance: " + std::to_string(constraintPenaltyCoefficients_.penaltyTol); + displayText += " Penalty Coefficient: " + std::to_string(constraintPenaltyCoefficients_.penaltyCoeff) + ".\n"; + + this->printString(displayText); + } } /******************************************************************************************************/ @@ -886,171 +829,108 @@ void GaussNewtonDDP::updateConstraintPenalties(scalar_t equalityConstraintsSSE) /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::runInit() { - // disable Eigen multi-threading - Eigen::setNbThreads(1); - - initializationTimer_.startTimer(); - - // swap primal and dual data to cache - nominalDualData_.swap(cachedDualData_); - nominalPrimalData_.swap(cachedPrimalData_); - +bool GaussNewtonDDP::initializePrimalSolution() { try { - constexpr size_t taskId = 0; - constexpr scalar_t stepLength = 0.0; - - // clear before starting to fill nominalPrimalData_ + // clear before starting to fill nominalPrimalData_.clear(); - // for non-StateTriggeredRollout initialize modeSchedule - nominalPrimalData_.primalSolution.modeSchedule_ = this->getReferenceManager().getModeSchedule(); - // nominal controller is stored in the optimized primal data as it is either the result of previous solve or it is provided by user and - // copied to optimized data container manually at the beginning of runImpl - nominalPrimalData_.primalSolution.controllerPtr_.swap(optimizedPrimalSolution_.controllerPtr_); - // perform a rollout - rolloutInitialTrajectory(nominalPrimalData_.primalSolution); - - // adjust dual solution - totalDualSolutionTimer_.startTimer(); - if (!optimizedDualSolution_.timeTrajectory.empty()) { - const auto status = - trajectorySpread(optimizedPrimalSolution_.modeSchedule_, nominalPrimalData_.primalSolution.modeSchedule_, optimizedDualSolution_); - } - - // initialize dual solution - ocs2::initializeDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, optimizedDualSolution_, - nominalDualData_.dualSolution); - totalDualSolutionTimer_.endTimer(); + // for non-StateTriggeredRollout case, set modeSchedule + nominalPrimalData_.primalSolution.modeSchedule_ = getReferenceManager().getModeSchedule(); - computeRolloutMetrics(optimalControlProblemStock_[taskId], nominalPrimalData_.primalSolution, nominalDualData_.dualSolution, - nominalPrimalData_.problemMetrics); + // try to initialize with controller + bool initialSolutionExists = rolloutInitialController(optimizedPrimalSolution_, nominalPrimalData_.primalSolution); - // update dual - totalDualSolutionTimer_.startTimer(); - // ocs2::updateDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalPrimalData_.problemMetrics, - // nominalDualData_.dualSolution); - totalDualSolutionTimer_.endTimer(); + // if rolloutInitialController failed, try to initialize with PrimalSolution's state-input trajectories + if (!initialSolutionExists) { + // display + if (ddpSettings_.displayInfo_) { + std::cerr << "Initial controller is unavailable. Solver resorts to use PrimalSolution trajectories ...\n"; + } - // calculates rollout merit - performanceIndex_ = - computeRolloutPerformanceIndex(nominalPrimalData_.primalSolution.timeTrajectory_, nominalPrimalData_.problemMetrics); - performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); + initialSolutionExists = extractInitialTrajectories(optimizedPrimalSolution_, nominalPrimalData_.primalSolution); + } // display - if (ddpSettings_.displayInfo_) { - std::stringstream infoDisplay; - infoDisplay << " [Thread " << taskId << "] - step length " << stepLength << '\n'; - infoDisplay << std::setw(4) << performanceIndex_ << '\n'; - printString(infoDisplay.str()); + if (!initialSolutionExists && ddpSettings_.displayInfo_) { + std::cerr << "Initial PrimalSolution trajectories are unavailable. Solver resorts to use Initializer ...\n"; } + // finish rollout with Initializer + rolloutInitializer(nominalPrimalData_.primalSolution); + + // true if the rollout is not purely from the Initializer + return initialSolutionExists; + } catch (const std::exception& error) { - const std::string msg = "[GaussNewtonDDP::runInit] Initial controller does not generate a stable rollout.\n"; + const std::string msg = "[GaussNewtonDDP::initializePrimalSolution] the initial rollout is unstable!\n"; throw std::runtime_error(msg + error.what()); } - initializationTimer_.endTimer(); - - // initialize penalty coefficients - initializeConstraintPenalties(); +} - // linearizing the dynamics and quadratizing the cost function along nominal trajectories - linearQuadraticApproximationTimer_.startTimer(); - approximateOptimalControlProblem(); - linearQuadraticApproximationTimer_.endTimer(); +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void GaussNewtonDDP::initializeDualSolutionAndMetrics() { + // adjust dual solution + totalDualSolutionTimer_.startTimer(); + if (!optimizedDualSolution_.timeTrajectory.empty()) { + const auto status = + trajectorySpread(optimizedPrimalSolution_.modeSchedule_, nominalPrimalData_.primalSolution.modeSchedule_, optimizedDualSolution_); + } - // solve Riccati equations - backwardPassTimer_.startTimer(); - avgTimeStepBP_ = solveSequentialRiccatiEquations(nominalPrimalData_.modelDataFinalTime.cost); - backwardPassTimer_.endTimer(); + // initialize dual solution + ocs2::initializeDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, optimizedDualSolution_, + nominalDualData_.dualSolution); + totalDualSolutionTimer_.endTimer(); - // calculate controller - computeControllerTimer_.startTimer(); - // calculate controller. Result is stored in an intermediate variable. The optimized controller, the one after searching, will be - // swapped back to corresponding primalDataContainer in the search stage - calculateController(); - computeControllerTimer_.endTimer(); + computeRolloutMetrics(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalDualData_.dualSolution, + nominalPrimalData_.problemMetrics); - // display - if (ddpSettings_.displayInfo_) { - printRolloutInfo(); - } + // update dual + // totalDualSolutionTimer_.startTimer(); + // ocs2::updateDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalPrimalData_.problemMetrics, + // nominalDualData_.dualSolution); + // totalDualSolutionTimer_.endTimer(); - // TODO(mspieler): this is not exception safe - // restore default Eigen thread number - Eigen::setNbThreads(0); + // calculates rollout merit + performanceIndex_ = computeRolloutPerformanceIndex(nominalPrimalData_.primalSolution.timeTrajectory_, nominalPrimalData_.problemMetrics); + performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void GaussNewtonDDP::runIteration(scalar_t lqModelExpectedCost) { - // disable Eigen multi-threading - Eigen::setNbThreads(1); - - // finding the optimal stepLength +void GaussNewtonDDP::takePrimalDualStep(scalar_t lqModelExpectedCost) { + // update primal: run search strategy and find the optimal stepLength searchStrategyTimer_.startTimer(); - - // swap primal and dual data to cache before running search strategy - nominalDualData_.swap(cachedDualData_); - nominalPrimalData_.swap(cachedPrimalData_); - - // run search strategy scalar_t avgTimeStep; const auto& modeSchedule = this->getReferenceManager().getModeSchedule(); - search_strategy::SolutionRef solution(avgTimeStep, nominalDualData_.dualSolution, nominalPrimalData_.primalSolution, - nominalPrimalData_.problemMetrics, performanceIndex_); + search_strategy::SolutionRef solution(avgTimeStep, optimizedDualSolution_, optimizedPrimalSolution_, optimizedProblemMetrics_, + performanceIndex_); const bool success = searchStrategyPtr_->run({initTime_, finalTime_}, initState_, lqModelExpectedCost, unoptimizedController_, - cachedDualData_.dualSolution, modeSchedule, solution); + nominalDualData_.dualSolution, modeSchedule, solution); - // revert to the old solution if search failed if (success) { avgTimeStepFP_ = 0.9 * avgTimeStepFP_ + 0.1 * avgTimeStep; - - } else { // If fail, copy the entire cache back. To keep the consistency of cached data, all cache should be left untouched. - nominalDualData_ = cachedDualData_; - nominalPrimalData_ = cachedPrimalData_; - performanceIndex_ = performanceIndexHistory_.back(); } - searchStrategyTimer_.endTimer(); // update dual totalDualSolutionTimer_.startTimer(); - ocs2::updateDualSolution(optimalControlProblemStock_[0], nominalPrimalData_.primalSolution, nominalPrimalData_.problemMetrics, - nominalDualData_.dualSolution); - performanceIndex_ = computeRolloutPerformanceIndex(nominalPrimalData_.primalSolution.timeTrajectory_, nominalPrimalData_.problemMetrics); - performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); + if (success) { + ocs2::updateDualSolution(optimalControlProblemStock_[0], optimizedPrimalSolution_, optimizedProblemMetrics_, optimizedDualSolution_); + performanceIndex_ = computeRolloutPerformanceIndex(optimizedPrimalSolution_.timeTrajectory_, optimizedProblemMetrics_); + performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); + } totalDualSolutionTimer_.endTimer(); - // update the constraint penalty coefficients - updateConstraintPenalties(performanceIndex_.equalityConstraintsSSE); - - // linearizing the dynamics and quadratizing the cost function along nominal trajectories - linearQuadraticApproximationTimer_.startTimer(); - approximateOptimalControlProblem(); - linearQuadraticApproximationTimer_.endTimer(); - - // solve Riccati equations - backwardPassTimer_.startTimer(); - avgTimeStepBP_ = solveSequentialRiccatiEquations(nominalPrimalData_.modelDataFinalTime.cost); - backwardPassTimer_.endTimer(); - - // calculate controller - computeControllerTimer_.startTimer(); - // calculate controller. Result is stored in an intermediate variable. The optimized controller, the one after searching, will be - // swapped back to corresponding primalDataContainer in the search stage - calculateController(); - computeControllerTimer_.endTimer(); - - // display - if (ddpSettings_.displayInfo_) { - printRolloutInfo(); + // if failed, use nominal and to keep the consistency of cached data, all cache should be left untouched + if (!success) { + optimizedDualSolution_ = nominalDualData_.dualSolution; + optimizedPrimalSolution_ = nominalPrimalData_.primalSolution; + optimizedProblemMetrics_ = nominalPrimalData_.problemMetrics; + performanceIndex_ = performanceIndexHistory_.back(); } - - // TODO(mspieler): this is not exception safe - // restore default Eigen thread number - Eigen::setNbThreads(0); } /******************************************************************************************************/ @@ -1067,12 +947,33 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala if (linearControllerPtr == nullptr) { throw std::runtime_error("[GaussNewtonDDP::run] controller must be a LinearController type!"); } - *optimizedPrimalSolution_.controllerPtr_ = *linearControllerPtr; + optimizedPrimalSolution_.controllerPtr_.reset(linearControllerPtr->clone()); } runImpl(initTime, initState, finalTime); } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) { + // if PrimalSolution's controller exists, use it for initialization + if (primalSolution.controllerPtr_ != nullptr && !primalSolution.controllerPtr_->empty()) { + optimizedPrimalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + runImpl(initTime, initState, finalTime, primalSolution.controllerPtr_.get()); + + } else { + // otherwise initialize with PrimalSolution's state-input trajectories + optimizedPrimalSolution_.controllerPtr_->clear(); + optimizedPrimalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + optimizedPrimalSolution_.timeTrajectory_ = primalSolution.timeTrajectory_; + optimizedPrimalSolution_.postEventIndices_ = primalSolution.postEventIndices_; + optimizedPrimalSolution_.stateTrajectory_ = primalSolution.stateTrajectory_; + optimizedPrimalSolution_.inputTrajectory_ = primalSolution.inputTrajectory_; + runImpl(initTime, initState, finalTime); + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ @@ -1082,120 +983,107 @@ void GaussNewtonDDP::runImpl(scalar_t initTime, const vector_t& initState, scala std::cerr << "\n+++++++++++++ " + ddp::toAlgorithmName(ddpSettings_.algorithm_) + " solver is initialized ++++++++++++++"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; std::cerr << "\nSolver starts from initial time " << initTime << " to final time " << finalTime << ".\n"; - std::cerr << this->getReferenceManager().getModeSchedule() << "\n"; + std::cerr << getReferenceManager().getModeSchedule(); } - initState_ = initState; + // set cost desired trajectories + for (auto& ocp : optimalControlProblemStock_) { + ocp.targetTrajectoriesPtr = &this->getReferenceManager().getTargetTrajectories(); + } + + // initialize parameters initTime_ = initTime; + initState_ = initState; finalTime_ = finalTime; performanceIndexHistory_.clear(); const auto initIteration = totalNumIterations_; - - // adjust controller - if (!optimizedPrimalSolution_.controllerPtr_->empty()) { - std::ignore = trajectorySpread(optimizedPrimalSolution_.modeSchedule_, getReferenceManager().getModeSchedule(), - getLinearController(optimizedPrimalSolution_)); - } - - // check if after the truncation the internal controller is empty - bool unreliableControllerIncrement = optimizedPrimalSolution_.controllerPtr_->empty(); - - // set cost desired trajectories - for (size_t i = 0; i < ddpSettings_.nThreads_; i++) { - const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); - optimalControlProblemStock_[i].targetTrajectoriesPtr = &targetTrajectories; - } + initializeConstraintPenalties(); // initialize penalty coefficients // display if (ddpSettings_.displayInfo_) { std::cerr << "\n###################"; - std::cerr << "\n#### Iteration " << (totalNumIterations_ - initIteration) << " (Dynamics might have been violated)"; + std::cerr << "\n#### Initial Rollout"; std::cerr << "\n###################\n"; } - // run DDP initializer and update the member variables - runInit(); + // swap primal and dual data to cache + nominalDualData_.swap(cachedDualData_); + nominalPrimalData_.swap(cachedPrimalData_); + + // optimized --> nominal: initializes the nominal primal and dual solutions based on the optimized ones + initializationTimer_.startTimer(); + bool initialSolutionExists = initializePrimalSolution(); // true if the rollout is not purely from the Initializer + initializeDualSolutionAndMetrics(); + performanceIndexHistory_.push_back(performanceIndex_); + initializationTimer_.endTimer(); - // increment iteration counter - totalNumIterations_++; + // display + if (ddpSettings_.displayInfo_) { + std::cerr << performanceIndex_ << '\n'; + } // convergence variables of the main loop bool isConverged = false; std::string convergenceInfo; // DDP main loop - while (!isConverged && (totalNumIterations_ - initIteration) < ddpSettings_.maxNumIterations_) { - // display the iteration's input update norm (before caching the old nominals) + while (true) { if (ddpSettings_.displayInfo_) { std::cerr << "\n###################"; std::cerr << "\n#### Iteration " << (totalNumIterations_ - initIteration); std::cerr << "\n###################\n"; - std::cerr << "max feedforward norm: " << maxControllerUpdateNorm(unoptimizedController_) << "\n"; } - performanceIndexHistory_.push_back(performanceIndex_); - - // run the an iteration of the DDP algorithm and update the member variables - // the controller which is designed solely based on operation trajectories possibly has invalid feedforward. - // Therefore the expected cost/merit (calculated by the Riccati solution) is not reliable as well. - const scalar_t lqModelExpectedCost = - unreliableControllerIncrement ? performanceIndex_.merit : nominalDualData_.valueFunctionTrajectory.front().f; - - // run a DDP iteration and update the member variables - runIteration(lqModelExpectedCost); - - // increment iteration counter - totalNumIterations_++; - - // check convergence - std::tie(isConverged, convergenceInfo) = - searchStrategyPtr_->checkConvergence(unreliableControllerIncrement, performanceIndexHistory_.back(), performanceIndex_); - unreliableControllerIncrement = false; - } // end of while loop + // nominal --> nominal: constructs the LQ problem around the nominal trajectories + linearQuadraticApproximationTimer_.startTimer(); + approximateOptimalControlProblem(); + linearQuadraticApproximationTimer_.endTimer(); - // display the final iteration's input update norm (before caching the old nominals) - if (ddpSettings_.displayInfo_) { - std::cerr << "\n###################"; - std::cerr << "\n#### Final Rollout"; - std::cerr << "\n###################\n"; - std::cerr << "max feedforward norm: " << maxControllerUpdateNorm(unoptimizedController_) << "\n"; - } + // nominal --> nominal: solves the LQ problem + backwardPassTimer_.startTimer(); + avgTimeStepBP_ = solveSequentialRiccatiEquations(nominalPrimalData_.modelDataFinalTime.cost); + backwardPassTimer_.endTimer(); - performanceIndexHistory_.push_back(performanceIndex_); + // calculate controller and store the result in unoptimizedController_ + computeControllerTimer_.startTimer(); + calculateController(); + computeControllerTimer_.endTimer(); - // finding the final optimal stepLength and getting the optimal trajectories and controller - searchStrategyTimer_.startTimer(); + // the expected cost/merit calculated by the Riccati solution is not reliable + const auto lqModelExpectedCost = initialSolutionExists ? nominalDualData_.valueFunctionTrajectory.front().f : performanceIndex_.merit; - // run search strategy - scalar_t avgTimeStep; - const auto& modeSchedule = this->getReferenceManager().getModeSchedule(); - const auto lqModelExpectedCost = nominalDualData_.valueFunctionTrajectory.front().f; - search_strategy::SolutionRef solution(avgTimeStep, optimizedDualSolution_, optimizedPrimalSolution_, optimizedProblemMetrics_, - performanceIndex_); - const bool success = searchStrategyPtr_->run({initTime_, finalTime_}, initState_, lqModelExpectedCost, unoptimizedController_, - nominalDualData_.dualSolution, modeSchedule, solution); + // nominal --> optimized: based on the current LQ solution updates the optimized primal and dual solutions + takePrimalDualStep(lqModelExpectedCost); - // revert to the old solution if search failed - if (success) { - avgTimeStepFP_ = 0.9 * avgTimeStepFP_ + 0.1 * avgTimeStep; + // iteration info + ++totalNumIterations_; + performanceIndexHistory_.push_back(performanceIndex_); - } else { // If fail, copy the entire cache back. To keep the consistency of cached data, all cache should be left untouched. - optimizedDualSolution_ = nominalDualData_.dualSolution; - optimizedPrimalSolution_ = nominalPrimalData_.primalSolution; - optimizedProblemMetrics_ = nominalPrimalData_.problemMetrics; - performanceIndex_ = performanceIndexHistory_.back(); - } + // display + if (ddpSettings_.displayInfo_) { + printRolloutInfo(); + } - searchStrategyTimer_.endTimer(); + // check convergence + std::tie(isConverged, convergenceInfo) = searchStrategyPtr_->checkConvergence( + !initialSolutionExists, *std::prev(performanceIndexHistory_.end(), 2), performanceIndexHistory_.back()); + initialSolutionExists = true; - // update dual - totalDualSolutionTimer_.startTimer(); - ocs2::updateDualSolution(optimalControlProblemStock_[0], optimizedPrimalSolution_, optimizedProblemMetrics_, optimizedDualSolution_); - performanceIndex_ = computeRolloutPerformanceIndex(optimizedPrimalSolution_.timeTrajectory_, optimizedProblemMetrics_); - performanceIndex_.merit = calculateRolloutMerit(performanceIndex_); - totalDualSolutionTimer_.endTimer(); + if (isConverged || (totalNumIterations_ - initIteration) == ddpSettings_.maxNumIterations_) { + break; - performanceIndexHistory_.push_back(performanceIndex_); + } else { + // update the constraint penalty coefficients + updateConstraintPenalties(performanceIndex_.equalityConstraintsSSE); + + // optimized --> nominal: use the optimized solution as the nominal for the next iteration + nominalDualData_.swap(cachedDualData_); + nominalPrimalData_.swap(cachedPrimalData_); + optimizedDualSolution_.swap(nominalDualData_.dualSolution); + optimizedPrimalSolution_.swap(nominalPrimalData_.primalSolution); + optimizedProblemMetrics_.swap(nominalPrimalData_.problemMetrics); + } + } // end of while loop // display if (ddpSettings_.displayInfo_ || ddpSettings_.displayShortSummary_) { diff --git a/ocs2_ddp/src/HessianCorrection.cpp b/ocs2_ddp/src/HessianCorrection.cpp index 7b2dc331c..3a4f0424d 100644 --- a/ocs2_ddp/src/HessianCorrection.cpp +++ b/ocs2_ddp/src/HessianCorrection.cpp @@ -50,5 +50,27 @@ Strategy fromString(const std::string& name) { return strategyMap.at(name); } +void shiftHessian(Strategy strategy, matrix_t& matrix, scalar_t minEigenvalue) { + assert(matrix.rows() == matrix.cols()); + switch (strategy) { + case Strategy::DIAGONAL_SHIFT: { + matrix.diagonal().array() += minEigenvalue; + break; + } + case Strategy::CHOLESKY_MODIFICATION: { + LinearAlgebra::makePsdCholesky(matrix, minEigenvalue); + break; + } + case Strategy::EIGENVALUE_MODIFICATION: { + LinearAlgebra::makePsdEigenvalue(matrix, minEigenvalue); + break; + } + case Strategy::GERSHGORIN_MODIFICATION: { + LinearAlgebra::makePsdGershgorin(matrix, minEigenvalue); + break; + } + } +} + } // namespace hessian_correction } // namespace ocs2 diff --git a/ocs2_ddp/src/SLQ.cpp b/ocs2_ddp/src/SLQ.cpp index f2670e1e4..4023dfc44 100644 --- a/ocs2_ddp/src/SLQ.cpp +++ b/ocs2_ddp/src/SLQ.cpp @@ -239,7 +239,6 @@ void SLQ::riccatiEquationsWorker(size_t workerIndex, const std::pair& * SsNormalized = [-10.0, ..., -2.0, -1.0, -0.0] */ vector_array_t& allSsTrajectory = allSsTrajectoryStock_[workerIndex]; - allSsTrajectory.clear(); integrateRiccatiEquationNominalTime(*riccatiIntegratorPtrStock_[workerIndex], *riccatiEquationsPtrStock_[workerIndex], partitionInterval, nominalTimeTrajectory, nominalEventsPastTheEndIndices, std::move(allSsFinal), SsNormalizedTime, SsNormalizedPostEventIndices, allSsTrajectory); @@ -266,7 +265,6 @@ void SLQ::integrateRiccatiEquationNominalTime(IntegratorBase& riccatiIntegrator, const int nominalTimeSize = SsNormalizedTime.size(); const int numEvents = SsNormalizedPostEventIndices.size(); auto partitionDuration = nominalTimeTrajectory[partitionInterval.second] - nominalTimeTrajectory[partitionInterval.first]; - const auto numTimeSteps = static_cast(settings().maxNumStepsPerSecond_ * std::max(1.0, partitionDuration)); // Normalized switching time indices, start and end of the partition are added at the beginning and end using iterator_t = scalar_array_t::const_iterator; @@ -281,15 +279,16 @@ void SLQ::integrateRiccatiEquationNominalTime(IntegratorBase& riccatiIntegrator, // integrating the Riccati equations allSsTrajectory.clear(); - allSsTrajectory.reserve(numTimeSteps); + allSsTrajectory.reserve(nominalTimeSize); for (int i = 0; i <= numEvents; i++) { iterator_t beginTimeItr = SsNormalizedSwitchingTimesIndices[i].first; iterator_t endTimeItr = SsNormalizedSwitchingTimesIndices[i].second; - Observer observer(&allSsTrajectory); // solve Riccati equations + Observer observer(&allSsTrajectory); + const auto maxNumTimeSteps = static_cast(settings().maxNumStepsPerSecond_ * std::max(1.0, partitionDuration)); riccatiIntegrator.integrateTimes(riccatiEquation, observer, allSsFinal, beginTimeItr, endTimeItr, settings().timeStep_, - settings().absTolODE_, settings().relTolODE_, numTimeSteps); + settings().absTolODE_, settings().relTolODE_, maxNumTimeSteps); if (i < numEvents) { allSsFinal = riccatiEquation.computeJumpMap(*endTimeItr, allSsTrajectory.back()); diff --git a/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp b/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp index 0e79a1b14..4f2122eb9 100644 --- a/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp +++ b/ocs2_ddp/src/search_strategy/LevenbergMarquardtStrategy.cpp @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/search_strategy/LevenbergMarquardtStrategy.h" +#include + #include "ocs2_ddp/DDP_HelperFunctions.h" #include "ocs2_ddp/HessianCorrection.h" @@ -53,7 +55,7 @@ LevenbergMarquardtStrategy::LevenbergMarquardtStrategy(search_strategy::Settings /******************************************************************************************************/ /******************************************************************************************************/ void LevenbergMarquardtStrategy::reset() { - levenbergMarquardtModule_ = LevenbergMarquardtModule(); + lmModule_ = LevenbergMarquardtModule(); } /******************************************************************************************************/ @@ -119,14 +121,7 @@ bool LevenbergMarquardtStrategy::run(const std::pair& timePe // compute pho (the ratio between actual reduction and predicted reduction) const auto actualReduction = prevMerit - solution.performanceIndex.merit; - - if (std::abs(actualReduction) < baseSettings_.minRelCost || expectedReduction <= baseSettings_.minRelCost) { - levenbergMarquardtModule_.pho = 1.0; - } else if (actualReduction < 0.0) { - levenbergMarquardtModule_.pho = 0.0; - } else { - levenbergMarquardtModule_.pho = actualReduction / expectedReduction; - } + const auto pho = reductionToPredictedReduction(actualReduction, expectedReduction); // display if (baseSettings_.displayInfo) { @@ -134,73 +129,63 @@ bool LevenbergMarquardtStrategy::run(const std::pair& timePe } // adjust riccatiMultipleAdaptiveRatio and riccatiMultiple - if (levenbergMarquardtModule_.pho < 0.25) { + if (pho < 0.25) { // increase riccatiMultipleAdaptiveRatio - levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio = - std::max(1.0, levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio) * settings_.riccatiMultipleDefaultRatio; + lmModule_.riccatiMultipleAdaptiveRatio = std::max(1.0, lmModule_.riccatiMultipleAdaptiveRatio) * settings_.riccatiMultipleDefaultRatio; // increase riccatiMultiple - auto riccatiMultipleTemp = levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio * levenbergMarquardtModule_.riccatiMultiple; - if (riccatiMultipleTemp > settings_.riccatiMultipleDefaultFactor) { - levenbergMarquardtModule_.riccatiMultiple = riccatiMultipleTemp; - } else { - levenbergMarquardtModule_.riccatiMultiple = settings_.riccatiMultipleDefaultFactor; - } + const auto riccatiMultipleTemp = lmModule_.riccatiMultipleAdaptiveRatio * lmModule_.riccatiMultiple; + lmModule_.riccatiMultiple = std::max(riccatiMultipleTemp, settings_.riccatiMultipleDefaultFactor); - } else if (levenbergMarquardtModule_.pho > 0.75) { + } else if (pho > 0.75) { // decrease riccatiMultipleAdaptiveRatio - levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio = - std::min(1.0, levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio) / settings_.riccatiMultipleDefaultRatio; + lmModule_.riccatiMultipleAdaptiveRatio = std::min(1.0, lmModule_.riccatiMultipleAdaptiveRatio) / settings_.riccatiMultipleDefaultRatio; // decrease riccatiMultiple - auto riccatiMultipleTemp = levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio * levenbergMarquardtModule_.riccatiMultiple; - if (riccatiMultipleTemp > settings_.riccatiMultipleDefaultFactor) { - levenbergMarquardtModule_.riccatiMultiple = riccatiMultipleTemp; - } else { - levenbergMarquardtModule_.riccatiMultiple = 0.0; - } + const auto riccatiMultipleTemp = lmModule_.riccatiMultipleAdaptiveRatio * lmModule_.riccatiMultiple; + lmModule_.riccatiMultiple = (riccatiMultipleTemp > settings_.riccatiMultipleDefaultFactor) ? riccatiMultipleTemp : 0.0; + } else { - levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio = 1.0; - // levenbergMarquardtModule_.riccatiMultiple will not change. + lmModule_.riccatiMultipleAdaptiveRatio = 1.0; + // lmModule_.riccatiMultiple will not change. } // display if (baseSettings_.displayInfo) { std::stringstream displayInfo; - if (levenbergMarquardtModule_.numSuccessiveRejections == 0) { - displayInfo << "The step is accepted with pho: " << levenbergMarquardtModule_.pho << ". "; + if (lmModule_.numSuccessiveRejections == 0) { + displayInfo << "The step is accepted with pho: " << pho << ". "; } else { - displayInfo << "The step is rejected with pho: " << levenbergMarquardtModule_.pho << " (" - << levenbergMarquardtModule_.numSuccessiveRejections << " out of " << settings_.maxNumSuccessiveRejections << "). "; + displayInfo << "The step is rejected with pho: " << pho << " (" << lmModule_.numSuccessiveRejections << " out of " + << settings_.maxNumSuccessiveRejections << "). "; } - if (numerics::almost_eq(levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio, 1.0)) { + if (numerics::almost_eq(lmModule_.riccatiMultipleAdaptiveRatio, 1.0)) { displayInfo << "The Riccati multiple is kept constant: "; - } else if (levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio < 1.0) { + } else if (lmModule_.riccatiMultipleAdaptiveRatio < 1.0) { displayInfo << "The Riccati multiple is decreased to: "; } else { displayInfo << "The Riccati multiple is increased to: "; } - displayInfo << levenbergMarquardtModule_.riccatiMultiple << ", with ratio: " << levenbergMarquardtModule_.riccatiMultipleAdaptiveRatio - << ".\n"; + displayInfo << lmModule_.riccatiMultiple << ", with ratio: " << lmModule_.riccatiMultipleAdaptiveRatio << ".\n"; std::cerr << displayInfo.str(); } // max accepted number of successive rejections - if (levenbergMarquardtModule_.numSuccessiveRejections > settings_.maxNumSuccessiveRejections) { + if (lmModule_.numSuccessiveRejections > settings_.maxNumSuccessiveRejections) { throw std::runtime_error("The maximum number of successive solution rejections has been reached!"); } // accept or reject the step and modify numSuccessiveRejections - if (levenbergMarquardtModule_.pho >= settings_.minAcceptedPho) { + if (pho >= settings_.minAcceptedPho) { // accept the solution - levenbergMarquardtModule_.numSuccessiveRejections = 0; + lmModule_.numSuccessiveRejections = 0; return true; } else { // reject the solution - ++levenbergMarquardtModule_.numSuccessiveRejections; + ++lmModule_.numSuccessiveRejections; return false; } } @@ -218,7 +203,7 @@ std::pair LevenbergMarquardtStrategy::checkConvergence(bool u const scalar_t previousTotalCost = previousPerformanceIndex.cost + previousPerformanceIndex.equalityLagrangian + previousPerformanceIndex.inequalityLagrangian; const scalar_t relCost = std::abs(currentTotalCost - previousTotalCost); - if (levenbergMarquardtModule_.numSuccessiveRejections == 0 && !unreliableControllerIncrement) { + if (lmModule_.numSuccessiveRejections == 0 && !unreliableControllerIncrement) { isCostFunctionConverged = relCost <= baseSettings_.minRelCost; } const bool isConstraintsSatisfied = currentPerformanceIndex.equalityConstraintsSSE <= baseSettings_.constraintTolerance; @@ -250,8 +235,8 @@ void LevenbergMarquardtStrategy::computeRiccatiModification(const ModelData& pro // deltaQm, deltaRm, deltaPm deltaQm.setZero(projectedModelData.stateDim, projectedModelData.stateDim); - deltaGv.noalias() = levenbergMarquardtModule_.riccatiMultiple * BmProjected.transpose() * HvProjected; - deltaGm.noalias() = levenbergMarquardtModule_.riccatiMultiple * BmProjected.transpose() * AmProjected; + deltaGv.noalias() = lmModule_.riccatiMultiple * BmProjected.transpose() * HvProjected; + deltaGm.noalias() = lmModule_.riccatiMultiple * BmProjected.transpose() * AmProjected; } /******************************************************************************************************/ @@ -259,7 +244,7 @@ void LevenbergMarquardtStrategy::computeRiccatiModification(const ModelData& pro /******************************************************************************************************/ matrix_t LevenbergMarquardtStrategy::augmentHamiltonianHessian(const ModelData& modelData, const matrix_t& Hm) const { matrix_t HmAug = Hm; - HmAug.noalias() += levenbergMarquardtModule_.riccatiMultiple * modelData.dynamics.dfdu.transpose() * modelData.dynamics.dfdu; + HmAug.noalias() += lmModule_.riccatiMultiple * modelData.dynamics.dfdu.transpose() * modelData.dynamics.dfdu; return HmAug; } diff --git a/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp b/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp index 4d218eb61..4a6e3b7e1 100644 --- a/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp +++ b/ocs2_ddp/src/search_strategy/LineSearchStrategy.cpp @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/search_strategy/LineSearchStrategy.h" +#include + #include "ocs2_ddp/DDP_HelperFunctions.h" #include "ocs2_ddp/HessianCorrection.h" @@ -230,27 +232,15 @@ void LineSearchStrategy::lineSearchTask(const size_t taskId) { * cost should be better than the baseline cost but learning rate should * be as high as possible. This is equivalent to a single core line search. */ - const bool progressCondition = workersSolution_[taskId].performanceIndex.merit < (baselineMerit_ * (1.0 - 1e-3 * stepLength)); const bool armijoCondition = workersSolution_[taskId].performanceIndex.merit < (baselineMerit_ - settings_.armijoCoefficient * stepLength * unoptimizedControllerUpdateIS_); - - if (armijoCondition && stepLength > bestStepSize_) { + if (armijoCondition && stepLength > bestStepSize_) { // save solution bestStepSize_ = stepLength; swap(*bestSolutionRef_, workersSolution_[taskId]); - - // whether to stop all other thread. - terminateLinesearchTasks = true; - for (size_t i = 0; i < alphaExp; i++) { - if (!alphaProcessed_[i]) { - terminateLinesearchTasks = false; - break; - } - } // end of i loop - - } // end of if + terminateLinesearchTasks = std::all_of(alphaProcessed_.cbegin(), alphaProcessed_.cbegin() + alphaExp, [](bool f) { return f; }); + } alphaProcessed_[alphaExp] = true; - } // end lock // kill other ongoing line search tasks diff --git a/ocs2_ddp/test/CircularKinematicsTest.cpp b/ocs2_ddp/test/CircularKinematicsTest.cpp index df37d25d7..f51ebf070 100644 --- a/ocs2_ddp/test/CircularKinematicsTest.cpp +++ b/ocs2_ddp/test/CircularKinematicsTest.cpp @@ -47,14 +47,14 @@ class CircularKinematicsTest : public testing::TestWithParam #include -#include #include +#include #include #include @@ -70,6 +70,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. TEST(HybridSlqTest, state_rollout_slq) { using namespace ocs2; + constexpr scalar_t minRelCost = 1e-6; // to avoid early termination + const ddp::Settings ddpSettings = [&]() { ddp::Settings settings; @@ -78,7 +80,7 @@ TEST(HybridSlqTest, state_rollout_slq) { settings.displayShortSummary_ = true; settings.maxNumIterations_ = 100; settings.nThreads_ = 1; - settings.minRelCost_ = 1e-6; // to avoid early termination + settings.minRelCost_ = minRelCost; settings.checkNumericalStability_ = false; settings.absTolODE_ = 1e-10; settings.relTolODE_ = 1e-7; @@ -111,19 +113,16 @@ TEST(HybridSlqTest, state_rollout_slq) { // cost function const matrix_t Q = (matrix_t(STATE_DIM, STATE_DIM) << 50, 0, 0, 0, 50, 0, 0, 0, 0).finished(); const matrix_t R = (matrix_t(INPUT_DIM, INPUT_DIM) << 1).finished(); - std::unique_ptr cost(new QuadraticStateInputCost(Q, R)); - std::unique_ptr preJumpCost(new QuadraticStateCost(Q)); - std::unique_ptr finalCost(new QuadraticStateCost(Q)); - // constraints - std::unique_ptr boundsConstraints(new HybridSysBounds); + auto boundsConstraints = std::make_unique(); OptimalControlProblem problem; problem.dynamicsPtr.reset(systemDynamics.clone()); - problem.costPtr->add("cost", std::move(cost)); - problem.preJumpCostPtr->add("preJumpCost", std::move(preJumpCost)); - problem.finalCostPtr->add("finalCost", std::move(finalCost)); - problem.inequalityLagrangianPtr->add("bounds", create(std::move(boundsConstraints), augmented::SlacknessSquaredHingePenalty::create({200.0, 0.1}))); + problem.costPtr->add("cost", std::make_unique(Q, R)); + problem.preJumpCostPtr->add("preJumpCost", std::make_unique(Q)); + problem.finalCostPtr->add("finalCost", std::make_unique(Q)); + problem.inequalityLagrangianPtr->add("bounds", + create(std::move(boundsConstraints), augmented::SlacknessSquaredHingePenalty::create({200.0, 0.1}))); const vector_t xNominal = vector_t::Zero(STATE_DIM); const vector_t uNominal = vector_t::Zero(INPUT_DIM); @@ -136,22 +135,23 @@ TEST(HybridSlqTest, state_rollout_slq) { OperatingPoints operatingTrajectories(stateOperatingPoint, inputOperatingPoint); // Test 1: Check constraint compliance. It uses a solver observer to get metrics for the bounds constraints - std::unique_ptr boundsConstraintsObserverPtr(new AugmentedLagrangianObserver("bounds")); - boundsConstraintsObserverPtr->setMetricsCallback([&](const scalar_array_t& timeTraj, const std::vector& metricsTraj) { - constexpr scalar_t constraintViolationTolerance = 1e-1; - for (size_t i = 0; i < metricsTraj.size(); i++) { - const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); - EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(1), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(2), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - EXPECT_NEAR(constraintViolation(3), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; - } - }); + auto boundsConstraintsObserverPtr = SolverObserver::LagrangianTermObserver( + SolverObserver::Type::Intermediate, "bounds", + [&](const scalar_array_t& timeTraj, const std::vector& metricsTraj) { + constexpr scalar_t constraintViolationTolerance = 1e-1; + for (size_t i = 0; i < metricsTraj.size(); i++) { + const vector_t constraintViolation = metricsTraj[i].constraint.cwiseMin(0.0); + EXPECT_NEAR(constraintViolation(0), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(1), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(2), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + EXPECT_NEAR(constraintViolation(3), 0.0, constraintViolationTolerance) << "At time " << timeTraj[i] << "\n"; + } + }); // setup SLQ SLQ slq(ddpSettings, stateTriggeredRollout, problem, operatingTrajectories); slq.setReferenceManager(referenceManager); - slq.addAugmentedLagrangianObserver(std::move(boundsConstraintsObserverPtr)); + slq.addSolverObserver(std::move(boundsConstraintsObserverPtr)); // run SLQ slq.run(startTime, initState, finalTime); @@ -166,5 +166,5 @@ TEST(HybridSlqTest, state_rollout_slq) { // Test 3: Check of cost const auto performanceIndecesST = slq.getPerformanceIndeces(); - EXPECT_LT(performanceIndecesST.cost - 20.1, 10.0 * ddpSettings.minRelCost_); + EXPECT_LT(performanceIndecesST.cost - 20.1, 10.0 * minRelCost); } diff --git a/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp b/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp index bc3020220..3adb041ef 100644 --- a/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp +++ b/ocs2_ddp/test/bouncingmass/BouncingMassTest.cpp @@ -40,7 +40,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/test/bouncingmass/OverallReference.h" #include "ocs2_ddp/test/bouncingmass/SystemModel.h" -/* Test for StateTriggeredRollout in combination with SLQ +/* + * Test for State-Triggered hybrid SLQ * * The system being tested is a system consisting of a mass and a wall * the mass bounces against the wall. The control objective is to follow a predefined @@ -65,147 +66,133 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * (1) No penetration of Guard Surfaces * (2) Check of cost function compared against cost calculated during trusted run of SLQ */ -TEST(BouncingMassTest, DISABLED_state_rollout_slq) { - using scalar_t = ocs2::scalar_t; - using vector_t = ocs2::vector_t; - using matrix_t = ocs2::matrix_t; - using scalar_array_t = ocs2::scalar_array_t; - using vector_array_t = ocs2::vector_array_t; - using matrix_array_t = ocs2::matrix_array_t; - - ocs2::ddp::Settings ddpSettings; - ddpSettings.algorithm_ = ocs2::ddp::Algorithm::SLQ; - ddpSettings.displayInfo_ = false; - ddpSettings.displayShortSummary_ = true; - ddpSettings.maxNumIterations_ = 30; - ddpSettings.minRelCost_ = 1e-4; - ddpSettings.nThreads_ = 1; - ddpSettings.checkNumericalStability_ = true; - ddpSettings.absTolODE_ = 1e-10; - ddpSettings.relTolODE_ = 1e-7; - ddpSettings.maxNumStepsPerSecond_ = 10000; - ddpSettings.useFeedbackPolicy_ = true; - ddpSettings.debugPrintRollout_ = false; - - ocs2::rollout::Settings rolloutSettings; - rolloutSettings.absTolODE = 1e-10; - rolloutSettings.relTolODE = 1e-7; - rolloutSettings.timeStep = 1e-3; - rolloutSettings.maxNumStepsPerSecond = 10000; - rolloutSettings.maxSingleEventIterations = 5; - rolloutSettings.useTrajectorySpreadingController = true; - +TEST(BouncingMassTest, state_triggered_hybrid_slq) { // Parameters - const scalar_t startTime = 0.0; - const scalar_t finalTime = 2.5; - const vector_t x0 = Eigen::Matrix(0.7, 0.5, 0); - const vector_t u0 = vector_t::Zero(INPUT_DIM); - bool outputSolution = false; + constexpr bool outputSolution = false; + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 2.5; + const ocs2::vector_t x0 = Eigen::Matrix(0.7, 0.5, 0); + const ocs2::vector_t u0 = ocs2::vector_t::Zero(INPUT_DIM); + + const auto ddpSettings = [&]() { + ocs2::ddp::Settings s; + s.algorithm_ = ocs2::ddp::Algorithm::SLQ; + s.displayInfo_ = false; + s.displayShortSummary_ = true; + s.maxNumIterations_ = 30; + s.minRelCost_ = 1e-4; + s.nThreads_ = 1; + s.checkNumericalStability_ = true; + s.absTolODE_ = 1e-10; + s.relTolODE_ = 1e-7; + s.maxNumStepsPerSecond_ = 10000; + s.useFeedbackPolicy_ = true; + s.debugPrintRollout_ = false; + return s; + }(); + + const auto rolloutSettings = [&]() { + ocs2::rollout::Settings s; + s.absTolODE = 1e-10; + s.relTolODE = 1e-7; + s.timeStep = 1e-3; + s.maxNumStepsPerSecond = 10000; + s.maxSingleEventIterations = 5; + s.useTrajectorySpreadingController = true; + return s; + }(); // Generation of Reference Trajectory - const scalar_array_t trajTimes{0, 0.2, 0.8, 1.0, 1.2, 1.8, 2.0}; - - vector_t state0(STATE_DIM); // Initial and final state - state0 << 0.5, 0, 0; - vector_t state1(STATE_DIM); // Hard impact - state1 << 0, -5, 0; - vector_t state2(STATE_DIM); // Soft impact - state2 << 0, -1, 0; + const ocs2::scalar_array_t trajTimes{0, 0.2, 0.8, 1.0, 1.2, 1.8, 2.0}; + const ocs2::vector_t state0 = (ocs2::vector_t(STATE_DIM) << 0.5, 0, 0).finished(); // Initial and final state + const ocs2::vector_t state1 = (ocs2::vector_t(STATE_DIM) << 0, -5, 0).finished(); // Hard impact + const ocs2::vector_t state2 = (ocs2::vector_t(STATE_DIM) << 0, -1, 0).finished(); // Soft impact + const ocs2::vector_array_t trajStates{state0, state1, state2, state2, state1, state2, state0}; - const scalar_t delta = 0.5; - const vector_array_t trajStates{state0, state1, state2, state2, state1, state2, state0}; OverallReference reference(trajTimes, trajStates); + constexpr ocs2::scalar_t delta = 0.5; reference.extendref(delta); + // OCP + ocs2::OptimalControlProblem problem; + // Dynamics, Constraints and derivative classes BouncingMassDynamics systemDynamics; + problem.dynamicsPtr.reset(systemDynamics.clone()); // Cost Function - matrix_t Q(STATE_DIM, STATE_DIM); - Q << 50.0, 0.0, 0.0, 0.0, 50.0, 0.0, 0.0, 0.0, 0.0; - matrix_t R(INPUT_DIM, INPUT_DIM); - R << 1.0; - std::unique_ptr cost(new BouncingMassCost(reference, Q, R)); - - matrix_t Qf(STATE_DIM, STATE_DIM); - Qf << 56.63, 7.07, 0.0, 7.07, 8.01, 0.0, 0.0, 0.0, 0.0; - std::unique_ptr finalCost(new BouncingMassFinalCost(reference, Qf, finalTime)); - std::unique_ptr preJumpCost(new BouncingMassFinalCost(reference, Qf, finalTime)); - - ocs2::OptimalControlProblem problem; - problem.dynamicsPtr.reset(systemDynamics.clone()); - problem.costPtr->add("cost", std::move(cost)); - problem.preJumpCostPtr->add("preJumpCost", std::move(preJumpCost)); - problem.finalCostPtr->add("finalCost", std::move(finalCost)); + const ocs2::matrix_t Q = (ocs2::matrix_t(STATE_DIM, STATE_DIM) << 50.0, 0.0, 0.0, 0.0, 50.0, 0.0, 0.0, 0.0, 0.0).finished(); + const ocs2::matrix_t R = (ocs2::matrix_t(INPUT_DIM, INPUT_DIM) << 1.0).finished(); + const ocs2::matrix_t Qf = (ocs2::matrix_t(STATE_DIM, STATE_DIM) << 56.63, 7.07, 0.0, 7.07, 8.01, 0.0, 0.0, 0.0, 0.0).finished(); + problem.costPtr->add("cost", std::make_unique(reference, Q, R)); + problem.preJumpCostPtr->add("preJumpCost", std::make_unique(reference, Qf, finalTime)); + problem.finalCostPtr->add("finalCost", std::make_unique(reference, Qf, finalTime)); // Rollout Class ocs2::StateTriggeredRollout stateTriggeredRollout(systemDynamics, rolloutSettings); // Initial Controller - matrix_t controllerGain(INPUT_DIM, STATE_DIM); - controllerGain << 25, 10, 0; - vector_t controllerBias; - - matrix_array_t controllerGainArray; - vector_array_t controllerBiasArray; - scalar_array_t timeStampArray; - - constexpr scalar_t controllerDeltaTime = 1e-3; // Time step for controller time array - constexpr scalar_t eps = ocs2::numeric_traits::weakEpsilon(); - scalar_array_t controlTimes = trajTimes; - controlTimes.push_back(finalTime); - - for (int i = 0; i < controlTimes.size() - 1; i++) { - scalar_t timeSteps = (controlTimes[i + 1] + eps - controlTimes[i]) / controllerDeltaTime; - scalar_t timeStamp = 0; - vector_t refState; - - for (int j = 0; j <= timeSteps; j++) { - if (j == 0 && i < controlTimes.size() - 2) { - timeStamp = controlTimes[i] + eps; - } else { - timeStamp = controlTimes[i] + j * controllerDeltaTime; + const auto initController = [&]() { + ocs2::matrix_array_t controllerGainArray; + ocs2::vector_array_t controllerBiasArray; + ocs2::scalar_array_t timeStampArray; + + constexpr ocs2::scalar_t controllerDeltaTime = 1e-3; // Time step for controller time array + constexpr ocs2::scalar_t eps = ocs2::numeric_traits::weakEpsilon(); + ocs2::scalar_array_t controlTimes = trajTimes; + controlTimes.push_back(finalTime); + + const ocs2::matrix_t controllerGain = (ocs2::matrix_t(INPUT_DIM, STATE_DIM) << 25, 10, 0).finished(); + for (int i = 0; i < controlTimes.size() - 1; i++) { + const ocs2::scalar_t timeSteps = (controlTimes[i + 1] + eps - controlTimes[i]) / controllerDeltaTime; + ocs2::scalar_t timeStamp = 0; + for (int j = 0; j <= timeSteps; j++) { + if (j == 0 && i < controlTimes.size() - 2) { + timeStamp = controlTimes[i] + eps; + } else { + timeStamp = controlTimes[i] + j * controllerDeltaTime; + } + + ocs2::vector_t refState = reference.getState(timeStamp); + ocs2::vector_t refInput = reference.getInput(timeStamp); + ocs2::vector_t controllerBias = controllerGain * refState + refInput; + + timeStampArray.push_back(timeStamp); + controllerGainArray.push_back(-controllerGain); + controllerBiasArray.push_back(controllerBias); } - - vector_t refState = reference.getState(timeStamp); - vector_t refInput = reference.getInput(timeStamp); - controllerBias = controllerGain * refState + refInput; - - timeStampArray.push_back(timeStamp); - controllerGainArray.push_back(-controllerGain); - controllerBiasArray.push_back(controllerBias); } - } - ocs2::LinearController initController(timeStampArray, controllerBiasArray, controllerGainArray); + return ocs2::LinearController(timeStampArray, controllerBiasArray, controllerGainArray); + }(); + + // operating point + const ocs2::OperatingPoints operatingTrajectories(x0, u0); - ocs2::OperatingPoints operatingTrajectories(x0, u0); // SLQ ocs2::SLQ slq(ddpSettings, stateTriggeredRollout, problem, operatingTrajectories); slq.run(startTime, x0, finalTime, &initController); - auto solutionST = slq.primalSolution(finalTime); + const auto solutionST = slq.primalSolution(finalTime); + // Test 1: No penetration of Guard Surfaces for (int i = 0; i < solutionST.stateTrajectory_.size(); i++) { - // Test 1: No penetration of Guard Surfaces - EXPECT_GT(solutionST.stateTrajectory_[i][0], -ddpSettings.absTolODE_); - // Display output - // format: idx;time;x[0];xref[0];x[1];xref[1];u;uref - if (outputSolution) { - auto idx = static_cast(solutionST.stateTrajectory_[i][2]); - - vector_t uRef = reference.getInput(solutionST.timeTrajectory_[i]); - vector_t xRef = reference.getState(idx, solutionST.timeTrajectory_[i]); + EXPECT_GT(solutionST.stateTrajectory_[i](0), -ddpSettings.absTolODE_); + // Display output: format: idx;time;x[0];xref[0];x[1];xref[1];u;uref + if (outputSolution) { + const auto idx = static_cast(solutionST.stateTrajectory_[i][2]); + const auto uRef = reference.getInput(solutionST.timeTrajectory_[i]); + const auto xRef = reference.getState(idx, solutionST.timeTrajectory_[i]); std::cerr << i << ";" << idx << ";"; std::cerr << std::setprecision(25) << solutionST.timeTrajectory_[i]; - std::cerr << std::setprecision(6) << ";" << solutionST.stateTrajectory_[i][0] << ";" << xRef[0] << ";"; - std::cerr << solutionST.stateTrajectory_[i][1] << ";" << xRef[1] << ";"; - std::cerr << solutionST.inputTrajectory_[i][0] << ";" << uRef[0] << std::endl; + std::cerr << std::setprecision(6) << ";" << solutionST.stateTrajectory_[i](0) << ";" << xRef[0] << ";"; + std::cerr << solutionST.stateTrajectory_[i](1) << ";" << xRef[1] << ";"; + std::cerr << solutionST.inputTrajectory_[i](0) << ";" << uRef[0] << std::endl; } - } + } // end of i loop // Test 2: Check of cost function - auto performanceIndeces = slq.getPerformanceIndeces(); - constexpr scalar_t expectedCost = 7.15; - EXPECT_LT(std::fabs(performanceIndeces.cost - expectedCost), 100 * ddpSettings.minRelCost_); + constexpr ocs2::scalar_t expectedCost = 7.15; + const auto performanceIndeces = slq.getPerformanceIndeces(); + EXPECT_LE(performanceIndeces.cost, expectedCost); } diff --git a/ocs2_ddp/test/bouncingmass/OverallReference.cpp b/ocs2_ddp/test/bouncingmass/OverallReference.cpp index c6ccd631a..0a6628b9e 100644 --- a/ocs2_ddp/test/bouncingmass/OverallReference.cpp +++ b/ocs2_ddp/test/bouncingmass/OverallReference.cpp @@ -29,93 +29,49 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/test/bouncingmass/OverallReference.h" -OverallReference::OverallReference(const scalar_array_t trajTimes, const vector_array_t trajStates) { - References_.clear(); - References_.resize(trajTimes.size() - 1); +#include +OverallReference::OverallReference(const scalar_array_t& trajTimes, const vector_array_t& trajStates) { switchtimes_ = trajTimes; - vector_t x0 = trajStates[0]; + References_.reserve(trajTimes.size() - 1); for (int i = 0; i < trajTimes.size() - 1; i++) { - References_[i] = Reference(trajTimes[i], trajTimes[i + 1], x0, trajStates[i + 1]); - x0 = trajStates[i + 1]; - jumpMap(x0); - } -} - -int OverallReference::getIndex(scalar_t time) const { - for (int i = 0; i < switchtimes_.size() - 1; i++) { - if (switchtimes_[i + 1] >= time) { - return i; + if (i == 0) { + References_.emplace_back(trajTimes[i], trajTimes[i + 1], trajStates[i], trajStates[i + 1]); + } else { + References_.emplace_back(trajTimes[i], trajTimes[i + 1], jumpMap(trajStates[i]), trajStates[i + 1]); } } - - return -1; -} - -void OverallReference::getInput(scalar_t time, vector_t& input) const { - int idx = getIndex(time); - if (idx >= 0 && idx < References_.size()) { - References_[idx].getInput(time, input); - } else { - input.setZero(INPUT_DIM); - } } vector_t OverallReference::getInput(scalar_t time) const { - vector_t u; - getInput(time, u); - return u; + const int idx = ocs2::lookup::findIntervalInTimeArray(switchtimes_, time); + return (idx >= 0 && idx < References_.size()) ? References_[idx].getInput(time) : vector_t::Zero(INPUT_DIM); } -void OverallReference::getState(int idx, scalar_t time, vector_t& x) const { - if (idx >= 0 && idx < References_.size()) { - if (time < 2) { - References_[idx].getState(time, x); - } else { - getState(time, x); - } - } else { - References_[0].getState(0, x); - } +vector_t OverallReference::getState(scalar_t time) const { + const int idx = ocs2::lookup::findIntervalInTimeArray(switchtimes_, time); + return (idx >= 0 && idx < References_.size()) ? References_[idx].getState(time) : References_[0].getState(0); } vector_t OverallReference::getState(int idx, scalar_t time) const { vector_t state; - getState(idx, time, state); - return state; -} - -void OverallReference::getState(scalar_t time, vector_t& x) const { - int idx = getIndex(time); if (idx >= 0 && idx < References_.size()) { - References_[idx].getState(time, x); + if (time < 2) { + state = References_[idx].getState(time); + } else { + state = getState(time); + } } else { - References_[0].getState(0, x); + state = References_[0].getState(0); } -} - -vector_t OverallReference::getState(scalar_t time) const { - vector_t state; - getState(time, state); return state; } void OverallReference::extendref(scalar_t delta) { for (int i = 0; i < References_.size(); i++) { - Reference* pre; - Reference* post; - if (i > 0) { - pre = &References_[i - 1]; - } else { - pre = nullptr; - } - - if (i <= References_.size() - 1) { - post = &References_[i + 1]; - } else { - post = nullptr; - } + Reference* pre = i > 0 ? &References_[i - 1] : nullptr; + Reference* post = i < References_.size() - 1 ? &References_[i + 1] : nullptr; References_[i].extendref(delta, pre, post); } } @@ -124,8 +80,9 @@ void OverallReference::display(int i) { References_[i].display(); } -void OverallReference::jumpMap(vector_t& x) const { - scalar_t e = 0.95; - x[1] = x[1] - (1 + e) * x[1]; - x[2] = x[2] + 1; +vector_t OverallReference::jumpMap(const vector_t& x) const { + constexpr scalar_t e = 0.95; + vector_t y(x.size()); + y << x(0), -e * x(1), x(2) + 1.0; + return y; } diff --git a/ocs2_ddp/test/bouncingmass/Reference.cpp b/ocs2_ddp/test/bouncingmass/Reference.cpp index 2a1fc9656..17256c7d7 100644 --- a/ocs2_ddp/test/bouncingmass/Reference.cpp +++ b/ocs2_ddp/test/bouncingmass/Reference.cpp @@ -40,7 +40,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "ocs2_ddp/test/bouncingmass/Reference.h" Reference::Reference(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) { - Create5thOrdPol(t0, t1, p0, p1); + polX_ = Create5thOrdPol(t0, t1, p0, p1); polV_ = polyder(polX_); polU_ = polyder(polV_); @@ -48,20 +48,16 @@ Reference::Reference(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_ t1_ = t1; } -void Reference::getInput(scalar_t time, vector_t& input) const { - input.setZero(1); +vector_t Reference::getInput(scalar_t time) const { + vector_t input = vector_t::Zero(1); for (int i = 0; i < polU_.size(); i++) { input[0] += polU_[i] * std::pow(time, i); } -} - -vector_t Reference::getInput(scalar_t time) const { - vector_t input; - getInput(time, input); return input; } -void Reference::getState(scalar_t time, vector_t& x) const { +vector_t Reference::getState(scalar_t time) const { + vector_t x; if (time <= t1_ && time >= t0_) { x.setZero(3); for (int i = 0; i < polU_.size(); i++) { @@ -71,42 +67,33 @@ void Reference::getState(scalar_t time, vector_t& x) const { } else { interpolate_ext(time, x); } + return x; } void Reference::extendref(scalar_t delta, Reference* refPre, Reference* refPost) { - delta_ = delta; + constexpr scalar_t dt = 1e-3; + boost::numeric::odeint::runge_kutta_dopri5 stepper; + // Lambda for general system dynamics, assuming that the reference input is available - auto model = [](const vector_t& x, vector_t& dxdt, const double t, vector_t uref) { - matrix_t A(3, 3); - A << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - matrix_t B(3, 1); - B << 0.0, 1.0, 0.0; - - dxdt = A * x + B * uref; - }; + const matrix_t A = (matrix_t(3, 3) << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0).finished(); + const matrix_t B = (matrix_t(3, 1) << 0.0, 1.0, 0.0).finished(); + auto model = [&](const vector_t& x, const vector_t& uref, vector_t& dxdt, const scalar_t t) { dxdt = A * x + B * uref; }; + // pre-part of extension if (refPre != nullptr) { // Construct Lambda to represent System Dynamics with correct reference input - auto preModel = [&refPre, &model](const vector_t& x, vector_t& dxdt, const double t) { - vector_t uref = refPre->getInput(t); - model(x, dxdt, t, uref); - }; + auto preModel = [&](const vector_t& x, vector_t& dxdt, const scalar_t t) { model(x, refPre->getInput(t), dxdt, t); }; // Construct lambda to act as observer, which will store the time and state trajectories - scalar_array_t* timeStorePtr = &tPre_; - vector_array_t* stateStorePtr = &xPre_; - auto preObserver = [&timeStorePtr, &stateStorePtr](vector_t& x, scalar_t& t) { - timeStorePtr->push_back(t); - stateStorePtr->push_back(x); + auto preObserver = [&](const vector_t& x, scalar_t t) { + tPre_.push_back(t); + xPre_.push_back(x); }; - vector_t x0; - getState(t0_, x0); - scalar_t t0 = t0_; - scalar_t t1 = t0 - delta; - scalar_t dt = -1e-3; - - boost::numeric::odeint::integrate_adaptive(stepper, preModel, x0, t0, t1, dt, preObserver); + vector_t x0 = getState(t0_); + const scalar_t t0 = t0_; + const scalar_t t1 = t0 - delta; + boost::numeric::odeint::integrate_adaptive(stepper, preModel, x0, t0, t1, -dt, preObserver); std::reverse(std::begin(tPre_), std::end(tPre_)); std::reverse(std::begin(xPre_), std::end(xPre_)); } @@ -114,41 +101,31 @@ void Reference::extendref(scalar_t delta, Reference* refPre, Reference* refPost) // post-part of extension if (refPost != nullptr) { // Construct Lambda to represent System Dynamics with correct reference input - auto postModel = [&refPost, &model](const vector_t& x, vector_t& dxdt, const double t) { - vector_t uref = refPost->getInput(t); - model(x, dxdt, t, uref); - }; + auto postModel = [&](const vector_t& x, vector_t& dxdt, const scalar_t t) { model(x, refPost->getInput(t), dxdt, t); }; // Construct lambda to act as observer, which will store the time and state trajectories - scalar_array_t* timeStorePtr = &tPost_; - vector_array_t* stateStorePtr = &xPost_; - auto postObserver = [&timeStorePtr, &stateStorePtr](vector_t& x, scalar_t& t) { - timeStorePtr->push_back(t); - stateStorePtr->push_back(x); + auto postObserver = [&](const vector_t& x, scalar_t t) { + tPost_.push_back(t); + xPost_.push_back(x); }; - vector_t x0; - getState(t1_, x0); - scalar_t t0 = t1_; - scalar_t t1 = t0 + delta; - scalar_t dt = 1e-3; - boost::numeric::odeint::integrate_adaptive(stepper, postModel, x0, t0, t1, dt, postObserver); + vector_t x0 = getState(t1_); + const scalar_t t0 = t1_; + const scalar_t t1 = t0 + delta; + boost::numeric::odeint::integrate_adaptive(stepper, postModel, x0, t0, t1, -dt, postObserver); } } -void Reference::Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) { +Eigen::Matrix Reference::Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) const { Eigen::Matrix A; - Eigen::Matrix Ainv; - A << 1, t0, std::pow(t0, 2), std::pow(t0, 3), std::pow(t0, 4), std::pow(t0, 5), 0, 1, 2 * t0, 3 * std::pow(t0, 2), 4 * std::pow(t0, 3), 5 * std::pow(t0, 4), 0, 0, 2, 6 * t0, 12 * std::pow(t0, 2), 20 * std::pow(t0, 3), 1, t1, std::pow(t1, 2), std::pow(t1, 3), std::pow(t1, 4), std::pow(t1, 5), 0, 1, 2 * t1, 3 * std::pow(t1, 2), 4 * std::pow(t1, 3), 5 * std::pow(t1, 4), 0, 0, 2, 6 * t1, 12 * std::pow(t1, 2), 20 * std::pow(t1, 3); - Ainv = A.inverse(); + Eigen::Matrix b; + b << p0, p1; - Eigen::Matrix x; - x << p0, p1; - polX_ = Ainv * x; + return A.colPivHouseholderQr().solve(b); } void Reference::interpolate_ext(scalar_t time, vector_t& x) const { @@ -187,12 +164,10 @@ void Reference::display() { std::cerr << "####Normal-Trajectory####" << std::endl; std::cerr << "#########################" << std::endl; - scalar_t dt = 0.01; + constexpr scalar_t dt = 0.01; for (int i = 0; i < (t1_ - t0_) / dt; i++) { scalar_t t = t0_ + dt * i; - vector_t x; - getState(t, x); - + vector_t x = getState(t); std::cerr << t << ";" << x[0] << ";" << x[1] << std::endl; } diff --git a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h index 2b95ebfda..ad4ea08e9 100644 --- a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h +++ b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/OverallReference.h @@ -59,15 +59,7 @@ class OverallReference { * @param [in] trajTimes: list of times at which the reference is defined * @param [in] trajState: list of waypoints at which the reference is defined */ - OverallReference(const scalar_array_t trajTimes, const vector_array_t trajState); - - /* - * Calculate the input at a certain time - * - * @param [in] time: time moment at which the input is calculated - * @param [out] input: input corresponding to time - */ - void getInput(scalar_t time, vector_t& input) const; + OverallReference(const scalar_array_t& trajTimes, const vector_array_t& trajState); /* * Calculate the input at a certain time @@ -77,14 +69,6 @@ class OverallReference { */ vector_t getInput(scalar_t time) const; - /* - * Calculate the reference state at a certain time - * - * @param [in] time: time moment at which the input is calculated - * @param [out] state: state corresponding to time - */ - void getState(scalar_t time, vector_t& x) const; - /* * Calculate the reference state at a certain time * @@ -102,15 +86,6 @@ class OverallReference { */ vector_t getState(int idx, scalar_t time) const; - /* - * Calculate the reference state at a certain time and mode - * - * @param [in] idx: mode at which the input is calculated - * @param [in] time: time moment at which the input is calculated - * @param [out] state: state corresponding to time and mode - */ - void getState(int idx, scalar_t time, vector_t& x) const; - /* * Extend the reference past the event times, by integrating the input signal * without applying the jump map @@ -127,23 +102,12 @@ class OverallReference { void display(int i); private: - /* - * Find the index of the currently active reference - * - * @param [in] time: time moment at which the index is requested - * - * @return currently active index - */ - int getIndex(scalar_t time) const; - /* * Jump map of the system - * * @param [in] x: State before event - * - * @return currently active index + * @return state after event */ - void jumpMap(vector_t& x) const; + vector_t jumpMap(const vector_t& x) const; std::vector References_; std::vector switchtimes_; diff --git a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h index c486a68c6..9acfffdf1 100644 --- a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h +++ b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/Reference.h @@ -42,17 +42,11 @@ using vector_array_t = ocs2::vector_array_t; /* * This class constructs, contains a section of the reference, which is contained in * OverallReference and used in BouncingMass SLQ test. - * The reference is based on the reference presented - * in [On Optimal Trajectory Tracking for Mechanical Systems with Unilateral Constraints - * by M. Rijnen] + * The reference is based on the reference presented in + * "On Optimal Trajectory Tracking for Mechanical Systems with Unilateral Constraints" by M. Rijnen */ class Reference { public: - /* - * Constructor - */ - Reference() = default; - /* * Constructor * @@ -63,14 +57,6 @@ class Reference { */ Reference(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1); - /* - * Obtain reference input at the current time - * - * @param [in] time: current time - * @param [out] input: current reference input - */ - void getInput(const scalar_t time, vector_t& input) const; - /* * Obtain reference input at the current time * @@ -83,9 +69,9 @@ class Reference { * Obtain reference state at current time * * @param [in] time: current time - * @param [out] x: current reference state + * @return current reference state */ - void getState(const scalar_t time, vector_t& x) const; + vector_t getState(const scalar_t time) const; /* * Extend the reference by integrating the input signal of the next @@ -97,14 +83,11 @@ class Reference { */ void extendref(scalar_t delta, Reference* refPre, Reference* refPost); - /* - * Display the reference - */ + /* Display the reference */ void display(); scalar_t t0_; scalar_t t1_; - scalar_t delta_ = 0; private: /* @@ -116,7 +99,7 @@ class Reference { * @param [in] p0: first waypoint * @param [in] p1: second waypoint */ - void Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1); + Eigen::Matrix Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) const; /* * Find the derivative of the polynomial described by a diff --git a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/SystemModel.h b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/SystemModel.h index b01246b45..6f8682932 100644 --- a/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/SystemModel.h +++ b/ocs2_ddp/test/include/ocs2_ddp/test/bouncingmass/SystemModel.h @@ -51,10 +51,10 @@ class BouncingMassDynamics final : public ocs2::LinearSystemDynamics { public: BouncingMassDynamics() : LinearSystemDynamics(matrix_t(STATE_DIM, STATE_DIM), matrix_t(STATE_DIM, INPUT_DIM), matrix_t(STATE_DIM, STATE_DIM)) { - LinearSystemDynamics::A_ << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - LinearSystemDynamics::B_ << 0.0, 1.0, 0.0; - const scalar_t e = 0.95; - LinearSystemDynamics::G_ << 1.0, 0.0, 0.0, 0.0, -e, 0.0, 0.0, 0.0, 1.0; + constexpr scalar_t e = 0.95; + A_ << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; + B_ << 0.0, 1.0, 0.0; + G_ << 1.0, 0.0, 0.0, 0.0, -e, 0.0, 0.0, 0.0, 1.0; } ~BouncingMassDynamics() override = default; @@ -86,7 +86,7 @@ class BouncingMassDynamics final : public ocs2::LinearSystemDynamics { class BouncingMassCost final : public ocs2::StateInputCost { public: - BouncingMassCost(OverallReference ref, matrix_t Q, matrix_t R) : ref_(ref), Q_(std::move(Q)), R_(std::move(R)) {} + BouncingMassCost(OverallReference ref, matrix_t Q, matrix_t R) : ref_(std::move(ref)), Q_(std::move(Q)), R_(std::move(R)) {} ~BouncingMassCost() override = default; @@ -95,16 +95,16 @@ class BouncingMassCost final : public ocs2::StateInputCost { scalar_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const ocs2::TargetTrajectories&, const PreComputation&) const override { const auto stateInput = getNominalStateInput(t, x, u); - vector_t xDeviation = x - stateInput.first; - vector_t uDeviation = u - stateInput.second; + const vector_t xDeviation = x - stateInput.first; + const vector_t uDeviation = u - stateInput.second; return 0.5 * xDeviation.dot(Q_ * xDeviation) + 0.5 * uDeviation.dot(R_ * uDeviation); } ScalarFunctionQuadraticApproximation getQuadraticApproximation(scalar_t t, const vector_t& x, const vector_t& u, const ocs2::TargetTrajectories&, const PreComputation&) const override { const auto stateInput = getNominalStateInput(t, x, u); - vector_t xDeviation = x - stateInput.first; - vector_t uDeviation = u - stateInput.second; + const vector_t xDeviation = x - stateInput.first; + const vector_t uDeviation = u - stateInput.second; ScalarFunctionQuadraticApproximation L; L.f = 0.5 * xDeviation.dot(Q_ * xDeviation) + 0.5 * uDeviation.dot(R_ * uDeviation); @@ -118,38 +118,32 @@ class BouncingMassCost final : public ocs2::StateInputCost { private: std::pair getNominalStateInput(scalar_t t, const vector_t& x, const vector_t& u) const { - vector_t xRef; - vector_t uRef; const int currentMode = x.tail(1).value(); - ref_.getInput(t, uRef); - ref_.getState(currentMode, t, xRef); - return {xRef, uRef}; + return {ref_.getState(currentMode, t), ref_.getInput(t)}; } - private: - OverallReference ref_; - matrix_t Q_; - matrix_t R_; + const OverallReference ref_; + const matrix_t Q_; + const matrix_t R_; }; class BouncingMassFinalCost final : public ocs2::StateCost { public: BouncingMassFinalCost(OverallReference ref, matrix_t QFinal, scalar_t timeFinal) - : ref_(ref), timeFinal_(timeFinal), QFinal_(std::move(QFinal)) {} + : ref_(std::move(ref)), timeFinal_(timeFinal), QFinal_(std::move(QFinal)) {} ~BouncingMassFinalCost() override = default; BouncingMassFinalCost* clone() const override { return new BouncingMassFinalCost(*this); } scalar_t getValue(scalar_t t, const vector_t& x, const ocs2::TargetTrajectories&, const PreComputation&) const override { - vector_t xDeviation = x - getNominalFinalState(t, x); + const vector_t xDeviation = x - getNominalFinalState(t, x); return 0.5 * xDeviation.dot(QFinal_ * xDeviation); } ScalarFunctionQuadraticApproximation getQuadraticApproximation(scalar_t t, const vector_t& x, const ocs2::TargetTrajectories&, const PreComputation&) const override { - vector_t xDeviation = x - getNominalFinalState(t, x); - + const vector_t xDeviation = x - getNominalFinalState(t, x); ScalarFunctionQuadraticApproximation Phi; Phi.f = 0.5 * xDeviation.dot(QFinal_ * xDeviation); Phi.dfdx = QFinal_ * xDeviation; @@ -162,16 +156,13 @@ class BouncingMassFinalCost final : public ocs2::StateCost { // Terminal cost only calculated for final state, not for intermediate switch states if (std::fabs(t - timeFinal_) > ocs2::numeric_traits::weakEpsilon()) { return x; + } else { + const int currentMode = x.tail(1).value(); + return ref_.getState(currentMode, t); } - - vector_t xRef; - const int currentMode = x.tail(1).value(); - ref_.getState(currentMode, t, xRef); - return xRef; } - private: - OverallReference ref_; - scalar_t timeFinal_; - matrix_t QFinal_; + const OverallReference ref_; + const scalar_t timeFinal_; + const matrix_t QFinal_; }; diff --git a/ocs2_ddp/test/testContinuousTimeLqr.cpp b/ocs2_ddp/test/testContinuousTimeLqr.cpp index 9eba70956..f88b05d3b 100644 --- a/ocs2_ddp/test/testContinuousTimeLqr.cpp +++ b/ocs2_ddp/test/testContinuousTimeLqr.cpp @@ -47,7 +47,7 @@ TEST(testContinousTimeLqr, compareWithMatlab) { const matrix_t Q = (matrix_t(2, 2) << 3.0, 2.0, 2.0, 4.0).finished(); const matrix_t R = (matrix_t(1, 1) << 5.0).finished(); const matrix_t P = (matrix_t(1, 2) << 0.1, 0.2).finished(); - std::unique_ptr cost(new QuadraticStateInputCost(Q, R, P)); + auto cost = std::make_unique(Q, R, P); const scalar_t time = 0.0; const vector_t state = vector_t::Zero(2); diff --git a/ocs2_ddp/test/testDdpHelperFunction.cpp b/ocs2_ddp/test/testDdpHelperFunction.cpp index a779c5352..06c7ed6b0 100644 --- a/ocs2_ddp/test/testDdpHelperFunction.cpp +++ b/ocs2_ddp/test/testDdpHelperFunction.cpp @@ -57,7 +57,7 @@ TEST(extractPrimalSolution, eventAtInitTime) { primalSolution.postEventIndices_.push_back(primalSolution.timeTrajectory_.size()); primalSolution.modeSchedule_.modeSequence.push_back(s + 1); primalSolution.modeSchedule_.eventTimes.push_back(primalSolution.timeTrajectory_.back()); - } // end of s + } // end of s // an extra event at final time primalSolution.timeTrajectory_.push_back(primalSolution.timeTrajectory_.back()); diff --git a/ocs2_ddp/test/testReachingTask.cpp b/ocs2_ddp/test/testReachingTask.cpp new file mode 100644 index 000000000..a19ecfe4d --- /dev/null +++ b/ocs2_ddp/test/testReachingTask.cpp @@ -0,0 +1,194 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include +#include + +#include +#include + +namespace ocs2 { + +namespace { +// rollout settings +rollout::Settings getRolloutSettings(ddp::Algorithm alg) { + rollout::Settings s; + s.absTolODE = 1e-9; + s.relTolODE = 1e-6; + s.timeStep = DoubleIntegratorReachingTask::timeStep; + s.integratorType = (alg == ddp::Algorithm::SLQ) ? IntegratorType::ODE45 : IntegratorType::RK4; + s.maxNumStepsPerSecond = 100000; + s.checkNumericalStability = true; + return s; +} + +// DDP settings +ddp::Settings getDdpSettings(ddp::Algorithm alg, bool display) { + ddp::Settings s; + s.algorithm_ = alg; + s.nThreads_ = 2; + s.maxNumIterations_ = 50; + s.displayInfo_ = false; + s.displayShortSummary_ = display; + s.debugPrintRollout_ = false; + s.minRelCost_ = DoubleIntegratorReachingTask::minRelCost; + s.constraintTolerance_ = DoubleIntegratorReachingTask::constraintTolerance; + s.timeStep_ = DoubleIntegratorReachingTask::timeStep; + s.backwardPassIntegratorType_ = (alg == ddp::Algorithm::SLQ) ? IntegratorType::ODE45 : IntegratorType::RK4; + s.strategy_ = search_strategy::Type::LINE_SEARCH; + s.lineSearch_.minStepLength = 0.01; + s.lineSearch_.maxStepLength = 1.0; + return s; +} +} // unnamed namespace + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +/* Reaching task at event time (tGoal) for a problem with horizon 2*tGoal. The solution input should be zero in [tGoal, 2*tGoal]. */ +class PreJumpDoubleIntegratorReachingTask : public DoubleIntegratorReachingTask, + public testing::Test, + public testing::WithParamInterface { + protected: + PreJumpDoubleIntegratorReachingTask() { + // reference manager + referenceManagerPtr = getReferenceManagerPtr(); + // optimal control problem + ocp.dynamicsPtr = getDynamicsPtr(); + ocp.costPtr->add("cost", getCostPtr()); + ocp.equalityConstraintPtr->add("zero_force", std::make_unique(*referenceManagerPtr)); + ocp.preJumpEqualityLagrangianPtr->add("goal_reaching", getGoalReachingAugmentedLagrangian(xGoal, GetParam())); + } + + void test(const PrimalSolution& primalSolution) const { + for (size_t i = 0; i < primalSolution.timeTrajectory_.size(); ++i) { + // zero input after tGoal + if (primalSolution.timeTrajectory_[i] > tGoal) { + EXPECT_NEAR(primalSolution.inputTrajectory_[i](0), 0.0, constraintTolerance) << " at time " << primalSolution.timeTrajectory_[i]; + } + // reaching to the target + if (primalSolution.timeTrajectory_[i] > tGoal) { + EXPECT_TRUE(primalSolution.stateTrajectory_[i].isApprox(xGoal, constraintTolerance)) + << " at time " << primalSolution.timeTrajectory_[i]; + } + } // end of i loop + } + + OptimalControlProblem ocp; + std::shared_ptr referenceManagerPtr; +}; + +TEST_P(PreJumpDoubleIntegratorReachingTask, SLQ) { + constexpr bool display = true; + constexpr auto alg = ddp::Algorithm::SLQ; + // rollout + TimeTriggeredRollout rollout(*ocp.dynamicsPtr, getRolloutSettings(alg)); + // solver + SLQ ddp(getDdpSettings(alg, display), rollout, ocp, *getInitializer()); + ddp.setReferenceManager(referenceManagerPtr); + // run solver + ddp.run(0.0, xInit, 2.0 * tGoal); + PrimalSolution primalSolution; + ddp.getPrimalSolution(2.0 * tGoal, &primalSolution); + // test + test(primalSolution); + printSolution(primalSolution, display); +} + +INSTANTIATE_TEST_CASE_P(PreJumpDoubleIntegratorReachingTaskCase, PreJumpDoubleIntegratorReachingTask, + testing::ValuesIn({DoubleIntegratorReachingTask::PenaltyType::QuadraticPenalty, + DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty}), + [](const testing::TestParamInfo& info) { + if (info.param == DoubleIntegratorReachingTask::PenaltyType::QuadraticPenalty) { + return std::string("QuadraticPenalty"); + } else if (info.param == DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty) { + return std::string("SmoothAbsolutePenalty"); + } + }); + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +/* Reaching task at final time for a problem with horizon tGoal. */ +class FinalDoubleIntegratorReachingTask : public DoubleIntegratorReachingTask, + public testing::Test, + public testing::WithParamInterface { + protected: + FinalDoubleIntegratorReachingTask() { + // reference manager + referenceManagerPtr = getReferenceManagerPtr(); + // optimal control problem + ocp.dynamicsPtr = getDynamicsPtr(); + ocp.costPtr->add("cost", getCostPtr()); + ocp.finalEqualityLagrangianPtr->add("goal_reaching", getGoalReachingAugmentedLagrangian(xGoal, GetParam())); + } + + void test(const PrimalSolution& primalSolution) const { + // reaching to the target + EXPECT_TRUE(primalSolution.stateTrajectory_.back().isApprox(xGoal, constraintTolerance)) + << " at time " << primalSolution.timeTrajectory_.back(); + } + + OptimalControlProblem ocp; + std::shared_ptr referenceManagerPtr; +}; + +TEST_P(FinalDoubleIntegratorReachingTask, SLQ) { + constexpr bool display = true; + constexpr auto alg = ddp::Algorithm::SLQ; + // rollout + TimeTriggeredRollout rollout(*ocp.dynamicsPtr, getRolloutSettings(alg)); + // solver + SLQ ddp(getDdpSettings(alg, display), rollout, ocp, *getInitializer()); + ddp.setReferenceManager(referenceManagerPtr); + // run solver + ddp.run(0.0, xInit, tGoal); + PrimalSolution primalSolution; + ddp.getPrimalSolution(tGoal, &primalSolution); + // test + test(primalSolution); + printSolution(primalSolution, display); +} + +INSTANTIATE_TEST_CASE_P(FinalDoubleIntegratorReachingTaskCase, FinalDoubleIntegratorReachingTask, + testing::ValuesIn({DoubleIntegratorReachingTask::PenaltyType::QuadraticPenalty, + DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty}), + [](const testing::TestParamInfo& info) { + if (info.param == DoubleIntegratorReachingTask::PenaltyType::QuadraticPenalty) { + return std::string("QuadraticPenalty"); + } else if (info.param == DoubleIntegratorReachingTask::PenaltyType::SmoothAbsolutePenalty) { + return std::string("SmoothAbsolutePenalty"); + } else { + throw std::runtime_error("Undefined PenaltyTyp!"); + } + }); + +} // namespace ocs2 diff --git a/ocs2_doc/docs/index.rst b/ocs2_doc/docs/index.rst index 11450c76d..44a2bf541 100644 --- a/ocs2_doc/docs/index.rst +++ b/ocs2_doc/docs/index.rst @@ -11,6 +11,7 @@ Table of Contents robotic_examples.rst from_urdf_to_ocp.rst profiling.rst + mpcnet.rst faq.rst .. rubric:: Reference and Index: diff --git a/ocs2_doc/docs/installation.rst b/ocs2_doc/docs/installation.rst index 1e8213f6e..1df98a6ee 100644 --- a/ocs2_doc/docs/installation.rst +++ b/ocs2_doc/docs/installation.rst @@ -98,6 +98,46 @@ Optional Dependencies * `Grid Map `__ catkin package, which may be installed with ``sudo apt install ros-noetic-grid-map-msgs``. +* `ONNX Runtime `__ is an inferencing and training accelerator. Here, it is used for deploying learned :ref:`MPC-Net ` policies in C++ code. To locally install it, do the following: + + .. code-block:: bash + + cd /tmp + wget https://github.com/microsoft/onnxruntime/releases/download/v1.7.0/onnxruntime-linux-x64-1.7.0.tgz + tar xf onnxruntime-linux-x64-1.7.0.tgz + mkdir -p ~/.local/bin ~/.local/include/onnxruntime ~/.local/lib ~/.local/share/cmake/onnxruntime + rsync -a /tmp/onnxruntime-linux-x64-1.7.0/include/ ~/.local/include/onnxruntime + rsync -a /tmp/onnxruntime-linux-x64-1.7.0/lib/ ~/.local/lib + rsync -a ~/git/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/ ~/.local/share/cmake/onnxruntime + + We provide custom cmake config and version files to enable ``find_package(onnxruntime)`` without modifying ``LIBRARY_PATH`` and ``LD_LIBRARY_PATH``. Note that the last command above assumes that you cloned OCS2 into the folder ``git`` in your user's home directory. + +* `Virtual environments `__ are recommended when training :ref:`MPC-Net ` policies: + + .. code-block:: bash + + sudo apt-get install python3-venv + + Create an environment and give it access to the system site packages: + + .. code-block:: bash + + mkdir venvs && cd venvs + python3 -m venv mpcnet + + Activate the environment and install the requirements: + + .. code-block:: bash + + source ~/venvs/mpcnet/bin/activate + python3 -m pip install -r ~/git/ocs2/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt + + Newer graphics cards might require a CUDA capability which is currently not supported by the standard PyTorch installation. + In that case check `PyTorch Start Locally `__ for a compatible version and, e.g., run: + + .. code-block:: bash + + pip3 install torch==1.10.2+cu113 -f https://download.pytorch.org/whl/cu113/torch_stable.html .. _doxid-ocs2_doc_installation_ocs2_doc_install: diff --git a/ocs2_doc/docs/intro.rst b/ocs2_doc/docs/intro.rst index 2cab1805b..ffac40b6e 100644 --- a/ocs2_doc/docs/intro.rst +++ b/ocs2_doc/docs/intro.rst @@ -7,10 +7,11 @@ Introduction **S**\ witched **S**\ ystems (OCS2). The toolbox provides an efficient implementation of the following algorithms: -* **SLQ**\: Continuous-time domain DDP -* **iLQR**\: Discrete-time domain DDP +* **SLQ**\: Continuous-time domain constrained DDP +* **iLQR**\: Discrete-time domain constrained DDP * **SQP**\: Multiple-shooting algorithm based on `HPIPM `__ -* **PISOC**\: Path integral stochastic optimal control +* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method +* **SLP**\: Sequential Linear Programming based on `PIPG `__ OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic diff --git a/ocs2_doc/docs/mpcnet.rst b/ocs2_doc/docs/mpcnet.rst new file mode 100644 index 000000000..39993c850 --- /dev/null +++ b/ocs2_doc/docs/mpcnet.rst @@ -0,0 +1,164 @@ +.. index:: pair: page; MPC-Net + +.. _doxid-ocs2_doc_mpcnet: + +MPC-Net +======= + +MPC-Net is an imitation learning approach that uses solutions from MPC to guide the policy search. +The main idea is to imitate MPC by minimizing the control Hamiltonian while representing the corresponding control inputs by a parametrized policy. +MPC-Net can be used to clone a model predictive controller into a neural network policy, which can be evaluated much faster than MPC. +Therefore, MPC-Net is a useful proxy for MPC in computationally demanding applications that do not require the most exact solution. + +The multi-threaded data generation and policy evaluation run asynchronously with the policy training. +The data generation and policy evaluation are implemented in C++ and run on CPU, while the policy training is implemented in Python and runs on GPU. +The control Hamiltonian is represented by a linear-quadratic approximation. +Therefore, the training can run on GPU without callbacks to OCS2 C++ code running on CPU to evaluate the Hamiltonian, and one can exploit batch processing on GPU. + +Robots +~~~~~~ + +MPC-Net has been implemented for the following :ref:`Robotic Examples `: + +============= ================ ================= ======== ============= +Robot Recom. CPU Cores Recom. GPU Memory RaiSim Training Time +============= ================ ================= ======== ============= +ballbot 4 2 GB No 0m 20s +legged_robot 12 8 GB Yes / No 7m 40s +============= ================ ================= ======== ============= + +Setup +~~~~~ + +Make sure to follow the :ref:`Installation ` page. +Follow all the instructions for the dependencies. +Regarding the optional dependencies, make sure to follow the instruction for ONNX Runtime and the virtual environment, optionally set up RaiSim. + +To build all MPC-Net packages, build the meta package: + +.. code-block:: bash + + cd + catkin_build ocs2_mpcnet + + # Example: + cd ~/catkin_ws + catkin_build ocs2_mpcnet + +To build a robot-specific package, replace :code:`` with the robot name: + +.. code-block:: bash + + cd + catkin_build ocs2__mpcnet + + # Example: + cd ~/catkin_ws + catkin_build ocs2_ballbot_mpcnet + +Training +~~~~~~~~ + +To train an MPC-Net policy, run: + +.. code-block:: bash + + cd /ocs2_mpcnet/ocs2__mpcnet/python/ocs2__mpcnet + source /devel/setup.bash + source /mpcnet/bin/activate + python3 train.py + + # Example: + cd ~/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet + source ~/catkin_ws/devel/setup.bash + source ~/venvs/mpcnet/bin/activate + python3 train.py + +To monitor the training progress with Tensorboard, run: + +.. code-block:: bash + + cd /ocs2_mpcnet/ocs2__mpcnet/python/ocs2__mpcnet + source /mpcnet/bin/activate + tensorboard --logdir=runs + + # Example: + cd ~/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet + source ~/venvs/mpcnet/bin/activate + tensorboard --logdir=runs + +If you use RaiSim, you can visualize the data generation and policy evaluation rollouts with RaiSim Unity, where pre-built executables are provided in RaiSim's :code:`raisimUnity` folder. For example, on Linux run: + +.. code-block:: bash + + /raisimUnity/linux/raisimUnity.x86_64 + + # Example: + ~/git/raisimLib/raisimUnity/linux/raisimUnity.x86_64 + +Deployment +~~~~~~~~~~ + +To deploy the default policy stored in the robot-specific package's :code:`policy` folder, run: + +.. code-block:: bash + + cd + source devel/setup.bash + roslaunch ocs2__mpcnet _mpcnet.launch + + # Example: + cd ~/catkin_ws + source devel/setup.bash + roslaunch ocs2_ballbot_mpcnet ballbot_mpcnet.launch + +To deploy a new policy stored in the robot-specific package's :code:`python/ocs2__mpcnet/runs` folder, replace :code:`` with the absolute file path to the final policy and run: + +.. code-block:: bash + + cd + source devel/setup.bash + roslaunch ocs2__mpcnet _mpcnet.launch policyFile:= + + # Example: + cd ~/catkin_ws + source devel/setup.bash + roslaunch ocs2_ballbot_mpcnet ballbot_mpcnet.launch policyFile:='/home/user/git/ocs2/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/runs/2022-04-01_12-00-00_ballbot_description/final_policy.onnx' + +How to Set Up a New Robot +~~~~~~~~~~~~~~~~~~~~~~~~~ + +Setting up MPC-Net for a new robot is relatively easy, as the **ocs2_mpcnet_core** package takes care of the data generation as well as policy evaluation rollouts and implements important learning components, such as the memory, policy, and loss function. + +This section assumes that you already have the packages for the robot-specific MPC implementation: + +1. **ocs2_**: Provides the library with the robot-specific MPC implementation. +2. **ocs2__ros**: Wraps around the MPC implementation with ROS to define ROS nodes. +3. **ocs2__raisim**: (Optional) interface between the robot-specific MPC implementation and RaiSim. + +For the actual **ocs2__mpcnet** package, follow the structure of existing robot-specific MPC-Net packages. +The most important classes/files that have to be implemented are: + +* **MpcnetDefinition**: Defines how OCS2 state variables are transformed to the policy observations. and how policy actions are transformed to OCS2 control inputs. +* **MpcnetInterface**: Provides the interface between C++ and Python, allowing to exchange data and policies. +* **.yaml**: Stores the configuration parameters. +* **mpcnet.py**: Adds robot-specific methods, e.g. implements the tasks that the robot should execute, for the MPC-Net training. +* **train.py**: Starts the main training script. + +Known Issues +~~~~~~~~~~~~ + +Stiff inequality constraints can lead to very large Hamiltonians and gradients of the Hamilltonian near the log barrier. +This can obstruct the learning process and the policy might not learn something useful. +In that case, enable the gradient clipping in the robot's MPC-Net YAML configuration file and tune the gradient clipping value. + +References +~~~~~~~~~~ + +This part of the toolbox has been developed based on the following publications: + +.. bibliography:: + :list: enumerated + + carius2020mpcnet + reske2021imitation diff --git a/ocs2_doc/docs/overview.rst b/ocs2_doc/docs/overview.rst index 6c7e5f68c..0f55770c7 100644 --- a/ocs2_doc/docs/overview.rst +++ b/ocs2_doc/docs/overview.rst @@ -9,10 +9,11 @@ Overview for **S**\ witched **S**\ ystems (OCS2). The toolbox provides an efficient implementation of the following algorithms: -* **SLQ**\: Continuous-time domain DDP -* **iLQR**\: Discrete-time domain DDP -* **SQP**\: Multiple-shooting algorithm based on `HPIPM `__ -* **PISOC**\: Path integral stochastic optimal control +* **SLQ**\: Continuous-time domain constrained DDP. +* **iLQR**\: Discrete-time domain constrained DDP. +* **SQP**\: Multiple-shooting algorithm based on `HPIPM `__. +* **SLP**\: Sequential Linear Programming based on `PIPG `__. +* **IPM**\: Multiple-shooting algorithm based on nonlinear interior point method. OCS2 handles general path constraints through Augmented Lagrangian or relaxed barrier methods. To facilitate the application of OCS2 in robotic @@ -51,17 +52,20 @@ Credits The following people have been involved in the development of OCS2: **Project Manager**: -Farbod Farshidian (ETHZ). +Farbod Farshidian. **Main Developers**: -Farbod Farshidian (ETHZ), -Ruben Grandia (ETHZ), -Michael Spieler (ETHZ), -Jan Carius (ETHZ), -Jean-Pierre Sleiman (ETHZ). - -**Other Developers**: +Farbod Farshidian, +Ruben Grandia, +Michael Spieler, +Jan Carius, +Jean-Pierre Sleiman. + +**Other Developers**: +Alexander Reske, +Sotaro Katayama, Mayank Mittal, +Jia-​Ruei Chiu, Johannes Pankert, Perry Franklin, Tom Lankhorst, @@ -75,6 +79,26 @@ Edo Jelavic. project has continued to evolve at RSL, ETH Zurich. The RSL team now actively supports the development of OCS2. +Citing OCS2 +~~~~~~~~~~~ + +To cite the toolbox in your academic research, please use the following: + +.. code-block:: latex + + @misc{OCS2, + title = {{OCS2}: An open source library for Optimal Control of Switched Systems}, + note = {[Online]. Available: \url{https://github.com/leggedrobotics/ocs2}}, + author = {Farbod Farshidian and others} + } + +Tutorials +~~~~~~~~~ + +* Tutorial on OCS2 toolbox, Farbod Farshidian, MPC Workshop, RSS 2021 (`link `__). + +* Real-time optimal control for legged locomotion and manipulation, Marco Hutter, MPC Workshop, RSS 2021 (`link `__). + Related publications ~~~~~~~~~~~~~~~~~~~~ diff --git a/ocs2_ipm/CMakeLists.txt b/ocs2_ipm/CMakeLists.txt new file mode 100644 index 000000000..798f35478 --- /dev/null +++ b/ocs2_ipm/CMakeLists.txt @@ -0,0 +1,114 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_ipm) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_core + ocs2_mpc + ocs2_oc + blasfeo_catkin + hpipm_catkin +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem +) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIRS} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + LIBRARIES + ${PROJECT_NAME} + DEPENDS + Boost +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +# Multiple shooting solver library +add_library(${PROJECT_NAME} + src/IpmHelpers.cpp + src/IpmInitialization.cpp + src/IpmPerformanceIndexComputation.cpp + src/IpmSettings.cpp + src/IpmSolver.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) + +# ######################### +# ### CLANG TOOLING ### +# ######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) + add_clang_tooling( + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR + ) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## Testing ## +############# + +catkin_add_gtest(test_${PROJECT_NAME} + test/Exp0Test.cpp + test/Exp1Test.cpp + test/testCircularKinematics.cpp + test/testSwitchedProblem.cpp + test/testUnconstrained.cpp + test/testValuefunction.cpp +) +add_dependencies(test_${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main +) \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h new file mode 100644 index 000000000..d9f27dac5 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmHelpers.h @@ -0,0 +1,134 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +namespace ocs2 { +namespace ipm { + +/** + * Removes the Newton directions of slack and dual variables and the linearized inequality constraints are removed from the linear system + * equations for the Newton step computation. These terms are considered in the quadratic approximation of the Lagrangian. + * + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slack : The slack variable associated with the inequality constraints. + * @param[in] dual : The dual variable associated with the inequality constraints. + * @param[in] ineqConstraints : Linear approximation of the inequality constraints. + * @param[in, out] lagrangian : Quadratic approximation of the Lagrangian. + */ +void condenseIneqConstraints(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, + const VectorFunctionLinearApproximation& ineqConstraints, ScalarFunctionQuadraticApproximation& lagrangian); + +/** + * Computes the SSE of the residual in the perturbed complementary slackness. + * + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slack : The slack variable associated with the inequality constraints. + * @param[in] dual : The dual variable associated with the inequality constraints. + * @return SSE of the residual in the perturbed complementary slackness + */ +inline scalar_t evaluateComplementarySlackness(scalar_t barrierParam, const vector_t& slack, const vector_t& dual) { + return (slack.array() * dual.array() - barrierParam).matrix().squaredNorm(); +} + +/** + * Retrieves the Newton directions of the slack variable associated with state-input inequality constraints. + * + * @param[in] stateInputIneqConstraints : State-input inequality constraints + * @param[in] dx : Newton direction of the state + * @param[in] du : Newton direction of the input + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slackStateInputIneq : The slack variable associated with the state-input inequality constraints. + * @return Newton directions of the slack variable. + */ +vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateInputIneqConstraints, const vector_t& dx, const vector_t& du, + scalar_t barrierParam, const vector_t& slackStateInputIneq); + +/** + * Retrieves the Newton directions of the slack variable associated with state-only inequality constraints. + * + * @param[in] stateIneqConstraints : State-only inequality constraints + * @param[in] dx : Newton direction of the state + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slackStateIneq : The slack variable associated with the state-only inequality constraints. + * @return Newton directions of the slack variable. + */ +vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateIneqConstraints, const vector_t& dx, scalar_t barrierParam, + const vector_t& slackStateIneq); + +/** + * Retrieves the Newton directions of the dual variable. + * + * @param[in] barrierParam : The barrier parameter of the interior point method. + * @param[in] slack : The slack variable associated with the inequality constraints. + * @param[in] dual : The dual variable associated with the inequality constraints. + * @param[in] slackDirection : The Newton direction of the slack variable. + * @return Newton directions of the dual variable. + */ +vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, const vector_t& slackDirection); + +/** + * Computes the step size via fraction-to-boundary-rule, which is introduced in the IPOPT's implementaion paper, + * "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" + * https://link.springer.com/article/10.1007/s10107-004-0559-y + * + * @param [in] v: A variable (slack or dual). + * @param [in] dv: The serach direction of the variable (slack dirction or dual dirction). + * @param [in] marginRate: Margin rate to avoid the slack or dual variables becoming too close to 0. Typical values are 0.95 or 0.995. + */ +scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scalar_t marginRate = 0.995); + +/** + * Convert the optimized slack or dual trajectories as a DualSolution. + * + * @param time: The time discretization. + * @param constraintsSize: The constraint tems size. + * @param stateIneq: The slack/dual variable trajectory of the state inequality constraints. + * @param stateInputIneq: The slack/dual variable trajectory of the state-input inequality constraints. + * @return A dual solution. + */ +DualSolution toDualSolution(const std::vector& time, const std::vector& constraintsSize, + const vector_array_t& stateIneq, const vector_array_t& stateInputIneq); + +/** + * Extracts slack/dual variables of the state-only and state-input constraints from a MultiplierCollection + * + * @param multiplierCollection: The MultiplierCollection. + * @return slack/dual variables of the state-only (first) and state-input constraints (second). + */ +std::pair fromMultiplierCollection(const MultiplierCollection& multiplierCollection); + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h new file mode 100644 index 000000000..cdc50a4ec --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmInitialization.h @@ -0,0 +1,116 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +namespace ocs2 { +namespace ipm { + +/** + * Initializes the slack variable. The initialization of the slack variables follows IPOPT + * (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Initialization). + * + * @param ineqConstraint: The value of the inequality constraint. + * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. + * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + * @return Initialized slack variable. + */ +inline vector_t initializeSlackVariable(const vector_t& ineqConstraint, scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + if (ineqConstraint.size() > 0) { + return (1.0 + initialSlackMarginRate) * ineqConstraint.cwiseMax(initialSlackLowerBound); + } else { + return vector_t(); + } +} + +/** + * Initializes the dual variable. The initialization of dual variables follows IPOPT option `bound_mult_init_method`: `mu-based` + * (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Initialization) with additoinal options. + * + * @param slack: The slack variable. + * @param barrierParam: The barrier parameter of the IPM. Must be positive. + * @param initialDualLowerBound : Lower bound of the initial dual variables. + * @param initialDualMarginRate : Additional margin rate of the initial dual variables. + * @return Initialized dual variable. + */ +inline vector_t initializeDualVariable(const vector_t& slack, scalar_t barrierParam, scalar_t initialDualLowerBound, + scalar_t initialDualMarginRate) { + if (slack.size() > 0) { + return (1.0 + initialDualMarginRate) * (barrierParam * slack.cwiseInverse()).cwiseMax(initialDualLowerBound); + } else { + return vector_t(); + } +} + +/** + * Initializes the slack variable at a single intermediate node. + * + * @param ocpDefinition: Definition of the optimal control problem. + * @param time : Time of this node + * @param state : State + * @param input : Input + * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. + * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + * @return Initialized slack variables of the intermediate state-only (first) and state-input (second) constraints. + */ +std::pair initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, + const vector_t& state, const vector_t& input, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); + +/** + * Initializes the slack variable at the terminal node. + * + * @param ocpDefinition: Definition of the optimal control problem. + * @param time : Time at the terminal node + * @param state : Terminal state + * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. + * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + * @return Initialized slack variable of the terminal state-only constraints. + */ +vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); + +/** + * Initializes the slack variable at an event node. + * + * @param ocpDefinition: Definition of the optimal control problem. + * @param time : Time at the event node + * @param state : Pre-event state + * @param initialSlackLowerBound : Lower bound of the initial slack variables. Corresponds to `slack_bound_push` option of IPOPT. + * @param initialSlackMarginRate : Additional margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + * @return Initialized slack variable of the pre-jump state-only constraints. + */ +vector_t initializeEventSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate); + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmMpc.h b/ocs2_ipm/include/ocs2_ipm/IpmMpc.h new file mode 100644 index 000000000..f5a5ada10 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmMpc.h @@ -0,0 +1,71 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include "ocs2_ipm/IpmSolver.h" + +namespace ocs2 { + +class IpmMpc final : public MPC_BASE { + public: + /** + * Constructor + * + * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use + * multiple shooting IPM settings directly. + * @param settings : settings for the multiple shooting IPM solver. + * @param [in] optimalControlProblem: The optimal control problem formulation. + * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. + */ + IpmMpc(mpc::Settings mpcSettings, ipm::Settings settings, const OptimalControlProblem& optimalControlProblem, + const Initializer& initializer) + : MPC_BASE(std::move(mpcSettings)), + solverPtr_(std::make_unique(std::move(settings), optimalControlProblem, initializer)) {} + + ~IpmMpc() override = default; + + IpmSolver* getSolverPtr() override { return solverPtr_.get(); } + const IpmSolver* getSolverPtr() const override { return solverPtr_.get(); } + + protected: + void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override { + if (settings().coldStart_) { + solverPtr_->reset(); + } + solverPtr_->run(initTime, initState, finalTime); + } + + private: + std::unique_ptr solverPtr_; +}; + +} // namespace ocs2 diff --git a/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h b/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h new file mode 100644 index 000000000..e36d6d6b5 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmPerformanceIndexComputation.h @@ -0,0 +1,152 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +namespace ocs2 { +namespace ipm { + +/** + * Compute the performance index from the transcription for a single intermediate node. + * + * @param transcription : transcription computed by "multiple_shooting::setupIntermediateNode" + * @param dt : Duration of the interval + * @param barrierParam : Barrier parameter of the interior point method + * @param slackStateIneq : Slack variable of the state inequality constraints + * @param slackStateInputIneq : Slack variable of the state-input inequality constraints + * @return Performance index for a single intermediate node + */ +PerformanceIndex computePerformanceIndex(const multiple_shooting::Transcription& transcription, scalar_t dt, scalar_t barrierParam, + const vector_t& slackStateIneq, const vector_t& slackStateInputIneq); + +/** + * Compute the performance index from the transcription for the terminal node. + * + * @param transcription : transcription computed by "multiple_shooting::setupTerminalNode" + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index for the terminal node + */ +PerformanceIndex computePerformanceIndex(const multiple_shooting::TerminalTranscription& transcription, scalar_t barrierParam, + const vector_t& slackIneq); + +/** + * Compute the performance index from the transcription for the event node. + * + * @param transcription : transcription computed by "multiple_shooting::setupEventNode" + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index for the event node + */ +PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscription& transcription, scalar_t barrierParam, + const vector_t& slackIneq); + +/** + * Compute only the performance index for a single intermediate node. + * Corresponds to the performance index computed from the multiple_shooting::Transcription returned by + * "multiple_shooting::setupIntermediateNode" + * + * @param optimalControlProblem : Definition of the optimal control problem + * @param discretizer : Integrator to use for creating the discrete dynamics + * @param t : Start of the discrete interval + * @param dt : Duration of the interval + * @param x : State at start of the interval + * @param x_next : State at the end of the interval + * @param u : Input, taken to be constant across the interval + * @param barrierParam : Barrier parameter of the interior point method + * @param slackStateIneq : Slack variable of the state inequality constraints + * @param slackStateInputIneq : Slack variable of the state-input inequality constraints + * @return Performance index for a single intermediate node + */ +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + scalar_t barrierParam, const vector_t& slackStateIneq, const vector_t& slackStateInputIneq, + bool enableStateInequalityConstraints); + +/** + * Compute only the performance index for the terminal node. + * Corresponds to the performance index computed from multiple_shooting::TerminalTranscription returned by + * "multiple_shooting::setTerminalNode" + * + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the terminal node + * @param x : Terminal state + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index for the terminal node + */ +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + scalar_t barrierParam, const vector_t& slackIneq); + +/** + * Compute only the performance index for the event node. + * Corresponds to the performance index computed from multiple_shooting::EventTranscription returned by + * "multiple_shooting::setEventNode" + * + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the event node + * @param x : Pre-event state + * @param x_next : Post-event state + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index for the event node + */ +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next, scalar_t barrierParam, const vector_t& slackIneq); + +/** + * Computes the PerformanceIndex of the interior point method based on a given Metrics at an intermediate node. + * + * @param metrics : Metrics at an intermediate node + * @param dt : Duration of the interval + * @param barrierParam : Barrier parameter of the interior point method + * @param slackStateIneq : Slack variable of the state inequality constraints + * @param slackStateInputIneq : Slack variable of the state-input inequality constraints + * @return Performance index of the interior point method + */ +PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_t barrierParam, const vector_t& slackStateIneq, + const vector_t& slackStateInputIneq); + +/** + * Computes the PerformanceIndex of the interior point method based on a given Metrics at the event or terminal node. + * + * @param metrics : Metrics at the event or terminal node + * @param barrierParam : Barrier parameter of the interior point method + * @param slackIneq : Slack variable of the inequality constraints + * @return Performance index of the interior point method + */ +PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t barrierParam, const vector_t& slackIneq); + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSettings.h b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h new file mode 100644 index 000000000..6a917cf50 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmSettings.h @@ -0,0 +1,111 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +#include + +namespace ocs2 { +namespace ipm { + +struct Settings { + // Ipm settings + size_t ipmIteration = 10; // Maximum number of IPM iterations + scalar_t deltaTol = 1e-6; // Termination condition : RMS update of x(t) and u(t) are both below this value + scalar_t costTol = 1e-4; // Termination condition : (cost{i+1} - (cost{i}) < costTol AND constraints{i+1} < g_min + + // Linesearch - step size rules + scalar_t alpha_decay = 0.5; // multiply the step size by this factor every time a linesearch step is rejected. + scalar_t alpha_min = 1e-4; // terminate linesearch if the attempted step size is below this threshold + + // Linesearch - step acceptance criteria with c = costs, g = the norm of constraint violation, and w = [x; u] + scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} + scalar_t g_min = 1e-6; // (2): ELSE IF (g{i} < g_min AND g{i+1} < g_min AND dc/dw'{i} * delta_w < 0) REQUIRE armijo condition + scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * dc/dw'{i} * delta_w + scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} + + // controller type + bool useFeedbackPolicy = true; // true to use feedback, false to use feedforward + bool createValueFunction = false; // true to store the value function, false to ignore it + bool computeLagrangeMultipliers = false; // If set to true to compute the Lagrange multipliers. If set to false the dualFeasibilitiesSSE + // in the PerformanceIndex log is incorrect but it will not affect algorithm correctness. + + // QP subproblem solver settings + hpipm_interface::Settings hpipmSettings = hpipm_interface::Settings(); + + // Discretization method + scalar_t dt = 0.01; // user-defined time discretization + SensitivityIntegratorType integratorType = SensitivityIntegratorType::RK2; + + // Barrier strategy of the primal-dual interior point method. Conventions follows Ipopt. + scalar_t initialBarrierParameter = 1.0e-02; // Initial value of the barrier parameter + scalar_t targetBarrierParameter = 1.0e-04; // Targer value of the barrier parameter. The barreir will decrease until reaches this value. + scalar_t barrierReductionCostTol = 1.0e-02; // Barrier reduction condition : (cost{i+1} - (cost{i}) < costTol + scalar_t barrierReductionConstraintTol = 1.0e-02; // Barrier reduction condition : Constraint violations below this value + scalar_t barrierLinearDecreaseFactor = 0.2; // Linear decrease factor of the barrier parameter, i.e., mu <- mu * factor. + scalar_t barrierSuperlinearDecreasePower = 1.5; // Superlinear decrease factor of the barrier parameter, i.e., mu <- mu ^ factor + + // Initialization of the interior point method. Follows the initialization method of IPOPT + // (https://coin-or.github.io/Ipopt/OPTIONS.html#OPT_Initialization). + scalar_t initialSlackLowerBound = + 1.0e-04; // Minimum value of the initial slack variable. Corresponds to `slack_bound_push` option of IPOPT. + scalar_t initialDualLowerBound = 1.0e-04; // Minimum value of the initial dual variable. + scalar_t initialSlackMarginRate = 0.01; // Margin rate of the initial slack variables. Corresponds to `slack_bound_frac` option of IPOPT. + scalar_t initialDualMarginRate = 0.01; // Margin rate of the initial dual variables. + + // Linesearch for the interior point method. + scalar_t fractionToBoundaryMargin = + 0.995; // Margin of the fraction-to-boundary-rule for the step size selection. Correcponds to `tau_min` option of IPOPT. + bool usePrimalStepSizeForDual = true; // If true, the primal step size is also used as the dual step size. + + // Printing + bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem + bool printSolverStatistics = false; // Print benchmarking of the multiple shooting method + bool printLinesearch = false; // Print linesearch information + + // Threading + size_t nThreads = 4; + int threadPriority = 50; +}; + +/** + * Loads the multiple shooting IPM settings from a given file. + * + * @param [in] filename: File name which contains the configuration data. + * @param [in] fieldName: Field name which contains the configuration data. + * @param [in] verbose: Flag to determine whether to print out the loaded settings or not. + * @return The settings + */ +Settings loadSettings(const std::string& filename, const std::string& fieldName = "ipm", bool verbose = true); + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolver.h b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h new file mode 100644 index 000000000..56e7e282b --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolver.h @@ -0,0 +1,240 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +#include "ocs2_ipm/IpmSettings.h" +#include "ocs2_ipm/IpmSolverStatus.h" + +namespace ocs2 { + +class IpmSolver : public SolverBase { + public: + /** + * Constructor + * + * @param settings : settings for the multiple shooting IPM solver. + * @param [in] optimalControlProblem: The optimal control problem formulation. + * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. + */ + IpmSolver(ipm::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); + + ~IpmSolver() override; + + void reset() override; + + scalar_t getFinalTime() const override { return primalSolution_.timeTrajectory_.back(); }; + + void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } + + const DualSolution* getDualSolution() const override { return &dualIneqTrajectory_; } + + const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; } + + size_t getNumIterations() const override { return totalNumIterations_; } + + const OptimalControlProblem& getOptimalControlProblem() const override { return ocpDefinitions_.front(); } + + const PerformanceIndex& getPerformanceIndeces() const override { return getIterationsLog().back(); }; + + const std::vector& getIterationsLog() const override; + + ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override; + + ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) override { + throw std::runtime_error("[IpmSolver] getHamiltonian() not available yet."); + } + + vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override; + + MultiplierCollection getIntermediateDualSolution(scalar_t time) const override; + + private: + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override; + + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const ControllerBase* externalControllerPtr) override { + if (externalControllerPtr == nullptr) { + runImpl(initTime, initState, finalTime); + } else { + throw std::runtime_error("[IpmSolver::run] This solver does not support external controller!"); + } + } + + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) override { + // Copy all except the controller + primalSolution_.timeTrajectory_ = primalSolution.timeTrajectory_; + primalSolution_.stateTrajectory_ = primalSolution.stateTrajectory_; + primalSolution_.inputTrajectory_ = primalSolution.inputTrajectory_; + primalSolution_.postEventIndices_ = primalSolution.postEventIndices_; + primalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + runImpl(initTime, initState, finalTime); + } + + /** Run a task in parallel with settings.nThreads */ + void runParallel(std::function taskFunction); + + /** Get profiling information as a string */ + std::string getBenchmarkingInformation() const; + + /** Initializes for the costate trajectories */ + void initializeCostateTrajectory(const std::vector& timeDiscretization, const vector_array_t& stateTrajectory, + vector_array_t& costateTrajectory) const; + + /** Initializes for the Lagrange multiplier trajectories of the constraint projection */ + void initializeProjectionMultiplierTrajectory(const std::vector& timeDiscretization, + vector_array_t& projectionMultiplierTrajectory) const; + + /** Initializes for the slack and dual trajectories of the hard inequality constraints */ + void initializeSlackDualTrajectory(const std::vector& timeDiscretization, const vector_array_t& x, const vector_array_t& u, + scalar_t barrierParam, vector_array_t& slackStateIneq, vector_array_t& dualStateIneq, + vector_array_t& slackStateInputIneq, vector_array_t& dualStateInputIneq); + + /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ + PerformanceIndex setupQuadraticSubproblem(const std::vector& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, const vector_array_t& lmd, const vector_array_t& nu, + scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateIneq, + const vector_array_t& dualStateInputIneq, std::vector& metrics); + + /** Computes only the performance metrics at the current {t, x(t), u(t)} */ + PerformanceIndex computePerformance(const std::vector& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, std::vector& metrics); + + /** Returns solution of the QP subproblem in delta coordinates: */ + struct OcpSubproblemSolution { + vector_array_t deltaXSol; // delta_x(t) + vector_array_t deltaUSol; // delta_u(t) + vector_array_t deltaLmdSol; // delta_lmd(t) + vector_array_t deltaNuSol; // delta_nu(t) + vector_array_t deltaSlackStateIneq; + vector_array_t deltaDualStateIneq; + vector_array_t deltaSlackStateInputIneq; + vector_array_t deltaDualStateInputIneq; + scalar_t armijoDescentMetric; // inner product of the cost gradient and decision variable step + scalar_t maxPrimalStepSize; + scalar_t maxDualStepSize; + }; + OcpSubproblemSolution getOCPSolution(const vector_t& delta_x0, scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& dualStateIneq, const vector_array_t& slackStateInputIneq, + const vector_array_t& dualStateInputIneq); + + /** Extract the value function based on the last solved QP */ + void extractValueFunction(const std::vector& time, const vector_array_t& x, const vector_array_t& lmd, + const vector_array_t& deltaXSol); + + /** Constructs the primal solution based on the optimized state and input trajectories */ + PrimalSolution toPrimalSolution(const std::vector& time, vector_array_t&& x, vector_array_t&& u); + + /** Decides on the step to take and overrides given trajectories {x(t), u(t), slackStateIneq(t), slackStateInputIneq(t)} + * <- {x(t) + a*dx(t), u(t) + a*du(t), slackStateIneq(t) + a*dslackStateIneq(t), slackStateInputIneq(t) + a*dslackStateInputIneq(t)} */ + ipm::StepInfo takePrimalStep(const PerformanceIndex& baseline, const std::vector& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq, std::vector& metrics); + + /** Updates the Lagrange multipliers */ + void takeDualStep(const OcpSubproblemSolution& subproblemSolution, const ipm::StepInfo& stepInfo, vector_array_t& lmd, vector_array_t& nu, + vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq) const; + + /** Updates the barrier parameter */ + scalar_t updateBarrierParameter(scalar_t currentBarrierParameter, const PerformanceIndex& baseline, const ipm::StepInfo& stepInfo) const; + + /** Determine convergence after a step */ + ipm::Convergence checkConvergence(int iteration, scalar_t barrierParam, const PerformanceIndex& baseline, + const ipm::StepInfo& stepInfo) const; + + // Problem definition + const ipm::Settings settings_; + DynamicsDiscretizer discretizer_; + DynamicsSensitivityDiscretizer sensitivityDiscretizer_; + std::vector ocpDefinitions_; + std::unique_ptr initializerPtr_; + FilterLinesearch filterLinesearch_; + + // Solver interface + HpipmInterface hpipmInterface_; + + // Threading + ThreadPool threadPool_; + + // Solution + PrimalSolution primalSolution_; + vector_array_t costateTrajectory_; + vector_array_t projectionMultiplierTrajectory_; + DualSolution slackIneqTrajectory_; + DualSolution dualIneqTrajectory_; + + // Value function in absolute state coordinates (without the constant value) + std::vector valueFunction_; + + // LQ approximation + std::vector lagrangian_; + std::vector dynamics_; + std::vector stateInputEqConstraints_; + std::vector stateIneqConstraints_; + std::vector stateInputIneqConstraints_; + std::vector constraintsProjection_; + + // Constraint terms size + std::vector constraintsSize_; + + // Lagrange multipliers + std::vector projectionMultiplierCoefficients_; + + // Iteration performance log + std::vector performanceIndeces_; + + // The ProblemMetrics associated to primalSolution_ + ProblemMetrics problemMetrics_; + + // Benchmarking + size_t totalNumIterations_{0}; + benchmark::RepeatedTimer initializationTimer_; + benchmark::RepeatedTimer linearQuadraticApproximationTimer_; + benchmark::RepeatedTimer solveQpTimer_; + benchmark::RepeatedTimer linesearchTimer_; + benchmark::RepeatedTimer computeControllerTimer_; +}; + +} // namespace ocs2 diff --git a/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h b/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h new file mode 100644 index 000000000..42a5f8a47 --- /dev/null +++ b/ocs2_ipm/include/ocs2_ipm/IpmSolverStatus.h @@ -0,0 +1,78 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include +#include +#include + +namespace ocs2 { +namespace ipm { + +/** Different types of convergence */ +enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; + +/** Struct to contain the result and logging data of the stepsize computation */ +struct StepInfo { + // Step sizes and type + scalar_t primalStepSize = 0.0; + scalar_t dualStepSize = 0.0; + FilterLinesearch::StepType stepType = FilterLinesearch::StepType::UNKNOWN; + + // Step in primal variables + scalar_t dx_norm = 0.0; // norm of the state trajectory update + scalar_t du_norm = 0.0; // norm of the input trajectory update + + // Performance result after the step + PerformanceIndex performanceAfterStep; + scalar_t totalConstraintViolationAfterStep; // constraint metric used in the line search +}; + +/** Transforms ipm::Convergence to string */ +inline std::string toString(const Convergence& convergence) { + switch (convergence) { + case Convergence::ITERATIONS: + return "Maximum number of iterations reached"; + case Convergence::STEPSIZE: + return "Step size below minimum"; + case Convergence::METRICS: + return "Cost decrease and constraint satisfaction below tolerance"; + case Convergence::PRIMAL: + return "Primal update below tolerance"; + case Convergence::FALSE: + default: + return "Not Converged"; + } +} + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/package.xml b/ocs2_ipm/package.xml new file mode 100644 index 000000000..9cb5787b9 --- /dev/null +++ b/ocs2_ipm/package.xml @@ -0,0 +1,19 @@ + + + ocs2_ipm + 0.0.0 + The ocs2_ipm package + + Sotaro Katayama + + BSD-3 + + catkin + ocs2_core + ocs2_mpc + ocs2_oc + blasfeo_catkin + hpipm_catkin + ocs2_qp_solver + + diff --git a/ocs2_ipm/src/IpmHelpers.cpp b/ocs2_ipm/src/IpmHelpers.cpp new file mode 100644 index 000000000..f7ebcc812 --- /dev/null +++ b/ocs2_ipm/src/IpmHelpers.cpp @@ -0,0 +1,184 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_ipm/IpmHelpers.h" + +#include + +namespace ocs2 { +namespace ipm { + +void condenseIneqConstraints(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, + const VectorFunctionLinearApproximation& ineqConstraint, ScalarFunctionQuadraticApproximation& lagrangian) { + assert(barrierParam > 0.0); + const size_t nc = ineqConstraint.f.size(); + const size_t nu = ineqConstraint.dfdu.cols(); + + if (nc == 0) { + return; + } + + // dual feasibilities + lagrangian.dfdx.noalias() -= ineqConstraint.dfdx.transpose() * dual; + if (ineqConstraint.dfdu.cols() > 0) { + lagrangian.dfdu.noalias() -= ineqConstraint.dfdu.transpose() * dual; + } + + // coefficients for condensing + const vector_t condensingLinearCoeff = (dual.array() * ineqConstraint.f.array() - barrierParam) / slack.array(); + const vector_t condensingQuadraticCoeff = dual.cwiseQuotient(slack); + + // condensing + lagrangian.dfdx.noalias() += ineqConstraint.dfdx.transpose() * condensingLinearCoeff; + const matrix_t condensingQuadraticCoeff_dfdx = condensingQuadraticCoeff.asDiagonal() * ineqConstraint.dfdx; + lagrangian.dfdxx.noalias() += ineqConstraint.dfdx.transpose() * condensingQuadraticCoeff_dfdx; + + if (nu > 0) { + lagrangian.dfdu.noalias() += ineqConstraint.dfdu.transpose() * condensingLinearCoeff; + const matrix_t condensingQuadraticCoeff_dfdu = condensingQuadraticCoeff.asDiagonal() * ineqConstraint.dfdu; + lagrangian.dfduu.noalias() += ineqConstraint.dfdu.transpose() * condensingQuadraticCoeff_dfdu; + lagrangian.dfdux.noalias() += ineqConstraint.dfdu.transpose() * condensingQuadraticCoeff_dfdx; + } +} + +vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateInputIneqConstraints, const vector_t& dx, const vector_t& du, + scalar_t barrierParam, const vector_t& slackStateInputIneq) { + assert(barrierParam > 0.0); + if (stateInputIneqConstraints.f.size() == 0) { + return vector_t(); + } + + vector_t slackDirection = stateInputIneqConstraints.f - slackStateInputIneq; + slackDirection.noalias() += stateInputIneqConstraints.dfdx * dx; + slackDirection.noalias() += stateInputIneqConstraints.dfdu * du; + return slackDirection; +} + +vector_t retrieveSlackDirection(const VectorFunctionLinearApproximation& stateIneqConstraints, const vector_t& dx, scalar_t barrierParam, + const vector_t& slackStateIneq) { + assert(barrierParam > 0.0); + if (stateIneqConstraints.f.size() == 0) { + return vector_t(); + } + + vector_t slackDirection = stateIneqConstraints.f - slackStateIneq; + slackDirection.noalias() += stateIneqConstraints.dfdx * dx; + return slackDirection; +} + +vector_t retrieveDualDirection(scalar_t barrierParam, const vector_t& slack, const vector_t& dual, const vector_t& slackDirection) { + assert(barrierParam > 0.0); + vector_t dualDirection = dual.cwiseProduct(slack + slackDirection); + dualDirection.array() -= barrierParam; + dualDirection.array() /= -slack.array(); + return dualDirection; +} + +scalar_t fractionToBoundaryStepSize(const vector_t& v, const vector_t& dv, scalar_t marginRate) { + assert(marginRate > 0.0); + assert(marginRate <= 1.0); + + if (v.size() == 0) { + return 1.0; + } + + const vector_t invFractionToBoundary = (-1.0 / marginRate) * dv.cwiseQuotient(v); + const auto alpha = invFractionToBoundary.maxCoeff(); + return alpha > 0.0 ? std::min(1.0 / alpha, 1.0) : 1.0; +} + +namespace { +MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& stateIneq) { + MultiplierCollection multiplierCollection; + size_t head = 0; + for (const size_t size : constraintsSize.stateIneq) { + multiplierCollection.stateIneq.emplace_back(0.0, stateIneq.segment(head, size)); + head += size; + } + return multiplierCollection; +} + +MultiplierCollection toMultiplierCollection(const multiple_shooting::ConstraintsSize constraintsSize, const vector_t& stateIneq, + const vector_t& stateInputIneq) { + MultiplierCollection multiplierCollection = toMultiplierCollection(constraintsSize, stateIneq); + size_t head = 0; + for (const size_t size : constraintsSize.stateInputIneq) { + multiplierCollection.stateInputIneq.emplace_back(0.0, stateInputIneq.segment(head, size)); + head += size; + } + return multiplierCollection; +} + +vector_t extractLagrangian(const std::vector& termsMultiplier) { + size_t n = 0; + std::for_each(termsMultiplier.begin(), termsMultiplier.end(), [&](const Multiplier& m) { n += m.lagrangian.size(); }); + + vector_t vec(n); + size_t head = 0; + for (const auto& m : termsMultiplier) { + vec.segment(head, m.lagrangian.size()) = m.lagrangian; + head += m.lagrangian.size(); + } // end of i loop + + return vec; +} +} // namespace + +DualSolution toDualSolution(const std::vector& time, const std::vector& constraintsSize, + const vector_array_t& stateIneq, const vector_array_t& stateInputIneq) { + // Problem horizon + const int N = static_cast(time.size()) - 1; + + DualSolution dualSolution; + dualSolution.timeTrajectory = toInterpolationTime(time); + dualSolution.postEventIndices = toPostEventIndices(time); + + dualSolution.preJumps.reserve(dualSolution.postEventIndices.size()); + dualSolution.intermediates.reserve(time.size()); + + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + dualSolution.preJumps.emplace_back(toMultiplierCollection(constraintsSize[i], stateIneq[i])); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); // no event at the initial node + } else { + dualSolution.intermediates.emplace_back(toMultiplierCollection(constraintsSize[i], stateIneq[i], stateInputIneq[i])); + } + } + dualSolution.final = toMultiplierCollection(constraintsSize[N], stateIneq[N]); + dualSolution.intermediates.push_back(dualSolution.intermediates.back()); + + return dualSolution; +} + +std::pair fromMultiplierCollection(const MultiplierCollection& multiplierCollection) { + return std::make_pair(extractLagrangian(multiplierCollection.stateIneq), extractLagrangian(multiplierCollection.stateInputIneq)); +} + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/src/IpmInitialization.cpp b/ocs2_ipm/src/IpmInitialization.cpp new file mode 100644 index 000000000..33f8b7aa1 --- /dev/null +++ b/ocs2_ipm/src/IpmInitialization.cpp @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_ipm/IpmInitialization.h" + +namespace ocs2 { +namespace ipm { + +std::pair initializeIntermediateSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, + const vector_t& state, const vector_t& input, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + vector_t slackStateIneq, slackStateInputIneq; + + if (!ocpDefinition.stateInequalityConstraintPtr->empty() || !ocpDefinition.inequalityConstraintPtr->empty()) { + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->request(request, time, state, input); + } + + if (!ocpDefinition.stateInequalityConstraintPtr->empty()) { + const auto ineqConstraint = + toVector(ocpDefinition.stateInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + slackStateIneq = initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); + } + + if (!ocpDefinition.inequalityConstraintPtr->empty()) { + const auto ineqConstraint = + toVector(ocpDefinition.inequalityConstraintPtr->getValue(time, state, input, *ocpDefinition.preComputationPtr)); + slackStateInputIneq = initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); + } + + return std::make_pair(std::move(slackStateIneq), std::move(slackStateInputIneq)); +} + +vector_t initializeTerminalSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + if (ocpDefinition.finalInequalityConstraintPtr->empty()) { + return vector_t(); + } + + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestFinal(request, time, state); + const auto ineqConstraint = toVector(ocpDefinition.finalInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); +} + +vector_t initializeEventSlackVariable(OptimalControlProblem& ocpDefinition, scalar_t time, const vector_t& state, + scalar_t initialSlackLowerBound, scalar_t initialSlackMarginRate) { + if (ocpDefinition.preJumpInequalityConstraintPtr->empty()) { + return vector_t(); + } + + constexpr auto request = Request::Constraint; + ocpDefinition.preComputationPtr->requestPreJump(request, time, state); + const auto ineqConstraint = + toVector(ocpDefinition.preJumpInequalityConstraintPtr->getValue(time, state, *ocpDefinition.preComputationPtr)); + return initializeSlackVariable(ineqConstraint, initialSlackLowerBound, initialSlackMarginRate); +} + +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp b/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp new file mode 100644 index 000000000..0e727c56a --- /dev/null +++ b/ocs2_ipm/src/IpmPerformanceIndexComputation.cpp @@ -0,0 +1,144 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_ipm/IpmPerformanceIndexComputation.h" + +#include + +#include +#include +#include + +namespace ocs2 { +namespace ipm { + +PerformanceIndex computePerformanceIndex(const multiple_shooting::Transcription& transcription, scalar_t dt, scalar_t barrierParam, + const vector_t& slackStateIneq, const vector_t& slackStateInputIneq) { + auto performance = multiple_shooting::computePerformanceIndex(transcription, dt); + + if (slackStateIneq.size() > 0) { + performance.cost -= dt * barrierParam * slackStateIneq.array().log().sum(); + } + if (slackStateInputIneq.size() > 0) { + performance.cost -= dt * barrierParam * slackStateInputIneq.array().log().sum(); + } + + if (transcription.stateIneqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE += dt * (transcription.stateIneqConstraints.f - slackStateIneq).squaredNorm(); + } + if (transcription.stateInputIneqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE += dt * (transcription.stateInputIneqConstraints.f - slackStateInputIneq).squaredNorm(); + } + + return performance; +} + +PerformanceIndex computePerformanceIndex(const multiple_shooting::TerminalTranscription& transcription, scalar_t barrierParam, + const vector_t& slackIneq) { + auto performance = multiple_shooting::computePerformanceIndex(transcription); + + if (slackIneq.size() > 0) { + performance.cost -= barrierParam * slackIneq.array().log().sum(); + } + + if (transcription.ineqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE += (transcription.ineqConstraints.f - slackIneq).squaredNorm(); + } + + return performance; +} + +PerformanceIndex computePerformanceIndex(const multiple_shooting::EventTranscription& transcription, scalar_t barrierParam, + const vector_t& slackIneq) { + auto performance = multiple_shooting::computePerformanceIndex(transcription); + + if (slackIneq.size() > 0) { + performance.cost -= barrierParam * slackIneq.array().log().sum(); + } + + if (transcription.ineqConstraints.f.size() > 0) { + performance.equalityConstraintsSSE += (transcription.ineqConstraints.f - slackIneq).squaredNorm(); + } + + return performance; +} + +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u, + scalar_t barrierParam, const vector_t& slackStateIneq, const vector_t& slackStateInputIneq, + bool enableStateInequalityConstraints) { + auto metrics = multiple_shooting::computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); + if (!enableStateInequalityConstraints) { + metrics.stateIneqConstraint.clear(); + } + return toPerformanceIndex(metrics, dt, barrierParam, slackStateIneq, slackStateInputIneq); +} + +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next, scalar_t barrierParam, const vector_t& slackIneq) { + const auto metrics = multiple_shooting::computeEventMetrics(optimalControlProblem, t, x, x_next); + return toPerformanceIndex(metrics, barrierParam, slackIneq); +} + +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + scalar_t barrierParam, const vector_t& slackIneq) { + const auto metrics = multiple_shooting::computeTerminalMetrics(optimalControlProblem, t, x); + return toPerformanceIndex(metrics, barrierParam, slackIneq); +} + +PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t dt, scalar_t barrierParam, const vector_t& slackStateIneq, + const vector_t& slackStateInputIneq) { + PerformanceIndex performance = toPerformanceIndex(metrics, dt); + + if (slackStateIneq.size() > 0) { + performance.cost -= dt * barrierParam * slackStateIneq.array().log().sum(); + performance.equalityConstraintsSSE += dt * (toVector(metrics.stateIneqConstraint) - slackStateIneq).squaredNorm(); + } + + if (slackStateInputIneq.size() > 0) { + performance.cost -= dt * barrierParam * slackStateInputIneq.array().log().sum(); + performance.equalityConstraintsSSE += dt * (toVector(metrics.stateInputIneqConstraint) - slackStateInputIneq).squaredNorm(); + } + + return performance; +} + +PerformanceIndex toPerformanceIndex(const Metrics& metrics, scalar_t barrierParam, const vector_t& slackIneq) { + PerformanceIndex performance = toPerformanceIndex(metrics); + + if (slackIneq.size() > 0) { + performance.cost -= barrierParam * slackIneq.array().log().sum(); + performance.equalityConstraintsSSE += (toVector(metrics.stateIneqConstraint) - slackIneq).squaredNorm(); + } + + return performance; +} + +} // namespace ipm +} // namespace ocs2 diff --git a/ocs2_ipm/src/IpmSettings.cpp b/ocs2_ipm/src/IpmSettings.cpp new file mode 100644 index 000000000..f99636ea0 --- /dev/null +++ b/ocs2_ipm/src/IpmSettings.cpp @@ -0,0 +1,111 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_ipm/IpmSettings.h" + +#include +#include + +#include + +#include + +namespace ocs2 { +namespace ipm { + +Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(filename, pt); + + Settings settings; + + if (verbose) { + std::cerr << "\n #### Multiple-Shooting IPM Settings:"; + std::cerr << "\n #### =============================================================================\n"; + } + + loadData::loadPtreeValue(pt, settings.ipmIteration, fieldName + ".ipmIteration", verbose); + loadData::loadPtreeValue(pt, settings.deltaTol, fieldName + ".deltaTol", verbose); + loadData::loadPtreeValue(pt, settings.alpha_decay, fieldName + ".alpha_decay", verbose); + loadData::loadPtreeValue(pt, settings.alpha_min, fieldName + ".alpha_min", verbose); + loadData::loadPtreeValue(pt, settings.gamma_c, fieldName + ".gamma_c", verbose); + loadData::loadPtreeValue(pt, settings.g_max, fieldName + ".g_max", verbose); + loadData::loadPtreeValue(pt, settings.g_min, fieldName + ".g_min", verbose); + loadData::loadPtreeValue(pt, settings.armijoFactor, fieldName + ".armijoFactor", verbose); + loadData::loadPtreeValue(pt, settings.costTol, fieldName + ".costTol", verbose); + loadData::loadPtreeValue(pt, settings.dt, fieldName + ".dt", verbose); + loadData::loadPtreeValue(pt, settings.useFeedbackPolicy, fieldName + ".useFeedbackPolicy", verbose); + loadData::loadPtreeValue(pt, settings.createValueFunction, fieldName + ".createValueFunction", verbose); + loadData::loadPtreeValue(pt, settings.computeLagrangeMultipliers, fieldName + ".computeLagrangeMultipliers", verbose); + auto integratorName = sensitivity_integrator::toString(settings.integratorType); + loadData::loadPtreeValue(pt, integratorName, fieldName + ".integratorType", verbose); + settings.integratorType = sensitivity_integrator::fromString(integratorName); + loadData::loadPtreeValue(pt, settings.initialBarrierParameter, fieldName + ".initialBarrierParameter", verbose); + loadData::loadPtreeValue(pt, settings.targetBarrierParameter, fieldName + ".targetBarrierParameter", verbose); + loadData::loadPtreeValue(pt, settings.barrierReductionCostTol, fieldName + ".barrierReductionCostTol ", verbose); + loadData::loadPtreeValue(pt, settings.barrierReductionConstraintTol, fieldName + ".barrierReductionConstraintTol", verbose); + loadData::loadPtreeValue(pt, settings.barrierLinearDecreaseFactor, fieldName + ".barrierLinearDecreaseFactor", verbose); + loadData::loadPtreeValue(pt, settings.barrierSuperlinearDecreasePower, fieldName + ".barrierSuperlinearDecreasePower", verbose); + loadData::loadPtreeValue(pt, settings.fractionToBoundaryMargin, fieldName + ".fractionToBoundaryMargin", verbose); + loadData::loadPtreeValue(pt, settings.usePrimalStepSizeForDual, fieldName + ".usePrimalStepSizeForDual", verbose); + loadData::loadPtreeValue(pt, settings.initialSlackLowerBound, fieldName + ".initialSlackLowerBound", verbose); + loadData::loadPtreeValue(pt, settings.initialDualLowerBound, fieldName + ".initialDualLowerBound", verbose); + loadData::loadPtreeValue(pt, settings.initialSlackMarginRate, fieldName + ".initialSlackMarginRate", verbose); + loadData::loadPtreeValue(pt, settings.initialDualMarginRate, fieldName + ".initialDualMarginRate", verbose); + loadData::loadPtreeValue(pt, settings.printSolverStatus, fieldName + ".printSolverStatus", verbose); + loadData::loadPtreeValue(pt, settings.printSolverStatistics, fieldName + ".printSolverStatistics", verbose); + loadData::loadPtreeValue(pt, settings.printLinesearch, fieldName + ".printLinesearch", verbose); + loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); + loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); + + if (settings.initialSlackLowerBound <= 0.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] initialSlackLowerBound must be positive!"); + } + if (settings.initialDualLowerBound <= 0.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] initialDualLowerBound must be positive!"); + } + if (settings.initialSlackMarginRate < 0.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] initialSlackMarginRate must be non-negative!"); + } + if (settings.initialDualMarginRate < 0.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] initialDualMarginRate must be non-negative!"); + } + if (settings.fractionToBoundaryMargin <= 0.0 || settings.fractionToBoundaryMargin > 1.0) { + throw std::runtime_error("[MultipleShootingIpmSettings] fractionToBoundaryMargin must be positive and no more than 1.0!"); + } + + if (verbose) { + std::cerr << settings.hpipmSettings; + std::cerr << " #### =============================================================================" << std::endl; + } + + return settings; +} +} // namespace ipm +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_ipm/src/IpmSolver.cpp b/ocs2_ipm/src/IpmSolver.cpp new file mode 100644 index 000000000..46ea304b7 --- /dev/null +++ b/ocs2_ipm/src/IpmSolver.cpp @@ -0,0 +1,934 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_ipm/IpmSolver.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ocs2_ipm/IpmHelpers.h" +#include "ocs2_ipm/IpmInitialization.h" +#include "ocs2_ipm/IpmPerformanceIndexComputation.h" + +namespace ocs2 { + +namespace { +ipm::Settings rectifySettings(const OptimalControlProblem& ocp, ipm::Settings&& settings) { + // We have to create the value function if we want to compute the Lagrange multipliers. + if (settings.computeLagrangeMultipliers) { + settings.createValueFunction = true; + } + // Turn off the barrier update strategy if there are no inequality constraints. + if (ocp.inequalityConstraintPtr->empty() && ocp.stateInequalityConstraintPtr->empty() && ocp.preJumpInequalityConstraintPtr->empty() && + ocp.finalInequalityConstraintPtr->empty()) { + settings.targetBarrierParameter = settings.initialBarrierParameter; + } + return settings; +} +} // anonymous namespace + +IpmSolver::IpmSolver(ipm::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) + : settings_(rectifySettings(optimalControlProblem, std::move(settings))), + hpipmInterface_(OcpSize(), settings_.hpipmSettings), + threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) { + Eigen::setNbThreads(1); // No multithreading within Eigen. + Eigen::initParallel(); + + // Dynamics discretization + discretizer_ = selectDynamicsDiscretization(settings_.integratorType); + sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings_.integratorType); + + // Clone objects to have one for each worker + for (int w = 0; w < settings_.nThreads; w++) { + ocpDefinitions_.push_back(optimalControlProblem); + } + + // Operating points + initializerPtr_.reset(initializer.clone()); + + // Linesearch + filterLinesearch_.g_max = settings_.g_max; + filterLinesearch_.g_min = settings_.g_min; + filterLinesearch_.gamma_c = settings_.gamma_c; + filterLinesearch_.armijoFactor = settings_.armijoFactor; +} + +IpmSolver::~IpmSolver() { + if (settings_.printSolverStatistics) { + std::cerr << getBenchmarkingInformation() << std::endl; + } +} + +void IpmSolver::reset() { + // Clear solution + primalSolution_ = PrimalSolution(); + costateTrajectory_.clear(); + projectionMultiplierTrajectory_.clear(); + slackIneqTrajectory_.clear(); + dualIneqTrajectory_.clear(); + valueFunction_.clear(); + performanceIndeces_.clear(); + + // reset timers + totalNumIterations_ = 0; + initializationTimer_.reset(); + linearQuadraticApproximationTimer_.reset(); + solveQpTimer_.reset(); + linesearchTimer_.reset(); + computeControllerTimer_.reset(); +} + +std::string IpmSolver::getBenchmarkingInformation() const { + const auto initializationTotal = initializationTimer_.getTotalInMilliseconds(); + const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); + const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); + const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); + const auto computeControllerTotal = computeControllerTimer_.getTotalInMilliseconds(); + + const auto benchmarkTotal = + initializationTotal + linearQuadraticApproximationTotal + solveQpTotal + linesearchTotal + computeControllerTotal; + + std::stringstream infoStream; + if (benchmarkTotal > 0.0) { + const scalar_t inPercent = 100.0; + infoStream << "\n########################################################################\n"; + infoStream << "The benchmarking is computed over " << totalNumIterations_ << " iterations. \n"; + infoStream << "IPM Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tInitialization :\t" << initializationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << initializationTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tLQ Approximation :\t" << linearQuadraticApproximationTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << linearQuadraticApproximationTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tSolve QP :\t" << solveQpTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << solveQpTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tLinesearch :\t" << linesearchTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << linesearchTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tCompute Controller :\t" << computeControllerTimer_.getAverageInMilliseconds() << " [ms] \t\t(" + << computeControllerTotal / benchmarkTotal * inPercent << "%)\n"; + } + return infoStream.str(); +} + +const std::vector& IpmSolver::getIterationsLog() const { + if (performanceIndeces_.empty()) { + throw std::runtime_error("[IpmSolver]: No performance log yet, no problem solved yet?"); + } else { + return performanceIndeces_; + } +} + +ScalarFunctionQuadraticApproximation IpmSolver::getValueFunction(scalar_t time, const vector_t& state) const { + if (valueFunction_.empty()) { + throw std::runtime_error("[IpmSolver] Value function is empty! Is createValueFunction true and did the solver run?"); + } else { + // Interpolation + const auto indexAlpha = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_); + + ScalarFunctionQuadraticApproximation valueFunction; + using T = std::vector; + using LinearInterpolation::interpolate; + valueFunction.f = 0.0; + valueFunction.dfdx = interpolate(indexAlpha, valueFunction_, [](const T& v, size_t ind) -> const vector_t& { return v[ind].dfdx; }); + valueFunction.dfdxx = interpolate(indexAlpha, valueFunction_, [](const T& v, size_t ind) -> const matrix_t& { return v[ind].dfdxx; }); + + // Re-center around query state + valueFunction.dfdx.noalias() += valueFunction.dfdxx * state; + + return valueFunction; + } +} + +vector_t IpmSolver::getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const { + if (settings_.computeLagrangeMultipliers && !projectionMultiplierTrajectory_.empty()) { + using T = std::vector; + const auto indexAlpha = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_); + + const auto nominalState = LinearInterpolation::interpolate(indexAlpha, primalSolution_.stateTrajectory_); + const auto sensitivityWrtState = LinearInterpolation::interpolate( + indexAlpha, projectionMultiplierCoefficients_, [](const T& v, size_t ind) -> const matrix_t& { return v[ind].dfdx; }); + + auto multiplier = LinearInterpolation::interpolate(indexAlpha, projectionMultiplierTrajectory_); + multiplier.noalias() += sensitivityWrtState * (state - nominalState); + + return multiplier; + + } else { + throw std::runtime_error("[IpmSolver] getStateInputEqualityConstraintLagrangian() not available yet."); + } +} + +MultiplierCollection IpmSolver::getIntermediateDualSolution(scalar_t time) const { + if (!dualIneqTrajectory_.timeTrajectory.empty()) { + return getIntermediateDualSolutionAtTime(dualIneqTrajectory_, time); + } else { + throw std::runtime_error("[IpmSolver] getIntermediateDualSolution() not available yet."); + } +} + +void IpmSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++ IPM solver is initialized ++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + } + + // Determine time discretization, taking into account event times. + const auto& eventTimes = this->getReferenceManager().getModeSchedule().eventTimes; + const auto timeDiscretization = timeDiscretizationWithEvents(initTime, finalTime, settings_.dt, eventTimes); + + // Initialize references + for (auto& ocpDefinition : ocpDefinitions_) { + const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); + ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; + } + + // old and new mode schedules for the trajectory spreading + const auto oldModeSchedule = primalSolution_.modeSchedule_; + const auto& newModeSchedule = this->getReferenceManager().getModeSchedule(); + + initializationTimer_.startTimer(); + // Initialize the state and input + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, primalSolution_); + } + vector_array_t x, u; + multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); + + // Initialize the slack and dual variables of the interior point method + if (!slackIneqTrajectory_.timeTrajectory.empty()) { + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, slackIneqTrajectory_); + std::ignore = trajectorySpread(oldModeSchedule, newModeSchedule, dualIneqTrajectory_); + } + scalar_t barrierParam = settings_.initialBarrierParameter; + vector_array_t slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq; + initializeSlackDualTrajectory(timeDiscretization, x, u, barrierParam, slackStateIneq, dualStateIneq, slackStateInputIneq, + dualStateInputIneq); + + // Initialize the costate and projection multiplier + vector_array_t lmd, nu; + if (settings_.computeLagrangeMultipliers) { + initializeCostateTrajectory(timeDiscretization, x, lmd); + initializeProjectionMultiplierTrajectory(timeDiscretization, nu); + } + initializationTimer_.endTimer(); + + // Bookkeeping + performanceIndeces_.clear(); + std::vector metrics; + + int iter = 0; + ipm::Convergence convergence = ipm::Convergence::FALSE; + while (convergence == ipm::Convergence::FALSE) { + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\nIPM iteration: " << iter << " (barrier parameter: " << barrierParam << ")\n"; + } + + // Make QP approximation + linearQuadraticApproximationTimer_.startTimer(); + const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u, lmd, nu, barrierParam, slackStateIneq, + slackStateInputIneq, dualStateIneq, dualStateInputIneq, metrics); + linearQuadraticApproximationTimer_.endTimer(); + + // Solve QP + solveQpTimer_.startTimer(); + const vector_t delta_x0 = initState - x[0]; + const auto deltaSolution = + getOCPSolution(delta_x0, barrierParam, slackStateIneq, dualStateIneq, slackStateInputIneq, dualStateInputIneq); + extractValueFunction(timeDiscretization, x, lmd, deltaSolution.deltaXSol); + solveQpTimer_.endTimer(); + + // Apply step + linesearchTimer_.startTimer(); + const scalar_t maxPrimalStepSize = settings_.usePrimalStepSizeForDual + ? std::min(deltaSolution.maxDualStepSize, deltaSolution.maxPrimalStepSize) + : deltaSolution.maxPrimalStepSize; + const auto stepInfo = takePrimalStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, barrierParam, + slackStateIneq, slackStateInputIneq, metrics); + takeDualStep(deltaSolution, stepInfo, lmd, nu, dualStateIneq, dualStateInputIneq); + performanceIndeces_.push_back(stepInfo.performanceAfterStep); + linesearchTimer_.endTimer(); + + // Check convergence + convergence = checkConvergence(iter, barrierParam, baselinePerformance, stepInfo); + + // Update the barrier parameter + barrierParam = updateBarrierParameter(barrierParam, baselinePerformance, stepInfo); + + // Next iteration + ++iter; + ++totalNumIterations_; + } + + computeControllerTimer_.startTimer(); + primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + costateTrajectory_ = std::move(lmd); + projectionMultiplierTrajectory_ = std::move(nu); + slackIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, constraintsSize_, slackStateIneq, slackStateInputIneq); + dualIneqTrajectory_ = ipm::toDualSolution(timeDiscretization, constraintsSize_, dualStateIneq, dualStateInputIneq); + problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); + computeControllerTimer_.endTimer(); + + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\nConvergence : " << toString(convergence) << "\n"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++ IPM solver has terminated ++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + } +} + +void IpmSolver::runParallel(std::function taskFunction) { + threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); +} + +void IpmSolver::initializeCostateTrajectory(const std::vector& timeDiscretization, const vector_array_t& stateTrajectory, + vector_array_t& costateTrajectory) const { + costateTrajectory.clear(); + costateTrajectory.reserve(stateTrajectory.size()); + + // Determine till when to use the previous solution + const auto interpolateTill = + primalSolution_.timeTrajectory_.size() < 2 ? timeDiscretization.front().time : primalSolution_.timeTrajectory_.back(); + + const scalar_t initTime = getIntervalStart(timeDiscretization[0]); + if (initTime < interpolateTill) { + costateTrajectory.push_back(LinearInterpolation::interpolate(initTime, primalSolution_.timeTrajectory_, costateTrajectory_)); + } else { + costateTrajectory.push_back(vector_t::Zero(stateTrajectory[0].size())); + } + + for (int i = 1; i < stateTrajectory.size(); i++) { + const auto time = getIntervalEnd(timeDiscretization[i]); + if (time < interpolateTill) { // interpolate previous solution + costateTrajectory.push_back(LinearInterpolation::interpolate(time, primalSolution_.timeTrajectory_, costateTrajectory_)); + } else { // Initialize with zero + costateTrajectory.push_back(vector_t::Zero(stateTrajectory[i].size())); + } + } +} + +void IpmSolver::initializeProjectionMultiplierTrajectory(const std::vector& timeDiscretization, + vector_array_t& projectionMultiplierTrajectory) const { + const size_t N = static_cast(timeDiscretization.size()) - 1; // size of the input trajectory + projectionMultiplierTrajectory.clear(); + projectionMultiplierTrajectory.reserve(N); + const auto& ocpDefinition = ocpDefinitions_[0]; + + // Determine till when to use the previous solution + const auto interpolateTill = + primalSolution_.timeTrajectory_.size() < 2 ? timeDiscretization.front().time : *std::prev(primalSolution_.timeTrajectory_.end(), 2); + + // @todo Fix this using trajectory spreading + auto interpolateProjectionMultiplierTrajectory = [&](scalar_t time) -> vector_t { + const size_t numConstraints = ocpDefinition.equalityConstraintPtr->getNumConstraints(time); + const size_t index = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_).first; + if (projectionMultiplierTrajectory_.size() > index + 1) { + if (projectionMultiplierTrajectory_[index].size() == numConstraints && + projectionMultiplierTrajectory_[index].size() == projectionMultiplierTrajectory_[index + 1].size()) { + return LinearInterpolation::interpolate(time, primalSolution_.timeTrajectory_, projectionMultiplierTrajectory_); + } + } + if (projectionMultiplierTrajectory_.size() > index) { + if (projectionMultiplierTrajectory_[index].size() == numConstraints) { + return projectionMultiplierTrajectory_[index]; + } + } + return vector_t::Zero(numConstraints); + }; + + for (int i = 0; i < N; i++) { + if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { + // Event Node + projectionMultiplierTrajectory.push_back(vector_t()); // no input at event node + } else { + // Intermediate node + const scalar_t time = getIntervalStart(timeDiscretization[i]); + const size_t numConstraints = ocpDefinition.equalityConstraintPtr->getNumConstraints(time); + if (time < interpolateTill) { // interpolate previous solution + projectionMultiplierTrajectory.push_back(interpolateProjectionMultiplierTrajectory(time)); + } else { // Initialize with zero + projectionMultiplierTrajectory.push_back(vector_t::Zero(numConstraints)); + } + } + } +} + +void IpmSolver::initializeSlackDualTrajectory(const std::vector& timeDiscretization, const vector_array_t& x, + const vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& dualStateIneq, vector_array_t& slackStateInputIneq, + vector_array_t& dualStateInputIneq) { + const auto& oldTimeTrajectory = slackIneqTrajectory_.timeTrajectory; + const auto& oldPostEventIndices = slackIneqTrajectory_.postEventIndices; + const auto newTimeTrajectory = toInterpolationTime(timeDiscretization); + const auto newPostEventIndices = toPostEventIndices(timeDiscretization); + + // find the time period that we can interpolate the cached solution + const auto timePeriod = std::make_pair(newTimeTrajectory.front(), newTimeTrajectory.back()); + const auto interpolatableTimePeriod = + findIntersectionToExtendableInterval(oldTimeTrajectory, this->getReferenceManager().getModeSchedule().eventTimes, timePeriod); + const bool interpolateTillFinalTime = numerics::almost_eq(interpolatableTimePeriod.second, timePeriod.second); + const auto cacheEventIndexBias = [&]() -> size_t { + if (!newPostEventIndices.empty()) { + const auto firstEventTime = newTimeTrajectory[newPostEventIndices[0] - 1]; + return getNumberOfPrecedingEvents(oldTimeTrajectory, oldPostEventIndices, firstEventTime); + } else { + return 0; + } + }(); + + auto& ocpDefinition = ocpDefinitions_.front(); + const size_t N = static_cast(timeDiscretization.size()) - 1; // size of the input trajectory + slackStateIneq.resize(N + 1); + dualStateIneq.resize(N + 1); + slackStateInputIneq.resize(N); + dualStateInputIneq.resize(N); + + int eventIdx = 0; + for (size_t i = 0; i < N; i++) { + if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { + const auto cachedEventIndex = cacheEventIndexBias + eventIdx; + if (cachedEventIndex < slackIneqTrajectory_.preJumps.size()) { + std::tie(slackStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.preJumps[cachedEventIndex]); + std::tie(dualStateIneq[i], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.preJumps[cachedEventIndex]); + } else { + scalar_t time = timeDiscretization[i].time; + slackStateIneq[i] = ipm::initializeEventSlackVariable(ocpDefinition, timeDiscretization[i].time, x[i], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = + ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + } + slackStateInputIneq[i].resize(0); + dualStateInputIneq[i].resize(0); + ++eventIdx; + } else { + const scalar_t time = getIntervalStart(timeDiscretization[i]); + if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { + std::tie(slackStateIneq[i], slackStateInputIneq[i]) = + ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + std::tie(dualStateIneq[i], dualStateInputIneq[i]) = + ipm::fromMultiplierCollection(getIntermediateDualSolutionAtTime(slackIneqTrajectory_, time)); + } else { + std::tie(slackStateIneq[i], slackStateInputIneq[i]) = ipm::initializeIntermediateSlackVariable( + ocpDefinition, time, x[i], u[i], settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[i] = + ipm::initializeDualVariable(slackStateIneq[i], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + dualStateInputIneq[i] = ipm::initializeDualVariable(slackStateInputIneq[i], barrierParam, settings_.initialDualLowerBound, + settings_.initialDualMarginRate); + } + } + } + + // Disable the state-only inequality constraints at the initial node + slackStateIneq[0].resize(0); + dualStateIneq[0].resize(0); + + if (interpolateTillFinalTime) { + std::tie(slackStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(slackIneqTrajectory_.final); + std::tie(dualStateIneq[N], std::ignore) = ipm::fromMultiplierCollection(dualIneqTrajectory_.final); + } else { + slackStateIneq[N] = ipm::initializeTerminalSlackVariable(ocpDefinition, getIntervalStart(timeDiscretization[N]), x[N], + settings_.initialSlackLowerBound, settings_.initialSlackMarginRate); + dualStateIneq[N] = + ipm::initializeDualVariable(slackStateIneq[N], barrierParam, settings_.initialDualLowerBound, settings_.initialDualMarginRate); + } +} + +IpmSolver::OcpSubproblemSolution IpmSolver::getOCPSolution(const vector_t& delta_x0, scalar_t barrierParam, + const vector_array_t& slackStateIneq, const vector_array_t& dualStateIneq, + const vector_array_t& slackStateInputIneq, + const vector_array_t& dualStateInputIneq) { + // Solve the QP + OcpSubproblemSolution solution; + auto& deltaXSol = solution.deltaXSol; + auto& deltaUSol = solution.deltaUSol; + hpipm_status status; + hpipmInterface_.resize(extractSizesFromProblem(dynamics_, lagrangian_, nullptr)); + status = hpipmInterface_.solve(delta_x0, dynamics_, lagrangian_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); + + if (status != hpipm_status::SUCCESS) { + throw std::runtime_error("[IpmSolver] Failed to solve QP"); + } + + // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + solution.armijoDescentMetric = armijoDescentMetric(lagrangian_, deltaXSol, deltaUSol); + + // Extract value function + if (settings_.createValueFunction) { + valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], lagrangian_[0]); + } + + // Problem horizon + const int N = static_cast(deltaXSol.size()) - 1; + + auto& deltaLmdSol = solution.deltaLmdSol; + auto& deltaNuSol = solution.deltaNuSol; + auto& deltaSlackStateIneq = solution.deltaSlackStateIneq; + auto& deltaDualStateIneq = solution.deltaDualStateIneq; + auto& deltaSlackStateInputIneq = solution.deltaSlackStateInputIneq; + auto& deltaDualStateInputIneq = solution.deltaDualStateInputIneq; + deltaLmdSol.resize(N + 1); + deltaNuSol.resize(N); + deltaSlackStateIneq.resize(N + 1); + deltaDualStateIneq.resize(N + 1); + deltaSlackStateInputIneq.resize(N); + deltaDualStateInputIneq.resize(N); + + scalar_array_t primalStepSizes(settings_.nThreads, 1.0); + scalar_array_t dualStepSizes(settings_.nThreads, 1.0); + + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + vector_t tmp; // 1 temporary for re-use for projection. + + int i = timeIndex++; + while (i < N) { + deltaSlackStateIneq[i] = ipm::retrieveSlackDirection(stateIneqConstraints_[i], deltaXSol[i], barrierParam, slackStateIneq[i]); + deltaDualStateIneq[i] = ipm::retrieveDualDirection(barrierParam, slackStateIneq[i], dualStateIneq[i], deltaSlackStateIneq[i]); + deltaSlackStateInputIneq[i] = + ipm::retrieveSlackDirection(stateInputIneqConstraints_[i], deltaXSol[i], deltaUSol[i], barrierParam, slackStateInputIneq[i]); + deltaDualStateInputIneq[i] = + ipm::retrieveDualDirection(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], deltaSlackStateInputIneq[i]); + primalStepSizes[workerId] = std::min( + {primalStepSizes[workerId], + ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin), + ipm::fractionToBoundaryStepSize(slackStateInputIneq[i], deltaSlackStateInputIneq[i], settings_.fractionToBoundaryMargin)}); + dualStepSizes[workerId] = std::min( + {dualStepSizes[workerId], + ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], settings_.fractionToBoundaryMargin), + ipm::fractionToBoundaryStepSize(dualStateInputIneq[i], deltaDualStateInputIneq[i], settings_.fractionToBoundaryMargin)}); + + // Extract Newton directions of the costate + if (settings_.computeLagrangeMultipliers) { + deltaLmdSol[i + 1] = valueFunction_[i + 1].dfdx; + deltaLmdSol[i + 1].noalias() += valueFunction_[i + 1].dfdxx * deltaXSol[i + 1]; + } + if (constraintsProjection_[i].f.size() > 0) { + // Extract Newton directions of the Lagrange multiplier associated with the state-input equality constraints + if (settings_.computeLagrangeMultipliers) { + deltaNuSol[i] = projectionMultiplierCoefficients_[i].f; + deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdx * deltaXSol[i]; + deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdu * deltaUSol[i]; + deltaNuSol[i].noalias() += projectionMultiplierCoefficients_[i].dfdcostate * deltaLmdSol[i + 1]; + } + // Re-map the projected input back to the original space. + tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i]; + deltaUSol[i] = tmp + constraintsProjection_[i].f; + deltaUSol[i].noalias() += constraintsProjection_[i].dfdx * deltaXSol[i]; + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + deltaSlackStateIneq[i] = ipm::retrieveSlackDirection(stateIneqConstraints_[i], deltaXSol[i], barrierParam, slackStateIneq[i]); + deltaDualStateIneq[i] = ipm::retrieveDualDirection(barrierParam, slackStateIneq[i], dualStateIneq[i], deltaSlackStateIneq[i]); + primalStepSizes[workerId] = + std::min(primalStepSizes[workerId], + ipm::fractionToBoundaryStepSize(slackStateIneq[i], deltaSlackStateIneq[i], settings_.fractionToBoundaryMargin)); + dualStepSizes[workerId] = std::min(dualStepSizes[workerId], ipm::fractionToBoundaryStepSize(dualStateIneq[i], deltaDualStateIneq[i], + settings_.fractionToBoundaryMargin)); + // Extract Newton directions of the costate + if (settings_.computeLagrangeMultipliers) { + deltaLmdSol[0] = valueFunction_[0].dfdx; + deltaLmdSol[0].noalias() += valueFunction_[i].dfdxx * deltaXSol[0]; + } + } + }; + runParallel(std::move(parallelTask)); + + solution.maxPrimalStepSize = *std::min_element(primalStepSizes.begin(), primalStepSizes.end()); + solution.maxDualStepSize = *std::min_element(dualStepSizes.begin(), dualStepSizes.end()); + + return solution; +} + +void IpmSolver::extractValueFunction(const std::vector& time, const vector_array_t& x, const vector_array_t& lmd, + const vector_array_t& deltaXSol) { + if (settings_.createValueFunction) { + // Correct for linearization state. Naive value function of hpipm is already extracted and stored in valueFunction_ in getOCPSolution(). + for (int i = 0; i < time.size(); ++i) { + valueFunction_[i].dfdx.noalias() -= valueFunction_[i].dfdxx * x[i]; + if (settings_.computeLagrangeMultipliers) { + valueFunction_[i].dfdx.noalias() += lmd[i]; + } + } + } +} + +PrimalSolution IpmSolver::toPrimalSolution(const std::vector& time, vector_array_t&& x, vector_array_t&& u) { + if (settings_.useFeedbackPolicy) { + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); + matrix_array_t KMatrices = hpipmInterface_.getRiccatiFeedback(dynamics_[0], lagrangian_[0]); + multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices); + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), std::move(KMatrices)); + + } else { + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u)); + } +} + +PerformanceIndex IpmSolver::setupQuadraticSubproblem(const std::vector& time, const vector_t& initState, + const vector_array_t& x, const vector_array_t& u, const vector_array_t& lmd, + const vector_array_t& nu, scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, const vector_array_t& dualStateIneq, + const vector_array_t& dualStateInputIneq, std::vector& metrics) { + // Problem horizon + const int N = static_cast(time.size()) - 1; + + std::vector performance(settings_.nThreads, PerformanceIndex()); + lagrangian_.resize(N + 1); + dynamics_.resize(N); + stateInputEqConstraints_.resize(N + 1); + stateIneqConstraints_.resize(N + 1); + stateInputIneqConstraints_.resize(N + 1); + constraintsProjection_.resize(N); + projectionMultiplierCoefficients_.resize(N); + constraintsSize_.resize(N + 1); + metrics.resize(N + 1); + + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; + + int i = timeIndex++; + while (i < N) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + // Event node + auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); + metrics[i] = multiple_shooting::computeMetrics(result); + performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[i]); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i].resize(0, x[i].size()); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); + stateInputIneqConstraints_[i].resize(0, x[i].size()); + constraintsProjection_[i].resize(0, x[i].size()); + projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); + constraintsSize_[i] = std::move(result.constraintsSize); + if (settings_.computeLagrangeMultipliers) { + lagrangian_[i] = multiple_shooting::evaluateLagrangianEventNode(lmd[i], lmd[i + 1], std::move(result.cost), dynamics_[i]); + } else { + lagrangian_[i] = std::move(result.cost); + } + + ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]); + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); + performance[workerId].dualFeasibilitiesSSE += + ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); + } else { + // Normal, intermediate node + const scalar_t ti = getIntervalStart(time[i]); + const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); + auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + // Disable the state-only inequality constraints at the initial node + if (i == 0) { + result.stateIneqConstraints.setZero(0, x[i].size()); + std::fill(result.constraintsSize.stateIneq.begin(), result.constraintsSize.stateIneq.end(), 0); + } + metrics[i] = multiple_shooting::computeMetrics(result); + performance[workerId] += ipm::computePerformanceIndex(result, dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); + multiple_shooting::projectTranscription(result, settings_.computeLagrangeMultipliers); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); + stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); + constraintsProjection_[i] = std::move(result.constraintsProjection); + projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); + constraintsSize_[i] = std::move(result.constraintsSize); + if (settings_.computeLagrangeMultipliers) { + lagrangian_[i] = multiple_shooting::evaluateLagrangianIntermediateNode(lmd[i], lmd[i + 1], nu[i], std::move(result.cost), + dynamics_[i], stateInputEqConstraints_[i]); + } else { + lagrangian_[i] = std::move(result.cost); + } + + ipm::condenseIneqConstraints(barrierParam, slackStateIneq[i], dualStateIneq[i], stateIneqConstraints_[i], lagrangian_[i]); + ipm::condenseIneqConstraints(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i], stateInputIneqConstraints_[i], + lagrangian_[i]); + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[i]); + performance[workerId].dualFeasibilitiesSSE += + ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[i], dualStateIneq[i]); + performance[workerId].dualFeasibilitiesSSE += + ipm::evaluateComplementarySlackness(barrierParam, slackStateInputIneq[i], dualStateInputIneq[i]); + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + const scalar_t tN = getIntervalStart(time[N]); + auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); + metrics[i] = multiple_shooting::computeMetrics(result); + performance[workerId] += ipm::computePerformanceIndex(result, barrierParam, slackStateIneq[N]); + stateInputEqConstraints_[i].resize(0, x[i].size()); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); + constraintsSize_[i] = std::move(result.constraintsSize); + if (settings_.computeLagrangeMultipliers) { + lagrangian_[i] = multiple_shooting::evaluateLagrangianTerminalNode(lmd[i], std::move(result.cost)); + } else { + lagrangian_[i] = std::move(result.cost); + } + ipm::condenseIneqConstraints(barrierParam, slackStateIneq[N], dualStateIneq[N], stateIneqConstraints_[N], lagrangian_[N]); + performance[workerId].dualFeasibilitiesSSE += multiple_shooting::evaluateDualFeasibilities(lagrangian_[N]); + performance[workerId].dualFeasibilitiesSSE += ipm::evaluateComplementarySlackness(barrierParam, slackStateIneq[N], dualStateIneq[N]); + } + }; + runParallel(std::move(parallelTask)); + + // Account for initial state in performance + const vector_t initDynamicsViolation = initState - x.front(); + metrics.front().dynamicsViolation += initDynamicsViolation; + performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); + + // Sum performance of the threads + PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); + totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + + return totalPerformance; +} + +PerformanceIndex IpmSolver::computePerformance(const std::vector& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, scalar_t barrierParam, const vector_array_t& slackStateIneq, + const vector_array_t& slackStateInputIneq, std::vector& metrics) { + // Problem horizon + const int N = static_cast(time.size()) - 1; + metrics.resize(N + 1); + + std::vector performance(settings_.nThreads, PerformanceIndex()); + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; + + int i = timeIndex++; + while (i < N) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + // Event node + metrics[i] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]); + performance[workerId] += ipm::toPerformanceIndex(metrics[i], barrierParam, slackStateIneq[i]); + } else { + // Normal, intermediate node + const scalar_t ti = getIntervalStart(time[i]); + const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); + const bool enableStateInequalityConstraints = (i > 0); + metrics[i] = multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + // Disable the state-only inequality constraints at the initial node + if (i == 0) { + metrics[i].stateIneqConstraint.clear(); + } + performance[workerId] += ipm::toPerformanceIndex(metrics[i], dt, barrierParam, slackStateIneq[i], slackStateInputIneq[i]); + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + const scalar_t tN = getIntervalStart(time[N]); + metrics[N] = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]); + performance[workerId] += ipm::toPerformanceIndex(metrics[N], barrierParam, slackStateIneq[N]); + } + }; + runParallel(std::move(parallelTask)); + + // Account for initial state in performance + const vector_t initDynamicsViolation = initState - x.front(); + metrics.front().dynamicsViolation += initDynamicsViolation; + performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); + + // Sum performance of the threads + PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); + totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; +} + +ipm::StepInfo IpmSolver::takePrimalStep(const PerformanceIndex& baseline, const std::vector& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u, scalar_t barrierParam, vector_array_t& slackStateIneq, + vector_array_t& slackStateInputIneq, std::vector& metrics) { + using StepType = FilterLinesearch::StepType; + + /* + * Filter linesearch based on: + * "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" + * https://link.springer.com/article/10.1007/s10107-004-0559-y + */ + if (settings_.printLinesearch) { + std::cerr << std::setprecision(9) << std::fixed; + std::cerr << "\n=== Linesearch ===\n"; + std::cerr << "Baseline:\n" << baseline << "\n"; + } + + // Baseline costs + const scalar_t baselineConstraintViolation = FilterLinesearch::totalConstraintViolation(baseline); + + // Update norm + const auto& dx = subproblemSolution.deltaXSol; + const auto& du = subproblemSolution.deltaUSol; + const auto& deltaSlackStateIneq = subproblemSolution.deltaSlackStateIneq; + const auto& deltaSlackStateInputIneq = subproblemSolution.deltaSlackStateInputIneq; + const auto deltaUnorm = multiple_shooting::trajectoryNorm(du); + const auto deltaXnorm = multiple_shooting::trajectoryNorm(dx); + + scalar_t alpha = subproblemSolution.maxPrimalStepSize; + vector_array_t xNew(x.size()); + vector_array_t uNew(u.size()); + vector_array_t slackStateIneqNew(slackStateIneq.size()); + vector_array_t slackStateInputIneqNew(slackStateInputIneq.size()); + std::vector metricsNew(metrics.size()); + do { + // Compute step + multiple_shooting::incrementTrajectory(u, du, alpha, uNew); + multiple_shooting::incrementTrajectory(x, dx, alpha, xNew); + multiple_shooting::incrementTrajectory(slackStateIneq, deltaSlackStateIneq, alpha, slackStateIneqNew); + multiple_shooting::incrementTrajectory(slackStateInputIneq, deltaSlackStateInputIneq, alpha, slackStateInputIneqNew); + + // Compute cost and constraints + const PerformanceIndex performanceNew = + computePerformance(timeDiscretization, initState, xNew, uNew, barrierParam, slackStateIneqNew, slackStateInputIneqNew, metricsNew); + + // Step acceptance and record step type + bool stepAccepted; + StepType stepType; + std::tie(stepAccepted, stepType) = + filterLinesearch_.acceptStep(baseline, performanceNew, alpha * subproblemSolution.armijoDescentMetric); + + if (settings_.printLinesearch) { + std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepType) + << (stepAccepted ? std::string{" (Accepted)"} : std::string{" (Rejected)"}) << "\n"; + std::cerr << "|dx| = " << alpha * deltaXnorm << "\t|du| = " << alpha * deltaUnorm << "\n"; + std::cerr << performanceNew << "\n"; + } + + if (stepAccepted) { // Return if step accepted + x = std::move(xNew); + u = std::move(uNew); + slackStateIneq = std::move(slackStateIneqNew); + slackStateInputIneq = std::move(slackStateInputIneqNew); + metrics = std::move(metricsNew); + + // Prepare step info + ipm::StepInfo stepInfo; + stepInfo.primalStepSize = alpha; + stepInfo.stepType = stepType; + stepInfo.dx_norm = alpha * deltaXnorm; + stepInfo.du_norm = alpha * deltaUnorm; + stepInfo.performanceAfterStep = performanceNew; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(performanceNew); + return stepInfo; + + } else { // Try smaller step + alpha *= settings_.alpha_decay; + + // Detect too small step size during back-tracking to escape early. Prevents going all the way to alpha_min + if (alpha * deltaXnorm < settings_.deltaTol && alpha * deltaUnorm < settings_.deltaTol) { + if (settings_.printLinesearch) { + std::cerr << "Exiting linesearch early due to too small primal steps |dx|: " << alpha * deltaXnorm + << ", and or |du|: " << alpha * deltaUnorm << " are below deltaTol: " << settings_.deltaTol << "\n"; + } + break; + } + } + } while (alpha >= settings_.alpha_min); + + // Alpha_min reached -> Don't take a step + ipm::StepInfo stepInfo; + stepInfo.primalStepSize = 0.0; + stepInfo.stepType = StepType::ZERO; + stepInfo.dx_norm = 0.0; + stepInfo.du_norm = 0.0; + stepInfo.performanceAfterStep = baseline; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); + + if (settings_.printLinesearch) { + std::cerr << "[Linesearch terminated] Primal Step size: " << stepInfo.primalStepSize << ", Step Type: " << toString(stepInfo.stepType) + << "\n"; + } + + return stepInfo; +} + +void IpmSolver::takeDualStep(const OcpSubproblemSolution& subproblemSolution, const ipm::StepInfo& stepInfo, vector_array_t& lmd, + vector_array_t& nu, vector_array_t& dualStateIneq, vector_array_t& dualStateInputIneq) const { + if (settings_.computeLagrangeMultipliers) { + multiple_shooting::incrementTrajectory(lmd, subproblemSolution.deltaLmdSol, stepInfo.primalStepSize, lmd); + multiple_shooting::incrementTrajectory(nu, subproblemSolution.deltaNuSol, stepInfo.primalStepSize, nu); + } + const scalar_t dualStepSize = settings_.usePrimalStepSizeForDual ? std::min(stepInfo.primalStepSize, subproblemSolution.maxDualStepSize) + : subproblemSolution.maxDualStepSize; + multiple_shooting::incrementTrajectory(dualStateIneq, subproblemSolution.deltaDualStateIneq, dualStepSize, dualStateIneq); + multiple_shooting::incrementTrajectory(dualStateInputIneq, subproblemSolution.deltaDualStateInputIneq, dualStepSize, dualStateInputIneq); +} + +scalar_t IpmSolver::updateBarrierParameter(scalar_t currentBarrierParameter, const PerformanceIndex& baseline, + const ipm::StepInfo& stepInfo) const { + if (currentBarrierParameter <= settings_.targetBarrierParameter) { + return currentBarrierParameter; + } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.barrierReductionCostTol && + FilterLinesearch::totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.barrierReductionConstraintTol) { + return std::min((currentBarrierParameter * settings_.barrierLinearDecreaseFactor), + std::pow(currentBarrierParameter, settings_.barrierSuperlinearDecreasePower)); + } else { + return currentBarrierParameter; + } +} + +ipm::Convergence IpmSolver::checkConvergence(int iteration, scalar_t barrierParam, const PerformanceIndex& baseline, + const ipm::StepInfo& stepInfo) const { + using Convergence = ipm::Convergence; + if ((iteration + 1) >= settings_.ipmIteration) { + // Converged because the next iteration would exceed the specified number of iterations + return Convergence::ITERATIONS; + } else if (stepInfo.primalStepSize < settings_.alpha_min) { + // Converged because step size is below the specified minimum + return Convergence::STEPSIZE; + } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.costTol && + FilterLinesearch::totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { + // Converged because the change in merit is below the specified tolerance while the constraint violation is below the minimum + return Convergence::METRICS; + } else if (stepInfo.dx_norm < settings_.deltaTol && stepInfo.du_norm < settings_.deltaTol && + barrierParam <= settings_.targetBarrierParameter) { + // Converged because the change in primal variables is below the specified tolerance + return Convergence::PRIMAL; + } else { + // None of the above convergence criteria were met -> not converged. + return Convergence::FALSE; + } +} + +} // namespace ocs2 diff --git a/ocs2_ipm/test/Exp0Test.cpp b/ocs2_ipm/test/Exp0Test.cpp new file mode 100644 index 000000000..ba7a8b25e --- /dev/null +++ b/ocs2_ipm/test/Exp0Test.cpp @@ -0,0 +1,177 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include +#include +#include +#include + +#include "ocs2_ipm/IpmSolver.h" + +#include +#include +#include +#include + +using namespace ocs2; + +TEST(Exp0Test, Unconstrained) { + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; + + // Solver settings + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); + + const scalar_array_t initEventTimes{0.1897}; + const size_array_t modeSequence{0, 1}; + auto referenceManagerPtr = getExp0ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp0Problem(referenceManagerPtr); + + const scalar_t startTime = 0.0; + const scalar_t finalTime = 2.0; + const vector_t initState = (vector_t(STATE_DIM) << 0.0, 2.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); +} + +TEST(Exp0Test, Constrained) { + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; + + auto getStateBoxConstraint = [&](const vector_t& minState, const vector_t& maxState) { + constexpr size_t numIneqConstraint = 2 * STATE_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minState, maxState).finished(); + const matrix_t C = + (matrix_t(numIneqConstraint, STATE_DIM) << matrix_t::Identity(STATE_DIM, STATE_DIM), -matrix_t::Identity(STATE_DIM, STATE_DIM)) + .finished(); + return std::make_unique(std::move(e), std::move(C)); + }; + + auto getInputBoxConstraint = [&](scalar_t minInput, scalar_t maxInput) { + constexpr size_t numIneqConstraint = 2 * INPUT_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minInput, maxInput).finished(); + const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM); + const matrix_t D = + (matrix_t(numIneqConstraint, INPUT_DIM) << matrix_t::Identity(INPUT_DIM, INPUT_DIM), -matrix_t::Identity(INPUT_DIM, INPUT_DIM)) + .finished(); + return std::make_unique(std::move(e), std::move(C), std::move(D)); + }; + + // Solver settings + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); + + const scalar_array_t initEventTimes{0.1897}; + const size_array_t modeSequence{0, 1}; + auto referenceManagerPtr = getExp0ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp0Problem(referenceManagerPtr); + + // add inequality constraints + const scalar_t umin = -7.5; + const scalar_t umax = 7.5; + problem.inequalityConstraintPtr->add("ubound", getInputBoxConstraint(umin, umax)); + const vector_t xmin = (vector_t(2) << -7.5, -7.5).finished(); + const vector_t xmax = (vector_t(2) << 7.5, 7.5).finished(); + problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + + const scalar_t startTime = 0.0; + const scalar_t finalTime = 2.0; + const vector_t initState = (vector_t(STATE_DIM) << 0.0, 2.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + + // check constraint satisfaction + for (const auto& x : primalSolution.stateTrajectory_) { + if (x.size() > 0) { + ASSERT_TRUE((x - xmin).minCoeff() >= 0); + ASSERT_TRUE((xmax - x).minCoeff() >= 0); + } + } + for (const auto& u : primalSolution.inputTrajectory_) { + if (u.size() > 0) { + ASSERT_TRUE(u(0) - umin >= 0); + ASSERT_TRUE(umax - u(0) >= 0); + } + } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } +} \ No newline at end of file diff --git a/ocs2_ipm/test/Exp1Test.cpp b/ocs2_ipm/test/Exp1Test.cpp new file mode 100644 index 000000000..15cceb2d3 --- /dev/null +++ b/ocs2_ipm/test/Exp1Test.cpp @@ -0,0 +1,272 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include +#include +#include +#include + +#include "ocs2_ipm/IpmSolver.h" + +#include +#include +#include +#include + +using namespace ocs2; + +class EXP1_MixedStateInputIneqConstraints final : public StateInputConstraint { + public: + EXP1_MixedStateInputIneqConstraints(scalar_t xumin, scalar_t xumax) + : StateInputConstraint(ConstraintOrder::Linear), xumin_(xumin), xumax_(xumax) {} + ~EXP1_MixedStateInputIneqConstraints() override = default; + + EXP1_MixedStateInputIneqConstraints* clone() const override { return new EXP1_MixedStateInputIneqConstraints(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 2; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { + vector_t e(2); + e << (x(0) * u(0) - xumin_), (xumax_ - x(1) * u(0)); + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e(2, x.size(), u.size()); + e.f = getValue(t, x, u, preComp); + e.dfdx << u(0), 0, 0, -u(0); + e.dfdu << x(0), -x(1); + return e; + } + + private: + const scalar_t xumin_, xumax_; +}; + +TEST(Exp1Test, Unconstrained) { + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; + + // Solver settings + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); + + const scalar_array_t initEventTimes{0.2262, 1.0176}; + const size_array_t modeSequence{0, 1, 2}; + auto referenceManagerPtr = getExp1ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp1Problem(referenceManagerPtr); + + const scalar_t startTime = 0.0; + const scalar_t finalTime = 3.0; + const vector_t initState = (vector_t(STATE_DIM) << 2.0, 3.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); +} + +TEST(Exp1Test, Constrained) { + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; + + auto getStateBoxConstraint = [&](const vector_t& minState, const vector_t& maxState) { + constexpr size_t numIneqConstraint = 2 * STATE_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minState, maxState).finished(); + const matrix_t C = + (matrix_t(numIneqConstraint, STATE_DIM) << matrix_t::Identity(STATE_DIM, STATE_DIM), -matrix_t::Identity(STATE_DIM, STATE_DIM)) + .finished(); + return std::make_unique(std::move(e), std::move(C)); + }; + + auto getInputBoxConstraint = [&](scalar_t minInput, scalar_t maxInput) { + constexpr size_t numIneqConstraint = 2 * INPUT_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minInput, maxInput).finished(); + const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM); + const matrix_t D = + (matrix_t(numIneqConstraint, INPUT_DIM) << matrix_t::Identity(INPUT_DIM, INPUT_DIM), -matrix_t::Identity(INPUT_DIM, INPUT_DIM)) + .finished(); + return std::make_unique(std::move(e), std::move(C), std::move(D)); + }; + + // Solver settings + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); + + const scalar_array_t initEventTimes{0.2262, 1.0176}; + const size_array_t modeSequence{0, 1, 2}; + auto referenceManagerPtr = getExp1ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp1Problem(referenceManagerPtr); + + // add inequality constraints + const scalar_t umin = -1.0; + const scalar_t umax = 1.0; + problem.inequalityConstraintPtr->add("ubound", getInputBoxConstraint(umin, umax)); + const vector_t xmin = (vector_t(2) << -0.0, -0.0).finished(); + const vector_t xmax = (vector_t(2) << 3.0, 4.0).finished(); + problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + + const scalar_t startTime = 0.0; + const scalar_t finalTime = 3.0; + const vector_t initState = (vector_t(STATE_DIM) << 2.0, 3.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + + // check constraint satisfaction + for (const auto& x : primalSolution.stateTrajectory_) { + if (x.size() > 0) { + ASSERT_TRUE((x - xmin).minCoeff() >= 0); + ASSERT_TRUE((xmax - x).minCoeff() >= 0); + } + } + for (const auto& u : primalSolution.inputTrajectory_) { + if (u.size() > 0) { + ASSERT_TRUE(u(0) - umin >= 0); + ASSERT_TRUE(umax - u(0) >= 0); + } + } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } +} + +TEST(Exp1Test, MixedConstrained) { + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 1; + + // Solver settings + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); + + const scalar_array_t initEventTimes{0.2262, 1.0176}; + const size_array_t modeSequence{0, 1, 2}; + auto referenceManagerPtr = getExp1ReferenceManager(initEventTimes, modeSequence); + auto problem = createExp1Problem(referenceManagerPtr); + + // add inequality constraints + const scalar_t xumin = -1.0; + const scalar_t xumax = 1.0; + auto stateInputIneqConstraint = std::make_unique(xumin, xumax); + auto stateInputIneqConstraintCloned = stateInputIneqConstraint->clone(); + problem.inequalityConstraintPtr->add("bound", std::move(stateInputIneqConstraint)); + const scalar_t startTime = 0.0; + const scalar_t finalTime = 3.0; + const vector_t initState = (vector_t(STATE_DIM) << 2.0, 3.0).finished(); + + DefaultInitializer zeroInitializer(INPUT_DIM); + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + + // check constraint satisfaction + const size_t N = primalSolution.inputTrajectory_.size(); + for (size_t i = 0; i < N; ++i) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, PreComputation()); + ASSERT_TRUE(constraintValue.minCoeff() >= 0.0); + } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } +} diff --git a/ocs2_ipm/test/testCircularKinematics.cpp b/ocs2_ipm/test/testCircularKinematics.cpp new file mode 100644 index 000000000..e05dc269b --- /dev/null +++ b/ocs2_ipm/test/testCircularKinematics.cpp @@ -0,0 +1,346 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include "ocs2_ipm/IpmSolver.h" + +#include +#include +#include +#include + +using namespace ocs2; + +class CircleKinematics_MixedStateInputIneqConstraints final : public StateInputConstraint { + public: + CircleKinematics_MixedStateInputIneqConstraints(scalar_t xumin, scalar_t xumax) + : StateInputConstraint(ConstraintOrder::Linear), xumin_(xumin), xumax_(xumax) {} + ~CircleKinematics_MixedStateInputIneqConstraints() override = default; + + CircleKinematics_MixedStateInputIneqConstraints* clone() const override { + return new CircleKinematics_MixedStateInputIneqConstraints(*this); + } + + size_t getNumConstraints(scalar_t time) const override { return 2; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { + vector_t e(2); + e << (x(0) * u(0) - xumin_), (xumax_ - x(1) * u(1)); + return e; + } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation& preComp) const override { + VectorFunctionLinearApproximation e(2, x.size(), u.size()); + e.f = getValue(t, x, u, preComp); + e.dfdx << u(0), 0, 0, -u(1); + e.dfdu << x(0), 0, 0, -x(1); + return e; + } + + private: + const scalar_t xumin_, xumax_; +}; + +TEST(test_circular_kinematics, solve_projected_EqConstraints) { + // optimal control problem + OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + + // Initializer + DefaultInitializer zeroInitializer(2); + + // Solver settings + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); + + // Additional problem definitions + const scalar_t startTime = 0.0; + const scalar_t finalTime = 1.0; + const vector_t initState = (vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + + // Check initial condition + ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); + + // Check constraint satisfaction. + const auto performance = solver.getPerformanceIndeces(); + ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); + ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); + + // Check feedback controller + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + // Feed forward part + ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); + } +} + +TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) { + constexpr size_t STATE_DIM = 2; + constexpr size_t INPUT_DIM = 2; + + // optimal control problem + OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + + auto getStateBoxConstraint = [&](const vector_t& minState, const vector_t& maxState) { + constexpr size_t numIneqConstraint = 2 * STATE_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minState, maxState).finished(); + const matrix_t C = + (matrix_t(numIneqConstraint, STATE_DIM) << matrix_t::Identity(STATE_DIM, STATE_DIM), -matrix_t::Identity(STATE_DIM, STATE_DIM)) + .finished(); + return std::make_unique(std::move(e), std::move(C)); + }; + + auto getInputBoxConstraint = [&](const vector_t& minInput, const vector_t& maxInput) { + constexpr size_t numIneqConstraint = 2 * INPUT_DIM; + const vector_t e = (vector_t(numIneqConstraint) << -minInput, maxInput).finished(); + const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM); + const matrix_t D = + (matrix_t(numIneqConstraint, INPUT_DIM) << matrix_t::Identity(INPUT_DIM, INPUT_DIM), -matrix_t::Identity(INPUT_DIM, INPUT_DIM)) + .finished(); + return std::make_unique(std::move(e), std::move(C), std::move(D)); + }; + + // inequality constraints + const vector_t umin = (vector_t(2) << -0.5, -0.5).finished(); + const vector_t umax = (vector_t(2) << 0.5, 0.5).finished(); + problem.inequalityConstraintPtr->add("ubound", getInputBoxConstraint(umin, umax)); + const vector_t xmin = (vector_t(2) << -0.5, -0.5).finished(); + const vector_t xmax = (vector_t(2) << 1.0e03, 1.0e03).finished(); // no upper bound + problem.stateInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + problem.finalInequalityConstraintPtr->add("xbound", getStateBoxConstraint(xmin, xmax)); + + // Initializer + DefaultInitializer zeroInitializer(2); + + // Solver settings + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.useFeedbackPolicy = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); + + // Additional problem definitions + const scalar_t startTime = 0.0; + const scalar_t finalTime = 1.0; + const vector_t initState = (vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + + // check constraint satisfaction + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + if (primalSolution.inputTrajectory_[i].size() > 0) { + const scalar_t projectionConstraintViolation = primalSolution.stateTrajectory_[i].dot(primalSolution.inputTrajectory_[i]); + EXPECT_LT(std::abs(projectionConstraintViolation), 1.0e-06); + } + } + + // check constraint satisfaction + for (const auto& x : primalSolution.stateTrajectory_) { + if (x.size() > 0) { + ASSERT_TRUE((x - xmin).minCoeff() >= 0); + ASSERT_TRUE((xmax - x).minCoeff() >= 0); + } + } + for (const auto& u : primalSolution.inputTrajectory_) { + if (u.size() > 0) { + ASSERT_TRUE((u - umin).minCoeff() >= 0); + ASSERT_TRUE((umax - u).minCoeff() >= 0); + } + } + + // Check initial condition + ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); + + // Check constraint satisfaction. + const auto performance = solver.getPerformanceIndeces(); + ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); + ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); + + // Check feedback controller + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + // Feed forward part + ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); + } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } +} + +TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraints) { + // optimal control problem + OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/ocs2/ipm_test_generated"); + + // inequality constraints + const scalar_t xumin = -2.0; + const scalar_t xumax = 2.0; + auto stateInputIneqConstraint = std::make_unique(xumin, xumax); + auto stateInputIneqConstraintCloned = stateInputIneqConstraint->clone(); + problem.inequalityConstraintPtr->add("xubound", std::move(stateInputIneqConstraint)); + + // Initializer + DefaultInitializer zeroInitializer(2); + + // Solver settings + const auto settings = []() { + ipm::Settings s; + s.dt = 0.01; + s.ipmIteration = 20; + s.useFeedbackPolicy = true; + s.computeLagrangeMultipliers = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.printSolverStatistics = true; + s.printSolverStatus = true; + s.printLinesearch = true; + s.nThreads = 1; + s.initialBarrierParameter = 1.0e-02; + s.targetBarrierParameter = 1.0e-04; + s.barrierLinearDecreaseFactor = 0.2; + s.barrierSuperlinearDecreasePower = 1.5; + s.fractionToBoundaryMargin = 0.995; + return s; + }(); + + // Additional problem definitions + const scalar_t startTime = 0.0; + const scalar_t finalTime = 1.0; + const vector_t initState = (vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 + + // Solve + IpmSolver solver(settings, problem, zeroInitializer); + solver.run(startTime, initState, finalTime); + + const auto primalSolution = solver.primalSolution(finalTime); + + // check constraint satisfaction + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + if (primalSolution.inputTrajectory_[i].size() > 0) { + const scalar_t projectionConstraintViolation = primalSolution.stateTrajectory_[i].dot(primalSolution.inputTrajectory_[i]); + EXPECT_LT(std::abs(projectionConstraintViolation), 1.0e-06); + } + } + + // check constraint satisfaction + const size_t N = primalSolution.inputTrajectory_.size(); + for (size_t i = 0; i < N; ++i) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, PreComputation()); + ASSERT_TRUE(constraintValue.minCoeff() >= 0.0); + } + + // Check initial condition + ASSERT_TRUE(primalSolution.stateTrajectory_.front().isApprox(initState)); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.front(), startTime); + ASSERT_DOUBLE_EQ(primalSolution.timeTrajectory_.back(), finalTime); + + // Check constraint satisfaction. + const auto performance = solver.getPerformanceIndeces(); + ASSERT_LT(performance.dynamicsViolationSSE, 1e-6); + ASSERT_LT(performance.equalityConstraintsSSE, 1e-6); + + // Check feedback controller + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + // Feed forward part + ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x))); + } + + // Check Lagrange multipliers + for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) { + const auto t = primalSolution.timeTrajectory_[i]; + const auto& x = primalSolution.stateTrajectory_[i]; + const auto& u = primalSolution.inputTrajectory_[i]; + ASSERT_NO_THROW(const auto multiplier = solver.getStateInputEqualityConstraintLagrangian(t, x);); + } + + // solve with shifted horizon + const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19}; + for (const auto e : shiftTime) { + solver.run(startTime + e, initState, finalTime + e); + } +} \ No newline at end of file diff --git a/ocs2_ipm/test/testSwitchedProblem.cpp b/ocs2_ipm/test/testSwitchedProblem.cpp new file mode 100644 index 000000000..dbc0f0813 --- /dev/null +++ b/ocs2_ipm/test/testSwitchedProblem.cpp @@ -0,0 +1,268 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include "ocs2_ipm/IpmSolver.h" + +#include +#include +#include +#include +#include +#include + +#include + +namespace ocs2 { +namespace { +/** + * A constraint that in mode 0 sets u[0] = 0 and in mode 1 sets u[1] = 1 + */ +class SwitchedConstraint : public StateInputConstraint { + public: + explicit SwitchedConstraint(std::shared_ptr referenceManagerPtr) + : StateInputConstraint(ConstraintOrder::Linear), referenceManagerPtr_(std::move(referenceManagerPtr)) { + constexpr int n = 3; + constexpr int m = 2; + constexpr int nc = 1; + + auto stateInputConstraints0 = VectorFunctionLinearApproximation::Zero(nc, n, m); + stateInputConstraints0.dfdu << 1.0, 0.0; + + auto stateInputConstraints1 = VectorFunctionLinearApproximation::Zero(nc, n, m); + stateInputConstraints1.dfdu << 0.0, 1.0; + + subsystemConstraintsPtr_.emplace_back(getOcs2Constraints(stateInputConstraints0)); + subsystemConstraintsPtr_.emplace_back(getOcs2Constraints(stateInputConstraints1)); + } + + ~SwitchedConstraint() override = default; + SwitchedConstraint* clone() const override { return new SwitchedConstraint(*this); } + + size_t getNumConstraints(scalar_t time) const override { + const auto activeMode = referenceManagerPtr_->getModeSchedule().modeAtTime(time); + return subsystemConstraintsPtr_[activeMode]->getNumConstraints(time); + } + + vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override { + const auto activeMode = referenceManagerPtr_->getModeSchedule().modeAtTime(time); + return subsystemConstraintsPtr_[activeMode]->getValue(time, state, input, preComp); + }; + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const { + const auto activeMode = referenceManagerPtr_->getModeSchedule().modeAtTime(time); + return subsystemConstraintsPtr_[activeMode]->getLinearApproximation(time, state, input, preComp); + } + + private: + SwitchedConstraint(const SwitchedConstraint& other) + : ocs2::StateInputConstraint(other), referenceManagerPtr_(other.referenceManagerPtr_) { + for (const auto& constraint : other.subsystemConstraintsPtr_) { + subsystemConstraintsPtr_.emplace_back(constraint->clone()); + } + } + + std::shared_ptr referenceManagerPtr_; + std::vector> subsystemConstraintsPtr_; +}; + +std::pair> solveWithEventTime(scalar_t eventTime) { + constexpr int n = 3; + constexpr int m = 2; + + ocs2::OptimalControlProblem problem; + + // System + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto jumpMap = matrix_t::Random(n, n); + problem.dynamicsPtr.reset(new ocs2::LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(ocs2::getRandomCost(n, m))); + problem.softConstraintPtr->add("intermediateCost", ocs2::getOcs2Cost(ocs2::getRandomCost(n, m))); + problem.preJumpCostPtr->add("eventCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + problem.preJumpSoftConstraintPtr->add("eventCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + problem.finalSoftConstraintPtr->add("finalCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + + // Reference Manager + const ocs2::ModeSchedule modeSchedule({eventTime}, {0, 1}); + const ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Random(n)}, {ocs2::vector_t::Random(m)}); + auto referenceManagerPtr = std::make_shared(targetTrajectories, modeSchedule); + + problem.targetTrajectoriesPtr = &targetTrajectories; + + // Constraint + problem.equalityConstraintPtr->add("switchedConstraint", std::make_unique(referenceManagerPtr)); + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.05; + settings.ipmIteration = 20; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = ocs2::vector_t::Random(n); + + // Set up solver + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Solve + solver.run(startTime, initState, finalTime); + return {solver.primalSolution(finalTime), solver.getIterationsLog()}; +} + +} // namespace +} // namespace ocs2 + +TEST(test_switched_problem, switched_constraint) { + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::scalar_t eventTime = 0.1875; + const double tol = 1e-9; + const auto solution = ocs2::solveWithEventTime(eventTime); + const auto& primalSolution = solution.first; + const auto& performanceLog = solution.second; + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(performanceLog.size(), 2); + ASSERT_LT(performanceLog.back().dynamicsViolationSSE, tol); + + // Should have correct node pre and post event time, with corresponding inputs + ASSERT_EQ(primalSolution.timeTrajectory_[4], eventTime); + ASSERT_EQ(primalSolution.timeTrajectory_[5], eventTime); + ASSERT_LT(std::abs(primalSolution.inputTrajectory_[4][0]), tol); + ASSERT_LT(std::abs(primalSolution.inputTrajectory_[5][1]), tol); + + // Inspect solution, check at a dt smaller than solution to check interpolation around the switch. + ocs2::scalar_t t_check = startTime; + ocs2::scalar_t dt_check = 1e-5; + while (t_check < finalTime) { + ocs2::vector_t xNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_); + ocs2::vector_t uNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_); + ocs2::vector_t uControl = primalSolution.controllerPtr_->computeInput(t_check, xNominal); + if (t_check < eventTime) { + ASSERT_LT(std::abs(uNominal[0]), tol); + ASSERT_LT(std::abs(uControl[0]), tol); + } else if (t_check > eventTime) { + ASSERT_LT(std::abs(uNominal[1]), tol); + ASSERT_LT(std::abs(uControl[1]), tol); + } + t_check += dt_check; + } +} + +TEST(test_switched_problem, event_at_beginning) { + // The event should replace the start time, all inputs should be after the event. + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::scalar_t eventTime = 1e-8; + const double tol = 1e-9; + const auto solution = ocs2::solveWithEventTime(eventTime); + const auto& primalSolution = solution.first; + const auto& performanceLog = solution.second; + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(performanceLog.size(), 2); + ASSERT_LT(performanceLog.back().dynamicsViolationSSE, tol); + + // Should have correct post event time start + ASSERT_EQ(primalSolution.timeTrajectory_[0], eventTime); + ASSERT_NE(primalSolution.timeTrajectory_[1], eventTime); + + // Inspect solution, check at a dt smaller than solution to check interpolation around the switch. + ocs2::scalar_t t_check = eventTime; + ocs2::scalar_t dt_check = 1e-5; + while (t_check < finalTime) { + ocs2::vector_t xNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_); + ocs2::vector_t uNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_); + ocs2::vector_t uControl = primalSolution.controllerPtr_->computeInput(t_check, xNominal); + ASSERT_LT(std::abs(uNominal[1]), tol); + ASSERT_LT(std::abs(uControl[1]), tol); + t_check += dt_check; + } +} + +TEST(test_switched_problem, event_at_end) { + // The event should be ignored because its too close to the final time, all inputs should be before the event. + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::scalar_t eventTime = 1.0 - 1e-8; + const double tol = 1e-9; + const auto solution = ocs2::solveWithEventTime(eventTime); + const auto& primalSolution = solution.first; + const auto& performanceLog = solution.second; + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(performanceLog.size(), 2); + ASSERT_LT(performanceLog.back().dynamicsViolationSSE, tol); + + // Should have correct node pre and post event time + ASSERT_TRUE(std::none_of(primalSolution.timeTrajectory_.begin(), primalSolution.timeTrajectory_.end(), + [=](ocs2::scalar_t t) { return t == eventTime; })); + + // Inspect solution, check at a dt smaller than solution to check interpolation around the switch. + ocs2::scalar_t t_check = startTime; + ocs2::scalar_t dt_check = 1e-5; + while (t_check < finalTime) { + ocs2::vector_t xNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_); + ocs2::vector_t uNominal = + ocs2::LinearInterpolation::interpolate(t_check, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_); + ocs2::vector_t uControl = primalSolution.controllerPtr_->computeInput(t_check, xNominal); + ASSERT_LT(std::abs(uNominal[0]), tol); + ASSERT_LT(std::abs(uControl[0]), tol); + t_check += dt_check; + } +} diff --git a/ocs2_ipm/test/testUnconstrained.cpp b/ocs2_ipm/test/testUnconstrained.cpp new file mode 100644 index 000000000..517252735 --- /dev/null +++ b/ocs2_ipm/test/testUnconstrained.cpp @@ -0,0 +1,161 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include "ocs2_ipm/IpmSolver.h" + +#include + +#include +#include + +namespace ocs2 { +namespace { + +std::pair> solveWithFeedbackSetting( + bool feedback, bool emptyConstraint, const VectorFunctionLinearApproximation& dynamicsMatrices, + const ScalarFunctionQuadraticApproximation& costMatrices) { + int n = dynamicsMatrices.dfdu.rows(); + int m = dynamicsMatrices.dfdu.cols(); + + ocs2::OptimalControlProblem problem; + + // System + problem.dynamicsPtr = getOcs2Dynamics(dynamicsMatrices); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(costMatrices)); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(costMatrices)); + + // Reference Managaer + ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Ones(n)}, {ocs2::vector_t::Ones(m)}); + auto referenceManagerPtr = std::make_shared(targetTrajectories); + + problem.targetTrajectoriesPtr = &referenceManagerPtr->getTargetTrajectories(); + + if (emptyConstraint) { + problem.equalityConstraintPtr->add("intermediateCost", ocs2::getOcs2Constraints(getRandomConstraints(n, m, 0))); + } + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.05; + settings.ipmIteration = 10; + settings.useFeedbackPolicy = feedback; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 100; + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = ocs2::vector_t::Ones(n); + + // Construct solver + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Solve + solver.run(startTime, initState, finalTime); + return {solver.primalSolution(finalTime), solver.getIterationsLog()}; +} + +} // namespace +} // namespace ocs2 + +TEST(test_unconstrained, withFeedback) { + int n = 3; + int m = 2; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto solWithEmptyConstraint = ocs2::solveWithFeedbackSetting(true, true, dynamics, costs); + const auto solWithNullConstraint = ocs2::solveWithFeedbackSetting(true, false, dynamics, costs); + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(solWithEmptyConstraint.second.size(), 2); + ASSERT_LE(solWithNullConstraint.second.size(), 2); + ASSERT_LT(solWithEmptyConstraint.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solWithNullConstraint.second.back().dynamicsViolationSSE, tol); + + // Compare + const auto& withEmptyConstraint = solWithEmptyConstraint.first; + const auto& withNullConstraint = solWithNullConstraint.first; + for (int i = 0; i < withEmptyConstraint.timeTrajectory_.size(); i++) { + ASSERT_DOUBLE_EQ(withEmptyConstraint.timeTrajectory_[i], withNullConstraint.timeTrajectory_[i]); + ASSERT_TRUE(withEmptyConstraint.stateTrajectory_[i].isApprox(withNullConstraint.stateTrajectory_[i], tol)); + ASSERT_TRUE(withEmptyConstraint.inputTrajectory_[i].isApprox(withNullConstraint.inputTrajectory_[i], tol)); + const auto t = withEmptyConstraint.timeTrajectory_[i]; + const auto& x = withEmptyConstraint.stateTrajectory_[i]; + ASSERT_TRUE( + withEmptyConstraint.controllerPtr_->computeInput(t, x).isApprox(withNullConstraint.controllerPtr_->computeInput(t, x), tol)); + } +} + +TEST(test_unconstrained, noFeedback) { + int n = 3; + int m = 2; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto solWithEmptyConstraint = ocs2::solveWithFeedbackSetting(false, true, dynamics, costs); + const auto solWithNullConstraint = ocs2::solveWithFeedbackSetting(false, false, dynamics, costs); + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(solWithEmptyConstraint.second.size(), 2); + ASSERT_LE(solWithNullConstraint.second.size(), 2); + ASSERT_LT(solWithEmptyConstraint.second.back().dynamicsViolationSSE, tol); + ASSERT_LT(solWithNullConstraint.second.back().dynamicsViolationSSE, tol); + + // Compare + const auto& withEmptyConstraint = solWithEmptyConstraint.first; + const auto& withNullConstraint = solWithNullConstraint.first; + for (int i = 0; i < withEmptyConstraint.timeTrajectory_.size(); i++) { + ASSERT_DOUBLE_EQ(withEmptyConstraint.timeTrajectory_[i], withNullConstraint.timeTrajectory_[i]); + ASSERT_TRUE(withEmptyConstraint.stateTrajectory_[i].isApprox(withNullConstraint.stateTrajectory_[i], tol)); + ASSERT_TRUE(withEmptyConstraint.inputTrajectory_[i].isApprox(withNullConstraint.inputTrajectory_[i], tol)); + + const auto t = withEmptyConstraint.timeTrajectory_[i]; + const auto& x = withEmptyConstraint.stateTrajectory_[i]; + ASSERT_TRUE( + withEmptyConstraint.controllerPtr_->computeInput(t, x).isApprox(withNullConstraint.controllerPtr_->computeInput(t, x), tol)); + } +} diff --git a/ocs2_ipm/test/testValuefunction.cpp b/ocs2_ipm/test/testValuefunction.cpp new file mode 100644 index 000000000..e121e6505 --- /dev/null +++ b/ocs2_ipm/test/testValuefunction.cpp @@ -0,0 +1,105 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include "ocs2_ipm/IpmSolver.h" + +#include + +#include +#include +#include + +TEST(test_valuefunction, linear_quadratic_problem) { + constexpr int n = 3; + constexpr int m = 2; + constexpr int nc = 1; + constexpr int Nsample = 10; + constexpr ocs2::scalar_t tol = 1e-9; + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t eventTime = 1.0 / 3.0; + const ocs2::scalar_t finalTime = 1.0; + + ocs2::OptimalControlProblem problem; + + // System + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto jumpMap = ocs2::matrix_t::Random(n, n); + problem.dynamicsPtr.reset(new ocs2::LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(ocs2::getRandomCost(n, m))); + problem.preJumpCostPtr->add("eventCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(ocs2::getRandomCost(n, 0))); + + // Reference Manager + const ocs2::ModeSchedule modeSchedule({eventTime}, {0, 1}); + const ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Random(n)}, {ocs2::vector_t::Random(m)}); + auto referenceManagerPtr = std::make_shared(targetTrajectories, modeSchedule); + + problem.targetTrajectoriesPtr = &targetTrajectories; + + // Constraint + problem.equalityConstraintPtr->add("constraint", ocs2::getOcs2Constraints(ocs2::getRandomConstraints(n, m, nc))); + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + ocs2::ipm::Settings settings; + settings.dt = 0.05; + settings.ipmIteration = 1; + settings.printSolverStatistics = false; + settings.printSolverStatus = false; + settings.printLinesearch = false; + settings.useFeedbackPolicy = true; + settings.createValueFunction = true; + + // Set up solver + ocs2::IpmSolver solver(settings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Get value function + const ocs2::vector_t zeroState = ocs2::vector_t::Random(n); + solver.reset(); + solver.run(startTime, zeroState, finalTime); + const auto costToGo = solver.getValueFunction(startTime, zeroState); + const ocs2::scalar_t zeroCost = solver.getPerformanceIndeces().cost; + + // Solve for random states and check consistency with value function + for (int i = 0; i < Nsample; ++i) { + const ocs2::vector_t sampleState = ocs2::vector_t::Random(n); + solver.reset(); + solver.run(startTime, sampleState, finalTime); + const ocs2::scalar_t sampleCost = solver.getPerformanceIndeces().cost; + const ocs2::vector_t dx = sampleState - zeroState; + + EXPECT_NEAR(sampleCost, zeroCost + costToGo.dfdx.dot(dx) + 0.5 * dx.dot(costToGo.dfdxx * dx), tol); + } +} diff --git a/ocs2_mpc/include/ocs2_mpc/MRT_BASE.h b/ocs2_mpc/include/ocs2_mpc/MRT_BASE.h index 217f3c575..3d63a1ab8 100644 --- a/ocs2_mpc/include/ocs2_mpc/MRT_BASE.h +++ b/ocs2_mpc/include/ocs2_mpc/MRT_BASE.h @@ -41,8 +41,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include -#include #include #include "ocs2_mpc/CommandData.h" diff --git a/ocs2_mpc/src/MPC_MRT_Interface.cpp b/ocs2_mpc/src/MPC_MRT_Interface.cpp index b15d90cbc..80b303015 100644 --- a/ocs2_mpc/src/MPC_MRT_Interface.cpp +++ b/ocs2_mpc/src/MPC_MRT_Interface.cpp @@ -117,19 +117,19 @@ void MPC_MRT_Interface::advanceMpc() { /******************************************************************************************************/ void MPC_MRT_Interface::copyToBuffer(const SystemObservation& mpcInitObservation) { // policy - std::unique_ptr primalSolutionPtr(new PrimalSolution); + auto primalSolutionPtr = std::make_unique(); const scalar_t startTime = mpcInitObservation.time; const scalar_t finalTime = (mpc_.settings().solutionTimeWindow_ < 0) ? mpc_.getSolverPtr()->getFinalTime() : startTime + mpc_.settings().solutionTimeWindow_; mpc_.getSolverPtr()->getPrimalSolution(finalTime, primalSolutionPtr.get()); // command - std::unique_ptr commandPtr(new CommandData); + auto commandPtr = std::make_unique(); commandPtr->mpcInitObservation_ = mpcInitObservation; commandPtr->mpcTargetTrajectories_ = mpc_.getSolverPtr()->getReferenceManager().getTargetTrajectories(); // performance indices - std::unique_ptr performanceIndicesPtr(new PerformanceIndex); + auto performanceIndicesPtr = std::make_unique(); *performanceIndicesPtr = mpc_.getSolverPtr()->getPerformanceIndeces(); this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/CMakeLists.txt b/ocs2_mpcnet/ocs2_ballbot_mpcnet/CMakeLists.txt new file mode 100644 index 000000000..d95da9050 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/CMakeLists.txt @@ -0,0 +1,128 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_ballbot_mpcnet) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_ballbot + ocs2_ballbot_ros + ocs2_mpcnet_core +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# main library +add_library(${PROJECT_NAME} + src/BallbotMpcnetDefinition.cpp + src/BallbotMpcnetInterface.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +# python bindings +pybind11_add_module(BallbotMpcnetPybindings SHARED + src/BallbotMpcnetPybindings.cpp +) +add_dependencies(BallbotMpcnetPybindings + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(BallbotMpcnetPybindings PRIVATE + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(BallbotMpcnetPybindings + PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +# MPC-Net dummy node +add_executable(ballbot_mpcnet_dummy + src/BallbotMpcnetDummyNode.cpp +) +add_dependencies(ballbot_mpcnet_dummy + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(ballbot_mpcnet_dummy + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(ballbot_mpcnet_dummy PRIVATE ${OCS2_CXX_FLAGS}) + +catkin_python_setup() + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target ocs2_ballbot_mpcnet") + add_clang_tooling( + TARGETS ${PROJECT_NAME} ballbot_mpcnet_dummy + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR +) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS BallbotMpcnetPybindings + ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +install(TARGETS ballbot_mpcnet_dummy + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch policy + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +# TODO(areske) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h b/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h new file mode 100644 index 000000000..e50cf779f --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h @@ -0,0 +1,71 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +namespace ocs2 { +namespace ballbot { + +/** + * MPC-Net definitions for ballbot. + */ +class BallbotMpcnetDefinition final : public ocs2::mpcnet::MpcnetDefinitionBase { + public: + /** + * Default constructor. + */ + BallbotMpcnetDefinition() = default; + + /** + * Default destructor. + */ + ~BallbotMpcnetDefinition() override = default; + + /** + * @see MpcnetDefinitionBase::getObservation + */ + vector_t getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) override; + + /** + * @see MpcnetDefinitionBase::getActionTransformation + */ + std::pair getActionTransformation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) override; + + /** + * @see MpcnetDefinitionBase::isValid + */ + bool isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) override; +}; + +} // namespace ballbot +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h b/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h new file mode 100644 index 000000000..4456b7ab5 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/include/ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h @@ -0,0 +1,66 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +namespace ocs2 { +namespace ballbot { + +/** + * Ballbot MPC-Net interface between C++ and Python. + */ +class BallbotMpcnetInterface final : public ocs2::mpcnet::MpcnetInterfaceBase { + public: + /** + * Constructor. + * @param [in] nDataGenerationThreads : Number of data generation threads. + * @param [in] nPolicyEvaluationThreads : Number of policy evaluation threads. + * @param [in] raisim : Whether to use RaiSim for the rollouts. + */ + BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim); + + /** + * Default destructor. + */ + ~BallbotMpcnetInterface() override = default; + + private: + /** + * Helper to get the MPC. + * @param [in] ballbotInterface : The ballbot interface. + * @return Pointer to the MPC. + */ + std::unique_ptr getMpc(BallbotInterface& ballbotInterface); +}; + +} // namespace ballbot +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch b/ocs2_mpcnet/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch new file mode 100644 index 000000000..ca53ee47b --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/launch/ballbot_mpcnet.launch @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/package.xml b/ocs2_mpcnet/ocs2_ballbot_mpcnet/package.xml new file mode 100644 index 000000000..c2448bd1b --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/package.xml @@ -0,0 +1,22 @@ + + + ocs2_ballbot_mpcnet + 0.0.0 + The ocs2_ballbot_mpcnet package + + Alexander Reske + + Farbod Farshidian + Alexander Reske + + BSD-3 + + catkin + + cmake_clang_tools + + ocs2_ballbot + ocs2_ballbot_ros + ocs2_mpcnet_core + + diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.onnx b/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.onnx new file mode 100644 index 000000000..56c3a7a8e Binary files /dev/null and b/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.onnx differ diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.pt b/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.pt new file mode 100644 index 000000000..06a0acb59 Binary files /dev/null and b/ocs2_mpcnet/ocs2_ballbot_mpcnet/policy/ballbot.pt differ diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py new file mode 100644 index 000000000..d5c07a503 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/__init__.py @@ -0,0 +1,2 @@ +from ocs2_ballbot_mpcnet.BallbotMpcnetPybindings import MpcnetInterface +from ocs2_ballbot_mpcnet.mpcnet import BallbotMpcnet diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml new file mode 100644 index 000000000..1626c547c --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/config/ballbot.yaml @@ -0,0 +1,123 @@ +# +# general +# +# name of the robot +NAME: "ballbot" +# description of the training run +DESCRIPTION: "description" +# state dimension +STATE_DIM: 10 +# input dimension +INPUT_DIM: 3 +# target trajectories state dimension +TARGET_STATE_DIM: 10 +# target trajectories input dimension +TARGET_INPUT_DIM: 3 +# observation dimension +OBSERVATION_DIM: 10 +# action dimension +ACTION_DIM: 3 +# expert number +EXPERT_NUM: 1 +# default state +DEFAULT_STATE: + - 0.0 # pose x + - 0.0 # pose y + - 0.0 # pose yaw + - 0.0 # pose pitch + - 0.0 # pose roll + - 0.0 # twist x + - 0.0 # twist y + - 0.0 # twist yaw + - 0.0 # twist pitch + - 0.0 # twist roll +# default target state +DEFAULT_TARGET_STATE: + - 0.0 # pose x + - 0.0 # pose y + - 0.0 # pose yaw + - 0.0 # pose pitch + - 0.0 # pose roll + - 0.0 # twist x + - 0.0 # twist y + - 0.0 # twist yaw + - 0.0 # twist pitch + - 0.0 # twist roll +# +# loss +# +# epsilon to improve numerical stability of logs and denominators +EPSILON: 1.e-8 +# whether to cheat by adding the gating loss +CHEATING: False +# parameter to control the relative importance of both loss types +LAMBDA: 1.0 +# dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) +EXPERT_FOR_MODE: + 0: 0 +# input cost for behavioral cloning +R: + - 2.0 # torque + - 2.0 # torque + - 2.0 # torque +# +# memory +# +# capacity of the memory +CAPACITY: 100000 +# +# policy +# +# observation scaling +OBSERVATION_SCALING: + - 1.0 # pose x + - 1.0 # pose y + - 1.0 # pose yaw + - 1.0 # pose pitch + - 1.0 # pose roll + - 1.0 # twist x + - 1.0 # twist y + - 1.0 # twist yaw + - 1.0 # twist pitch + - 1.0 # twist roll +# action scaling +ACTION_SCALING: + - 1.0 # torque + - 1.0 # torque + - 1.0 # torque +# +# rollout +# +# RaiSim or TimeTriggered rollout for data generation and policy evaluation +RAISIM: False +# settings for data generation +DATA_GENERATION_TIME_STEP: 0.1 +DATA_GENERATION_DURATION: 3.0 +DATA_GENERATION_DATA_DECIMATION: 1 +DATA_GENERATION_THREADS: 2 +DATA_GENERATION_TASKS: 10 +DATA_GENERATION_SAMPLES: 2 +DATA_GENERATION_SAMPLING_VARIANCE: + - 0.01 # pose x + - 0.01 # pose y + - 0.01745329251 # pose yaw: 1.0 / 180.0 * pi + - 0.01745329251 # pose pitch: 1.0 / 180.0 * pi + - 0.01745329251 # pose roll: 1.0 / 180.0 * pi + - 0.05 # twist x + - 0.05 # twist y + - 0.08726646259 # twist yaw: 5.0 / 180.0 * pi + - 0.08726646259 # twist pitch: 5.0 / 180.0 * pi + - 0.08726646259 # twist roll: 5.0 / 180.0 * pi +# settings for computing metrics +POLICY_EVALUATION_TIME_STEP: 0.1 +POLICY_EVALUATION_DURATION: 3.0 +POLICY_EVALUATION_THREADS: 1 +POLICY_EVALUATION_TASKS: 5 +# +# training +# +BATCH_SIZE: 32 +LEARNING_RATE: 1.e-2 +LEARNING_ITERATIONS: 10000 +GRADIENT_CLIPPING: False +GRADIENT_CLIPPING_VALUE: 1.0 diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py new file mode 100644 index 000000000..86b8fe743 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/mpcnet.py @@ -0,0 +1,137 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Ballbot MPC-Net class. + +Provides a class that handles the MPC-Net training for ballbot. +""" + +import numpy as np +from typing import Tuple + +from ocs2_mpcnet_core import helper +from ocs2_mpcnet_core import mpcnet +from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray + + +class BallbotMpcnet(mpcnet.Mpcnet): + """Ballbot MPC-Net. + + Adds robot-specific methods for the MPC-Net training. + """ + + @staticmethod + def get_default_event_times_and_mode_sequence(duration: float) -> Tuple[np.ndarray, np.ndarray]: + """Get the event times and mode sequence describing the default mode schedule. + + Creates the default event times and mode sequence for a certain time duration. + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + event_times_template = np.array([1.0], dtype=np.float64) + mode_sequence_template = np.array([0], dtype=np.uintp) + return helper.get_event_times_and_mode_sequence(0, duration, event_times_template, mode_sequence_template) + + def get_random_initial_state(self) -> np.ndarray: + """Get a random initial state. + + Samples a random initial state for the robot. + + Returns: + x: A random initial state given by a NumPy array containing floats. + """ + max_linear_velocity_x = 0.5 + max_linear_velocity_y = 0.5 + max_euler_angle_derivative_z = 45.0 / 180.0 * np.pi + max_euler_angle_derivative_y = 45.0 / 180.0 * np.pi + max_euler_angle_derivative_x = 45.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.STATE_DIM) + random_deviation[5] = np.random.uniform(-max_linear_velocity_x, max_linear_velocity_x) + random_deviation[6] = np.random.uniform(-max_linear_velocity_y, max_linear_velocity_y) + random_deviation[7] = np.random.uniform(-max_euler_angle_derivative_z, max_euler_angle_derivative_z) + random_deviation[8] = np.random.uniform(-max_euler_angle_derivative_y, max_euler_angle_derivative_y) + random_deviation[9] = np.random.uniform(-max_euler_angle_derivative_x, max_euler_angle_derivative_x) + return np.array(self.config.DEFAULT_STATE) + random_deviation + + def get_random_target_state(self) -> np.ndarray: + """Get a random target state. + + Samples a random target state for the robot. + + Returns: + x: A random target state given by a NumPy array containing floats. + """ + max_position_x = 1.0 + max_position_y = 1.0 + max_orientation_z = 45.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.TARGET_STATE_DIM) + random_deviation[0] = np.random.uniform(-max_position_x, max_position_x) + random_deviation[1] = np.random.uniform(-max_position_y, max_position_y) + random_deviation[2] = np.random.uniform(-max_orientation_z, max_orientation_z) + return np.array(self.config.DEFAULT_TARGET_STATE) + random_deviation + + def get_tasks( + self, tasks_number: int, duration: float + ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: + """Get tasks. + + Get a random set of task that should be executed by the data generation or policy evaluation. + + Args: + tasks_number: Number of tasks given by an integer. + duration: Duration of each task given by a float. + + Returns: + A tuple containing the components of the task. + - initial_observations: The initial observations given by an OCS2 system observation array. + - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. + - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. + """ + initial_mode = 0 + initial_time = 0.0 + initial_observations = helper.get_system_observation_array(tasks_number) + mode_schedules = helper.get_mode_schedule_array(tasks_number) + target_trajectories = helper.get_target_trajectories_array(tasks_number) + for i in range(tasks_number): + initial_observations[i] = helper.get_system_observation( + initial_mode, initial_time, self.get_random_initial_state(), np.zeros(self.config.INPUT_DIM) + ) + mode_schedules[i] = helper.get_mode_schedule(*self.get_default_event_times_and_mode_sequence(duration)) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + self.get_random_target_state().reshape((1, self.config.TARGET_STATE_DIM)), + np.zeros((1, self.config.TARGET_INPUT_DIM)), + ) + return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py new file mode 100644 index 000000000..4fa5c59ce --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/python/ocs2_ballbot_mpcnet/train.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 + +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Ballbot MPC-Net. + +Main script for training an MPC-Net policy for ballbot. +""" + +import sys +import os + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss import HamiltonianLoss +from ocs2_mpcnet_core.memory import CircularMemory +from ocs2_mpcnet_core.policy import LinearPolicy + +from ocs2_ballbot_mpcnet import BallbotMpcnet +from ocs2_ballbot_mpcnet import MpcnetInterface + + +def main(root_dir: str, config_file_name: str) -> None: + # config + config = Config(os.path.join(root_dir, "config", config_file_name)) + # interface + interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) + # loss + loss = HamiltonianLoss(config) + # memory + memory = CircularMemory(config) + # policy + policy = LinearPolicy(config) + # mpcnet + mpcnet = BallbotMpcnet(root_dir, config, interface, memory, policy, loss) + # train + mpcnet.train() + + +if __name__ == "__main__": + root_dir = os.path.dirname(os.path.abspath(__file__)) + if len(sys.argv) > 1: + main(root_dir, sys.argv[1]) + else: + main(root_dir, "ballbot.yaml") diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/setup.py b/ocs2_mpcnet/ocs2_ballbot_mpcnet/setup.py new file mode 100644 index 000000000..959f0cad9 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/setup.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup(packages=["ocs2_ballbot_mpcnet"], package_dir={"": "python"}) + +setup(**setup_args) diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp new file mode 100644 index 000000000..81ec0bc06 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDefinition.cpp @@ -0,0 +1,57 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h" + +namespace ocs2 { +namespace ballbot { + +vector_t BallbotMpcnetDefinition::getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + vector_t observation = x - targetTrajectories.getDesiredState(t); + const Eigen::Matrix R = + (Eigen::Matrix() << cos(x(2)), -sin(x(2)), sin(x(2)), cos(x(2))).finished().transpose(); + observation.segment<2>(0) = R * observation.segment<2>(0); + observation.segment<2>(5) = R * observation.segment<2>(5); + return observation; +} + +std::pair BallbotMpcnetDefinition::getActionTransformation(scalar_t t, const vector_t& x, + const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + return {matrix_t::Identity(3, 3), vector_t::Zero(3)}; +} + +bool BallbotMpcnetDefinition::isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + return true; +} + +} // namespace ballbot +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp new file mode 100644 index 000000000..012a178c0 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetDummyNode.cpp @@ -0,0 +1,109 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include + +#include +#include +#include +#include +#include +#include + +#include "ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h" + +using namespace ocs2; +using namespace ballbot; + +int main(int argc, char** argv) { + const std::string robotName = "ballbot"; + + // task and policy file + std::vector programArgs{}; + ::ros::removeROSArgs(argc, argv, programArgs); + if (programArgs.size() <= 2) { + throw std::runtime_error("No task name and policy file path specified. Aborting."); + } + const std::string taskFileFolderName = std::string(programArgs[1]); + const std::string policyFilePath = std::string(programArgs[2]); + + // initialize ros node + ros::init(argc, argv, robotName + "_mpcnet_dummy"); + ros::NodeHandle nodeHandle; + + // ballbot interface + const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; + const std::string libraryFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + BallbotInterface ballbotInterface(taskFile, libraryFolder); + + // ROS reference manager + auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // policy (MPC-Net controller) + auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); + auto mpcnetDefinitionPtr = std::make_shared(); + auto mpcnetControllerPtr = + std::make_unique(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr); + mpcnetControllerPtr->loadPolicyModel(policyFilePath); + + // rollout + std::unique_ptr rolloutPtr(ballbotInterface.getRollout().clone()); + + // observer + auto mpcnetDummyObserverRosPtr = std::make_shared(nodeHandle, robotName); + + // visualization + auto ballbotDummyVisualization = std::make_shared(nodeHandle); + + // MPC-Net dummy loop ROS + const scalar_t controlFrequency = ballbotInterface.mpcSettings().mrtDesiredFrequency_; + const scalar_t rosFrequency = ballbotInterface.mpcSettings().mpcDesiredFrequency_; + ocs2::mpcnet::MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), + rosReferenceManagerPtr); + mpcnetDummyLoopRos.addObserver(mpcnetDummyObserverRosPtr); + mpcnetDummyLoopRos.addObserver(ballbotDummyVisualization); + + // initial system observation + SystemObservation systemObservation; + systemObservation.mode = 0; + systemObservation.time = 0.0; + systemObservation.state = ballbotInterface.getInitialState(); + systemObservation.input = vector_t::Zero(ocs2::ballbot::INPUT_DIM); + + // initial target trajectories + const TargetTrajectories targetTrajectories({systemObservation.time}, {systemObservation.state}, {systemObservation.input}); + + // run MPC-Net dummy loop ROS + mpcnetDummyLoopRos.run(systemObservation, targetTrajectories); + + // successful exit + return 0; +} \ No newline at end of file diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp new file mode 100644 index 000000000..9a3da6748 --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetInterface.cpp @@ -0,0 +1,111 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h" + +#include + +#include +#include +#include +#include + +#include "ocs2_ballbot_mpcnet/BallbotMpcnetDefinition.h" + +namespace ocs2 { +namespace ballbot { + +BallbotMpcnetInterface::BallbotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim) { + // create ONNX environment + auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); + // path to config file + const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/mpc/task.info"; + // path to save auto-generated libraries + const std::string libraryFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + // set up MPC-Net rollout manager for data generation and policy evaluation + std::vector> mpcPtrs; + std::vector> mpcnetPtrs; + std::vector> rolloutPtrs; + std::vector> mpcnetDefinitionPtrs; + std::vector> referenceManagerPtrs; + mpcPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + mpcnetPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + rolloutPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + mpcnetDefinitionPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { + BallbotInterface ballbotInterface(taskFile, libraryFolder); + auto mpcnetDefinitionPtr = std::make_shared(); + mpcPtrs.push_back(getMpc(ballbotInterface)); + mpcnetPtrs.push_back(std::make_unique( + mpcnetDefinitionPtr, ballbotInterface.getReferenceManagerPtr(), onnxEnvironmentPtr)); + if (raisim) { + throw std::runtime_error("[BallbotMpcnetInterface::BallbotMpcnetInterface] raisim rollout not yet implemented for ballbot."); + } else { + rolloutPtrs.push_back(std::unique_ptr(ballbotInterface.getRollout().clone())); + } + mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr); + referenceManagerPtrs.push_back(ballbotInterface.getReferenceManagerPtr()); + } + mpcnetRolloutManagerPtr_.reset(new ocs2::mpcnet::MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, + std::move(mpcPtrs), std::move(mpcnetPtrs), std::move(rolloutPtrs), + mpcnetDefinitionPtrs, referenceManagerPtrs)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::unique_ptr BallbotMpcnetInterface::getMpc(BallbotInterface& ballbotInterface) { + // ensure MPC and DDP settings are as needed for MPC-Net + const auto mpcSettings = [&]() { + auto settings = ballbotInterface.mpcSettings(); + settings.debugPrint_ = false; + settings.coldStart_ = false; + return settings; + }(); + const auto ddpSettings = [&]() { + auto settings = ballbotInterface.ddpSettings(); + settings.algorithm_ = ocs2::ddp::Algorithm::SLQ; + settings.nThreads_ = 1; + settings.displayInfo_ = false; + settings.displayShortSummary_ = false; + settings.checkNumericalStability_ = false; + settings.debugPrintRollout_ = false; + settings.useFeedbackPolicy_ = true; + return settings; + }(); + // create one MPC instance + auto mpcPtr = std::make_unique(mpcSettings, ddpSettings, ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); + mpcPtr->getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); + return mpcPtr; +} + +} // namespace ballbot +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp new file mode 100644 index 000000000..c08e80a5d --- /dev/null +++ b/ocs2_mpcnet/ocs2_ballbot_mpcnet/src/BallbotMpcnetPybindings.cpp @@ -0,0 +1,34 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include "ocs2_ballbot_mpcnet/BallbotMpcnetInterface.h" + +CREATE_ROBOT_MPCNET_PYTHON_BINDINGS(ocs2::ballbot::BallbotMpcnetInterface, BallbotMpcnetPybindings) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/CMakeLists.txt b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/CMakeLists.txt new file mode 100644 index 000000000..6addac114 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/CMakeLists.txt @@ -0,0 +1,129 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_legged_robot_mpcnet) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_legged_robot + ocs2_legged_robot_raisim + ocs2_legged_robot_ros + ocs2_mpcnet_core +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# main library +add_library(${PROJECT_NAME} + src/LeggedRobotMpcnetDefinition.cpp + src/LeggedRobotMpcnetInterface.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +# python bindings +pybind11_add_module(LeggedRobotMpcnetPybindings SHARED + src/LeggedRobotMpcnetPybindings.cpp +) +add_dependencies(LeggedRobotMpcnetPybindings + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(LeggedRobotMpcnetPybindings PRIVATE + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(LeggedRobotMpcnetPybindings + PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +# MPC-Net dummy node +add_executable(legged_robot_mpcnet_dummy + src/LeggedRobotMpcnetDummyNode.cpp +) +add_dependencies(legged_robot_mpcnet_dummy + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(legged_robot_mpcnet_dummy + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(legged_robot_mpcnet_dummy PRIVATE ${OCS2_CXX_FLAGS}) + +catkin_python_setup() + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target ocs2_legged_robot_mpcnet") + add_clang_tooling( + TARGETS ${PROJECT_NAME} legged_robot_mpcnet_dummy + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR +) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS LeggedRobotMpcnetPybindings + ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +install(TARGETS legged_robot_mpcnet_dummy + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY launch policy + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +# TODO(areske) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h new file mode 100644 index 000000000..8a6b44f92 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h @@ -0,0 +1,81 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +namespace ocs2 { +namespace legged_robot { + +/** + * MPC-Net definitions for legged robot. + */ +class LeggedRobotMpcnetDefinition final : public ocs2::mpcnet::MpcnetDefinitionBase { + public: + /** + * Constructor. + * @param [in] leggedRobotInterface : Legged robot interface. + */ + LeggedRobotMpcnetDefinition(const LeggedRobotInterface& leggedRobotInterface) + : defaultState_(leggedRobotInterface.getInitialState()), centroidalModelInfo_(leggedRobotInterface.getCentroidalModelInfo()) {} + + /** + * Default destructor. + */ + ~LeggedRobotMpcnetDefinition() override = default; + + /** + * @see MpcnetDefinitionBase::getObservation + */ + vector_t getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) override; + + /** + * @see MpcnetDefinitionBase::getActionTransformation + */ + std::pair getActionTransformation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) override; + + /** + * @see MpcnetDefinitionBase::isValid + */ + bool isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) override; + + private: + const scalar_t allowedHeightDeviation_ = 0.2; + const scalar_t allowedPitchDeviation_ = 30.0 * M_PI / 180.0; + const scalar_t allowedRollDeviation_ = 30.0 * M_PI / 180.0; + const vector_t defaultState_; + const CentroidalModelInfo centroidalModelInfo_; +}; + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h new file mode 100644 index 000000000..3acb1d0ee --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/include/ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h @@ -0,0 +1,72 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace ocs2 { +namespace legged_robot { + +/** + * Legged robot MPC-Net interface between C++ and Python. + */ +class LeggedRobotMpcnetInterface final : public ocs2::mpcnet::MpcnetInterfaceBase { + public: + /** + * Constructor. + * @param [in] nDataGenerationThreads : Number of data generation threads. + * @param [in] nPolicyEvaluationThreads : Number of policy evaluation threads. + * @param [in] raisim : Whether to use RaiSim for the rollouts. + */ + LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim); + + /** + * Default destructor. + */ + ~LeggedRobotMpcnetInterface() override = default; + + private: + /** + * Helper to get the MPC. + * @param [in] leggedRobotInterface : The legged robot interface. + * @return Pointer to the MPC. + */ + std::unique_ptr getMpc(LeggedRobotInterface& leggedRobotInterface); + + // Legged robot interface pointers (keep alive for Pinocchio interface) + std::vector> leggedRobotInterfacePtrs_; + // Legged robot RaiSim conversions pointers (keep alive for RaiSim rollout) + std::vector> leggedRobotRaisimConversionsPtrs_; +}; + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch new file mode 100644 index 000000000..59311840a --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/launch/legged_robot_mpcnet.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/package.xml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/package.xml new file mode 100644 index 000000000..c39765d14 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/package.xml @@ -0,0 +1,23 @@ + + + ocs2_legged_robot_mpcnet + 0.0.0 + The ocs2_legged_robot_mpcnet package + + Alexander Reske + + Farbod Farshidian + Alexander Reske + + BSD-3 + + catkin + + cmake_clang_tools + + ocs2_legged_robot + ocs2_legged_robot_raisim + ocs2_legged_robot_ros + ocs2_mpcnet_core + + diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx new file mode 100644 index 000000000..ef5939cdf Binary files /dev/null and b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.onnx differ diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt new file mode 100644 index 000000000..3eb2df09d Binary files /dev/null and b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/policy/legged_robot.pt differ diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py new file mode 100644 index 000000000..688527a67 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/__init__.py @@ -0,0 +1,2 @@ +from ocs2_legged_robot_mpcnet.LeggedRobotMpcnetPybindings import MpcnetInterface +from ocs2_legged_robot_mpcnet.mpcnet import LeggedRobotMpcnet diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml new file mode 100644 index 000000000..0b6dde905 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/config/legged_robot.yaml @@ -0,0 +1,240 @@ +# +# general +# +# name of the robot +NAME: "legged_robot" +# description of the training run +DESCRIPTION: "description" +# state dimension +STATE_DIM: 24 +# input dimension +INPUT_DIM: 24 +# target trajectories state dimension +TARGET_STATE_DIM: 24 +# target trajectories input dimension +TARGET_INPUT_DIM: 24 +# observation dimension +OBSERVATION_DIM: 36 +# action dimension +ACTION_DIM: 24 +# expert number +EXPERT_NUM: 3 +# default state +DEFAULT_STATE: + - 0.0 # normalized linear momentum x + - 0.0 # normalized linear momentum y + - 0.0 # normalized linear momentum z + - 0.0 # normalized angular momentum x + - 0.0 # normalized angular momentum y + - 0.0 # normalized angular momentum z + - 0.0 # position x + - 0.0 # position y + - 0.575 # position z + - 0.0 # orientation z + - 0.0 # orientation y + - 0.0 # orientation x + - -0.25 # joint position LF HAA + - 0.6 # joint position LF HFE + - -0.85 # joint position LF KFE + - -0.25 # joint position LH HAA + - -0.6 # joint position LH HFE + - 0.85 # joint position LH KFE + - 0.25 # joint position RF HAA + - 0.6 # joint position RF HFE + - -0.85 # joint position RF KFE + - 0.25 # joint position RH HAA + - -0.6 # joint position RH HFE + - 0.85 # joint position RH KFE +# default target state +DEFAULT_TARGET_STATE: + - 0.0 # normalized linear momentum x + - 0.0 # normalized linear momentum y + - 0.0 # normalized linear momentum z + - 0.0 # normalized angular momentum x + - 0.0 # normalized angular momentum y + - 0.0 # normalized angular momentum z + - 0.0 # position x + - 0.0 # position y + - 0.575 # position z + - 0.0 # orientation z + - 0.0 # orientation y + - 0.0 # orientation x + - -0.25 # joint position LF HAA + - 0.6 # joint position LF HFE + - -0.85 # joint position LF KFE + - -0.25 # joint position LH HAA + - -0.6 # joint position LH HFE + - 0.85 # joint position LH KFE + - 0.25 # joint position RF HAA + - 0.6 # joint position RF HFE + - -0.85 # joint position RF KFE + - 0.25 # joint position RH HAA + - -0.6 # joint position RH HFE + - 0.85 # joint position RH KFE +# +# loss +# +# epsilon to improve numerical stability of logs and denominators +EPSILON: 1.e-8 +# whether to cheat by adding the gating loss +CHEATING: True +# parameter to control the relative importance of both loss types +LAMBDA: 1.0 +# dictionary for the gating loss (assigns modes to experts responsible for the corresponding contact configuration) +EXPERT_FOR_MODE: + 6: 1 # trot + 9: 2 # trot + 15: 0 # stance +# input cost for behavioral cloning +R: + - 0.001 # contact force LF x + - 0.001 # contact force LF y + - 0.001 # contact force LF z + - 0.001 # contact force LH x + - 0.001 # contact force LH y + - 0.001 # contact force LH z + - 0.001 # contact force RF x + - 0.001 # contact force RF y + - 0.001 # contact force RF z + - 0.001 # contact force RH x + - 0.001 # contact force RH y + - 0.001 # contact force RH z + - 5.0 # joint velocity LF HAA + - 5.0 # joint velocity LF HFE + - 5.0 # joint velocity LF KFE + - 5.0 # joint velocity LH HAA + - 5.0 # joint velocity LH HFE + - 5.0 # joint velocity LH KFE + - 5.0 # joint velocity RF HAA + - 5.0 # joint velocity RF HFE + - 5.0 # joint velocity RF KFE + - 5.0 # joint velocity RH HAA + - 5.0 # joint velocity RH HFE + - 5.0 # joint velocity RH KFE +# +# memory +# +# capacity of the memory +CAPACITY: 400000 +# +# policy +# +# observation scaling +OBSERVATION_SCALING: + - 1.0 # swing phase LF + - 1.0 # swing phase LH + - 1.0 # swing phase RF + - 1.0 # swing phase RH + - 1.0 # swing phase rate LF + - 1.0 # swing phase rate LH + - 1.0 # swing phase rate RF + - 1.0 # swing phase rate RH + - 1.0 # sinusoidal bump LF + - 1.0 # sinusoidal bump LH + - 1.0 # sinusoidal bump RF + - 1.0 # sinusoidal bump RH + - 1.0 # normalized linear momentum x + - 1.0 # normalized linear momentum y + - 1.0 # normalized linear momentum z + - 1.0 # normalized angular momentum x + - 1.0 # normalized angular momentum y + - 1.0 # normalized angular momentum z + - 1.0 # position x + - 1.0 # position y + - 1.0 # position z + - 1.0 # orientation z + - 1.0 # orientation y + - 1.0 # orientation x + - 1.0 # joint position LF HAA + - 1.0 # joint position LF HFE + - 1.0 # joint position LF KFE + - 1.0 # joint position LH HAA + - 1.0 # joint position LH HFE + - 1.0 # joint position LH KFE + - 1.0 # joint position RF HAA + - 1.0 # joint position RF HFE + - 1.0 # joint position RF KFE + - 1.0 # joint position RH HAA + - 1.0 # joint position RH HFE + - 1.0 # joint position RH KFE +# action scaling +ACTION_SCALING: + - 100.0 # contact force LF x + - 100.0 # contact force LF y + - 100.0 # contact force LF z + - 100.0 # contact force LH x + - 100.0 # contact force LH y + - 100.0 # contact force LH z + - 100.0 # contact force RF x + - 100.0 # contact force RF y + - 100.0 # contact force RF z + - 100.0 # contact force RH x + - 100.0 # contact force RH y + - 100.0 # contact force RH z + - 10.0 # joint velocity LF HAA + - 10.0 # joint velocity LF HFE + - 10.0 # joint velocity LF KFE + - 10.0 # joint velocity LH HAA + - 10.0 # joint velocity LH HFE + - 10.0 # joint velocity LH KFE + - 10.0 # joint velocity RF HAA + - 10.0 # joint velocity RF HFE + - 10.0 # joint velocity RF KFE + - 10.0 # joint velocity RH HAA + - 10.0 # joint velocity RH HFE + - 10.0 # joint velocity RH KFE +# +# rollout +# +# RaiSim or TimeTriggered rollout for data generation and policy evaluation +RAISIM: True +# weights defining how often a gait is chosen for rollout +WEIGHTS_FOR_GAITS: + stance: 1.0 + trot_1: 2.0 + trot_2: 2.0 +# settings for data generation +DATA_GENERATION_TIME_STEP: 0.0025 +DATA_GENERATION_DURATION: 4.0 +DATA_GENERATION_DATA_DECIMATION: 4 +DATA_GENERATION_THREADS: 12 +DATA_GENERATION_TASKS: 12 +DATA_GENERATION_SAMPLES: 2 +DATA_GENERATION_SAMPLING_VARIANCE: + - 0.05 # normalized linear momentum x + - 0.05 # normalized linear momentum y + - 0.05 # normalized linear momentum z + - 0.00135648942 # normalized angular momentum x: 1.62079 / 52.1348 * 2.5 / 180.0 * pi + - 0.00404705526 # normalized angular momentum y: 4.83559 / 52.1348 * 2.5 / 180.0 * pi + - 0.00395351148 # normalized angular momentum z: 4.72382 / 52.1348 * 2.5 / 180.0 * pi + - 0.01 # position x + - 0.01 # position y + - 0.01 # position z + - 0.00872664625 # orientation z: 0.5 / 180.0 * pi + - 0.00872664625 # orientation y: 0.5 / 180.0 * pi + - 0.00872664625 # orientation x: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LF KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position LH KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RF KFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH HAA: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH HFE: 0.5 / 180.0 * pi + - 0.00872664625 # joint position RH KFE: 0.5 / 180.0 * pi +# settings for computing metrics +POLICY_EVALUATION_TIME_STEP: 0.0025 +POLICY_EVALUATION_DURATION: 4.0 +POLICY_EVALUATION_THREADS: 3 +POLICY_EVALUATION_TASKS: 3 +# +# training +# +BATCH_SIZE: 128 +LEARNING_RATE: 1.e-3 +LEARNING_ITERATIONS: 100000 +GRADIENT_CLIPPING: True +GRADIENT_CLIPPING_VALUE: 1.0 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py new file mode 100644 index 000000000..213b600f8 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/mpcnet.py @@ -0,0 +1,254 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Legged robot MPC-Net class. + +Provides a class that handles the MPC-Net training for legged robot. +""" + +import random +import numpy as np +from typing import Tuple + +from ocs2_mpcnet_core import helper +from ocs2_mpcnet_core import mpcnet +from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray + + +class LeggedRobotMpcnet(mpcnet.Mpcnet): + """Legged robot MPC-Net. + + Adds robot-specific methods for the MPC-Net training. + """ + + @staticmethod + def get_stance(duration: float) -> Tuple[np.ndarray, np.ndarray]: + """Get the stance gait. + + Creates the stance event times and mode sequence for a certain time duration: + - contact schedule: STANCE + - swing schedule: - + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + event_times_template = np.array([1.0], dtype=np.float64) + mode_sequence_template = np.array([15], dtype=np.uintp) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) + + def get_random_initial_state_stance(self) -> np.ndarray: + """Get a random initial state for stance. + + Samples a random initial state for the robot in the stance gait. + + Returns: + x: A random initial state given by a NumPy array containing floats. + """ + max_normalized_linear_momentum_x = 0.1 + max_normalized_linear_momentum_y = 0.1 + max_normalized_linear_momentum_z = 0.1 + max_normalized_angular_momentum_x = 1.62079 / 52.1348 * 30.0 / 180.0 * np.pi + max_normalized_angular_momentum_y = 4.83559 / 52.1348 * 30.0 / 180.0 * np.pi + max_normalized_angular_momentum_z = 4.72382 / 52.1348 * 30.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.STATE_DIM) + random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) + random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) + random_deviation[2] = np.random.uniform( + -max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0 + ) + random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) + random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) + random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) + return np.array(self.config.DEFAULT_STATE) + random_deviation + + def get_random_target_state_stance(self) -> np.ndarray: + """Get a random target state for stance. + + Samples a random target state for the robot in the stance gait. + + Returns: + x: A random target state given by a NumPy array containing floats. + """ + max_position_z = 0.075 + max_orientation_z = 25.0 / 180.0 * np.pi + max_orientation_y = 15.0 / 180.0 * np.pi + max_orientation_x = 25.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.TARGET_STATE_DIM) + random_deviation[8] = np.random.uniform(-max_position_z, max_position_z) + random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) + random_deviation[10] = np.random.uniform(-max_orientation_y, max_orientation_y) + random_deviation[11] = np.random.uniform(-max_orientation_x, max_orientation_x) + return np.array(self.config.DEFAULT_TARGET_STATE) + random_deviation + + @staticmethod + def get_trot_1(duration: float) -> Tuple[np.ndarray, np.ndarray]: + """Get the first trot gait. + + Creates the first trot event times and mode sequence for a certain time duration: + - contact schedule: LF_RH, RF_LH + - swing schedule: RF_LH, LF_RH + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + event_times_template = np.array([0.35, 0.7], dtype=np.float64) + mode_sequence_template = np.array([9, 6], dtype=np.uintp) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) + + @staticmethod + def get_trot_2(duration: float) -> Tuple[np.ndarray, np.ndarray]: + """Get the second trot gait. + + Creates the second trot event times and mode sequence for a certain time duration: + - contact schedule: RF_LH, LF_RH + - swing schedule: LF_RH, RF_LH + + Args: + duration: The duration of the mode schedule given by a float. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + event_times_template = np.array([0.35, 0.7], dtype=np.float64) + mode_sequence_template = np.array([6, 9], dtype=np.uintp) + return helper.get_event_times_and_mode_sequence(15, duration, event_times_template, mode_sequence_template) + + def get_random_initial_state_trot(self) -> np.ndarray: + """Get a random initial state for trot. + + Samples a random initial state for the robot in a trot gait. + + Returns: + x: A random initial state given by a NumPy array containing floats. + """ + max_normalized_linear_momentum_x = 0.5 + max_normalized_linear_momentum_y = 0.25 + max_normalized_linear_momentum_z = 0.25 + max_normalized_angular_momentum_x = 1.62079 / 52.1348 * 60.0 / 180.0 * np.pi + max_normalized_angular_momentum_y = 4.83559 / 52.1348 * 60.0 / 180.0 * np.pi + max_normalized_angular_momentum_z = 4.72382 / 52.1348 * 35.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.STATE_DIM) + random_deviation[0] = np.random.uniform(-max_normalized_linear_momentum_x, max_normalized_linear_momentum_x) + random_deviation[1] = np.random.uniform(-max_normalized_linear_momentum_y, max_normalized_linear_momentum_y) + random_deviation[2] = np.random.uniform( + -max_normalized_linear_momentum_z, max_normalized_linear_momentum_z / 2.0 + ) + random_deviation[3] = np.random.uniform(-max_normalized_angular_momentum_x, max_normalized_angular_momentum_x) + random_deviation[4] = np.random.uniform(-max_normalized_angular_momentum_y, max_normalized_angular_momentum_y) + random_deviation[5] = np.random.uniform(-max_normalized_angular_momentum_z, max_normalized_angular_momentum_z) + return np.array(self.config.DEFAULT_STATE) + random_deviation + + def get_random_target_state_trot(self) -> np.ndarray: + """Get a random target state for trot. + + Samples a random target state for the robot in a trot gait. + + Returns: + x: A random target state given by a NumPy array containing floats. + """ + max_position_x = 0.3 + max_position_y = 0.15 + max_orientation_z = 30.0 / 180.0 * np.pi + random_deviation = np.zeros(self.config.TARGET_STATE_DIM) + random_deviation[6] = np.random.uniform(-max_position_x, max_position_x) + random_deviation[7] = np.random.uniform(-max_position_y, max_position_y) + random_deviation[9] = np.random.uniform(-max_orientation_z, max_orientation_z) + return np.array(self.config.DEFAULT_TARGET_STATE) + random_deviation + + def get_tasks( + self, tasks_number: int, duration: float + ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: + """Get tasks. + + Get a random set of task that should be executed by the data generation or policy evaluation. + + Args: + tasks_number: Number of tasks given by an integer. + duration: Duration of each task given by a float. + + Returns: + A tuple containing the components of the task. + - initial_observations: The initial observations given by an OCS2 system observation array. + - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. + - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. + """ + initial_mode = 15 + initial_time = 0.0 + initial_observations = helper.get_system_observation_array(tasks_number) + mode_schedules = helper.get_mode_schedule_array(tasks_number) + target_trajectories = helper.get_target_trajectories_array(tasks_number) + choices = random.choices( + list(self.config.WEIGHTS_FOR_GAITS.keys()), + k=tasks_number, + weights=list(self.config.WEIGHTS_FOR_GAITS.values()), + ) + for i in range(tasks_number): + if choices[i] == "stance": + initial_observations[i] = helper.get_system_observation( + initial_mode, initial_time, self.get_random_initial_state_stance(), np.zeros(self.config.INPUT_DIM) + ) + mode_schedules[i] = helper.get_mode_schedule(*self.get_stance(duration)) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + self.get_random_target_state_stance().reshape((1, self.config.TARGET_STATE_DIM)), + np.zeros((1, self.config.TARGET_INPUT_DIM)), + ) + elif choices[i] == "trot_1": + initial_observations[i] = helper.get_system_observation( + initial_mode, initial_time, self.get_random_initial_state_trot(), np.zeros(self.config.INPUT_DIM) + ) + mode_schedules[i] = helper.get_mode_schedule(*self.get_trot_1(duration)) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + self.get_random_target_state_trot().reshape((1, self.config.TARGET_STATE_DIM)), + np.zeros((1, self.config.TARGET_INPUT_DIM)), + ) + elif choices[i] == "trot_2": + initial_observations[i] = helper.get_system_observation( + initial_mode, initial_time, self.get_random_initial_state_trot(), np.zeros(self.config.INPUT_DIM) + ) + mode_schedules[i] = helper.get_mode_schedule(*self.get_trot_2(duration)) + target_trajectories[i] = helper.get_target_trajectories( + duration * np.ones((1, 1)), + self.get_random_target_state_trot().reshape((1, self.config.TARGET_STATE_DIM)), + np.zeros((1, self.config.TARGET_INPUT_DIM)), + ) + return initial_observations, mode_schedules, target_trajectories diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py new file mode 100644 index 000000000..f00533bba --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/python/ocs2_legged_robot_mpcnet/train.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 + +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Legged robot MPC-Net. + +Main script for training an MPC-Net policy for legged robot. +""" + +import sys +import os + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss import HamiltonianLoss +from ocs2_mpcnet_core.loss import CrossEntropyLoss +from ocs2_mpcnet_core.memory import CircularMemory +from ocs2_mpcnet_core.policy import MixtureOfNonlinearExpertsPolicy + +from ocs2_legged_robot_mpcnet import LeggedRobotMpcnet +from ocs2_legged_robot_mpcnet import MpcnetInterface + + +def main(root_dir: str, config_file_name: str) -> None: + # config + config = Config(os.path.join(root_dir, "config", config_file_name)) + # interface + interface = MpcnetInterface(config.DATA_GENERATION_THREADS, config.POLICY_EVALUATION_THREADS, config.RAISIM) + # loss + experts_loss = HamiltonianLoss(config) + gating_loss = CrossEntropyLoss(config) + # memory + memory = CircularMemory(config) + # policy + policy = MixtureOfNonlinearExpertsPolicy(config) + # mpcnet + mpcnet = LeggedRobotMpcnet(root_dir, config, interface, memory, policy, experts_loss, gating_loss) + # train + mpcnet.train() + + +if __name__ == "__main__": + root_dir = os.path.dirname(os.path.abspath(__file__)) + if len(sys.argv) > 1: + main(root_dir, sys.argv[1]) + else: + main(root_dir, "legged_robot.yaml") diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/setup.py b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/setup.py new file mode 100644 index 000000000..2227298c6 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/setup.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup(packages=["ocs2_legged_robot_mpcnet"], package_dir={"": "python"}) + +setup(**setup_args) diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp new file mode 100644 index 000000000..8c19df882 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDefinition.cpp @@ -0,0 +1,123 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" + +#include + +#include +#include +#include +#include + +namespace ocs2 { +namespace legged_robot { + +vector_t LeggedRobotMpcnetDefinition::getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + /** + * generalized time + */ + const feet_array_t swingPhasePerLeg = getSwingPhasePerLeg(t, modeSchedule); + vector_t generalizedTime(3 * swingPhasePerLeg.size()); + // phase + for (int i = 0; i < swingPhasePerLeg.size(); i++) { + if (swingPhasePerLeg[i].phase < 0.0) { + generalizedTime[i] = 0.0; + } else { + generalizedTime[i] = swingPhasePerLeg[i].phase; + } + } + // phase rate + for (int i = 0; i < swingPhasePerLeg.size(); i++) { + if (swingPhasePerLeg[i].phase < 0.0) { + generalizedTime[i + swingPhasePerLeg.size()] = 0.0; + } else { + generalizedTime[i + swingPhasePerLeg.size()] = 1.0 / swingPhasePerLeg[i].duration; + } + } + // sin(pi * phase) + for (int i = 0; i < swingPhasePerLeg.size(); i++) { + if (swingPhasePerLeg[i].phase < 0.0) { + generalizedTime[i + 2 * swingPhasePerLeg.size()] = 0.0; + } else { + generalizedTime[i + 2 * swingPhasePerLeg.size()] = std::sin(M_PI * swingPhasePerLeg[i].phase); + } + } + /** + * relative state + */ + vector_t relativeState = x - targetTrajectories.getDesiredState(t); + const matrix3_t R = getRotationMatrixFromZyxEulerAngles(x.segment<3>(9)).transpose(); + relativeState.segment<3>(0) = R * relativeState.segment<3>(0); + relativeState.segment<3>(3) = R * relativeState.segment<3>(3); + relativeState.segment<3>(6) = R * relativeState.segment<3>(6); + // TODO(areske): use quaternionDistance() for orientation error? + /** + * observation + */ + vector_t observation(36); + observation << generalizedTime, relativeState; + return observation; +} + +std::pair LeggedRobotMpcnetDefinition::getActionTransformation(scalar_t t, const vector_t& x, + const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + const matrix3_t R = getRotationMatrixFromZyxEulerAngles(x.segment<3>(9)); + matrix_t actionTransformationMatrix = matrix_t::Identity(24, 24); + actionTransformationMatrix.block<3, 3>(0, 0) = R; + actionTransformationMatrix.block<3, 3>(3, 3) = R; + actionTransformationMatrix.block<3, 3>(6, 6) = R; + actionTransformationMatrix.block<3, 3>(9, 9) = R; + // TODO(areske): check why less robust with weight compensating bias? + // const auto contactFlags = modeNumber2StanceLeg(modeSchedule.modeAtTime(t)); + // const vector_t actionTransformationVector = weightCompensatingInput(centroidalModelInfo_, contactFlags); + return {actionTransformationMatrix, vector_t::Zero(24)}; +} + +bool LeggedRobotMpcnetDefinition::isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + const vector_t deviation = x - defaultState_; + if (std::abs(deviation[8]) > allowedHeightDeviation_) { + std::cerr << "[LeggedRobotMpcnetDefinition::isValid] height diverged: " << x[8] << "\n"; + return false; + } else if (std::abs(deviation[10]) > allowedPitchDeviation_) { + std::cerr << "[LeggedRobotMpcnetDefinition::isValid] pitch diverged: " << x[10] << "\n"; + return false; + } else if (std::abs(deviation[11]) > allowedRollDeviation_) { + std::cerr << "[LeggedRobotMpcnetDefinition::isValid] roll diverged: " << x[11] << "\n"; + return false; + } else { + return true; + } +} + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp new file mode 100644 index 000000000..9788951b5 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetDummyNode.cpp @@ -0,0 +1,164 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" + +using namespace ocs2; +using namespace legged_robot; + +int main(int argc, char** argv) { + const std::string robotName = "legged_robot"; + + // initialize ros node + ros::init(argc, argv, robotName + "_mpcnet_dummy"); + ros::NodeHandle nodeHandle; + // Get node parameters + bool useRaisim; + std::string taskFile, urdfFile, referenceFile, raisimFile, resourcePath, policyFile; + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/urdfFile", urdfFile); + nodeHandle.getParam("/referenceFile", referenceFile); + nodeHandle.getParam("/raisimFile", raisimFile); + nodeHandle.getParam("/resourcePath", resourcePath); + nodeHandle.getParam("/policyFile", policyFile); + nodeHandle.getParam("/useRaisim", useRaisim); + + // legged robot interface + LeggedRobotInterface leggedRobotInterface(taskFile, urdfFile, referenceFile); + + // gait receiver + auto gaitReceiverPtr = + std::make_shared(nodeHandle, leggedRobotInterface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + + // ROS reference manager + auto rosReferenceManagerPtr = std::make_shared(robotName, leggedRobotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // policy (MPC-Net controller) + auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); + auto mpcnetDefinitionPtr = std::make_shared(leggedRobotInterface); + auto mpcnetControllerPtr = + std::make_unique(mpcnetDefinitionPtr, rosReferenceManagerPtr, onnxEnvironmentPtr); + mpcnetControllerPtr->loadPolicyModel(policyFile); + + // rollout + std::unique_ptr rolloutPtr; + raisim::HeightMap* terrainPtr = nullptr; + std::unique_ptr heightmapPub; + std::unique_ptr conversions; + if (useRaisim) { + conversions.reset(new LeggedRobotRaisimConversions(leggedRobotInterface.getPinocchioInterface(), + leggedRobotInterface.getCentroidalModelInfo(), + leggedRobotInterface.getInitialState())); + RaisimRolloutSettings raisimRolloutSettings(raisimFile, "rollout", true); + conversions->loadSettings(raisimFile, "rollout", true); + rolloutPtr.reset(new RaisimRollout( + urdfFile, resourcePath, + [&](const vector_t& state, const vector_t& input) { return conversions->stateToRaisimGenCoordGenVel(state, input); }, + [&](const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { return conversions->raisimGenCoordGenVelToState(q, dq); }, + [&](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return conversions->inputToRaisimGeneralizedForce(time, input, state, q, dq); + }, + nullptr, raisimRolloutSettings, + [&](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return conversions->inputToRaisimPdTargets(time, input, state, q, dq); + })); + // terrain + if (raisimRolloutSettings.generateTerrain_) { + raisim::TerrainProperties terrainProperties; + terrainProperties.zScale = raisimRolloutSettings.terrainRoughness_; + terrainProperties.seed = raisimRolloutSettings.terrainSeed_; + terrainPtr = static_cast(rolloutPtr.get())->generateTerrain(terrainProperties); + conversions->setTerrain(*terrainPtr); + heightmapPub.reset(new ocs2::RaisimHeightmapRosConverter()); + heightmapPub->publishGridmap(*terrainPtr, "odom"); + } + } else { + rolloutPtr.reset(leggedRobotInterface.getRollout().clone()); + } + + // observer + auto mpcnetDummyObserverRosPtr = std::make_shared(nodeHandle, robotName); + + // visualization + CentroidalModelPinocchioMapping pinocchioMapping(leggedRobotInterface.getCentroidalModelInfo()); + PinocchioEndEffectorKinematics endEffectorKinematics(leggedRobotInterface.getPinocchioInterface(), pinocchioMapping, + leggedRobotInterface.modelSettings().contactNames3DoF); + std::shared_ptr leggedRobotVisualizerPtr; + if (useRaisim) { + leggedRobotVisualizerPtr.reset(new LeggedRobotRaisimVisualizer( + leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + static_cast(leggedRobotVisualizerPtr.get())->updateTerrain(); + } else { + leggedRobotVisualizerPtr.reset(new LeggedRobotVisualizer( + leggedRobotInterface.getPinocchioInterface(), leggedRobotInterface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + } + + // MPC-Net dummy loop ROS + const scalar_t controlFrequency = leggedRobotInterface.mpcSettings().mrtDesiredFrequency_; + const scalar_t rosFrequency = leggedRobotInterface.mpcSettings().mpcDesiredFrequency_; + ocs2::mpcnet::MpcnetDummyLoopRos mpcnetDummyLoopRos(controlFrequency, rosFrequency, std::move(mpcnetControllerPtr), std::move(rolloutPtr), + rosReferenceManagerPtr); + mpcnetDummyLoopRos.addObserver(mpcnetDummyObserverRosPtr); + mpcnetDummyLoopRos.addObserver(leggedRobotVisualizerPtr); + mpcnetDummyLoopRos.addSynchronizedModule(gaitReceiverPtr); + + // initial system observation + SystemObservation systemObservation; + systemObservation.mode = ModeNumber::STANCE; + systemObservation.time = 0.0; + systemObservation.state = leggedRobotInterface.getInitialState(); + systemObservation.input = vector_t::Zero(leggedRobotInterface.getCentroidalModelInfo().inputDim); + + // initial target trajectories + const TargetTrajectories targetTrajectories({systemObservation.time}, {systemObservation.state}, {systemObservation.input}); + + // run MPC-Net dummy loop ROS + mpcnetDummyLoopRos.run(systemObservation, targetTrajectories); + + // successful exit + return 0; +} diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp new file mode 100644 index 000000000..533141a45 --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetInterface.cpp @@ -0,0 +1,143 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" + +#include + +#include +#include +#include +#include +#include +#include + +#include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetDefinition.h" + +namespace ocs2 { +namespace legged_robot { + +LeggedRobotMpcnetInterface::LeggedRobotMpcnetInterface(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, bool raisim) { + // create ONNX environment + auto onnxEnvironmentPtr = ocs2::mpcnet::createOnnxEnvironment(); + // paths to files + const std::string taskFile = ros::package::getPath("ocs2_legged_robot") + "/config/mpc/task.info"; + const std::string urdfFile = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/urdf/anymal.urdf"; + const std::string referenceFile = ros::package::getPath("ocs2_legged_robot") + "/config/command/reference.info"; + const std::string raisimFile = ros::package::getPath("ocs2_legged_robot_raisim") + "/config/raisim.info"; + const std::string resourcePath = ros::package::getPath("ocs2_robotic_assets") + "/resources/anymal_c/meshes"; + // set up MPC-Net rollout manager for data generation and policy evaluation + std::vector> mpcPtrs; + std::vector> mpcnetPtrs; + std::vector> rolloutPtrs; + std::vector> mpcnetDefinitionPtrs; + std::vector> referenceManagerPtrs; + mpcPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + mpcnetPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + rolloutPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + mpcnetDefinitionPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + referenceManagerPtrs.reserve(nDataGenerationThreads + nPolicyEvaluationThreads); + for (int i = 0; i < (nDataGenerationThreads + nPolicyEvaluationThreads); i++) { + leggedRobotInterfacePtrs_.push_back(std::make_unique(taskFile, urdfFile, referenceFile)); + auto mpcnetDefinitionPtr = std::make_shared(*leggedRobotInterfacePtrs_[i]); + mpcPtrs.push_back(getMpc(*leggedRobotInterfacePtrs_[i])); + mpcnetPtrs.push_back(std::unique_ptr(new ocs2::mpcnet::MpcnetOnnxController( + mpcnetDefinitionPtr, leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr(), onnxEnvironmentPtr))); + if (raisim) { + RaisimRolloutSettings raisimRolloutSettings(raisimFile, "rollout"); + raisimRolloutSettings.portNumber_ += i; + leggedRobotRaisimConversionsPtrs_.push_back(std::make_unique( + leggedRobotInterfacePtrs_[i]->getPinocchioInterface(), leggedRobotInterfacePtrs_[i]->getCentroidalModelInfo(), + leggedRobotInterfacePtrs_[i]->getInitialState())); + leggedRobotRaisimConversionsPtrs_[i]->loadSettings(raisimFile, "rollout", true); + rolloutPtrs.push_back(std::make_unique( + urdfFile, resourcePath, + [&, i](const vector_t& state, const vector_t& input) { + return leggedRobotRaisimConversionsPtrs_[i]->stateToRaisimGenCoordGenVel(state, input); + }, + [&, i](const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return leggedRobotRaisimConversionsPtrs_[i]->raisimGenCoordGenVelToState(q, dq); + }, + [&, i](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimGeneralizedForce(time, input, state, q, dq); + }, + nullptr, raisimRolloutSettings, + [&, i](double time, const vector_t& input, const vector_t& state, const Eigen::VectorXd& q, const Eigen::VectorXd& dq) { + return leggedRobotRaisimConversionsPtrs_[i]->inputToRaisimPdTargets(time, input, state, q, dq); + })); + if (raisimRolloutSettings.generateTerrain_) { + raisim::TerrainProperties terrainProperties; + terrainProperties.zScale = raisimRolloutSettings.terrainRoughness_; + terrainProperties.seed = raisimRolloutSettings.terrainSeed_ + i; + auto terrainPtr = static_cast(rolloutPtrs[i].get())->generateTerrain(terrainProperties); + leggedRobotRaisimConversionsPtrs_[i]->setTerrain(*terrainPtr); + } + } else { + rolloutPtrs.push_back(std::unique_ptr(leggedRobotInterfacePtrs_[i]->getRollout().clone())); + } + mpcnetDefinitionPtrs.push_back(mpcnetDefinitionPtr); + referenceManagerPtrs.push_back(leggedRobotInterfacePtrs_[i]->getReferenceManagerPtr()); + } + mpcnetRolloutManagerPtr_.reset(new ocs2::mpcnet::MpcnetRolloutManager(nDataGenerationThreads, nPolicyEvaluationThreads, + std::move(mpcPtrs), std::move(mpcnetPtrs), std::move(rolloutPtrs), + mpcnetDefinitionPtrs, referenceManagerPtrs)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::unique_ptr LeggedRobotMpcnetInterface::getMpc(LeggedRobotInterface& leggedRobotInterface) { + // ensure MPC and DDP settings are as needed for MPC-Net + const auto mpcSettings = [&]() { + auto settings = leggedRobotInterface.mpcSettings(); + settings.debugPrint_ = false; + settings.coldStart_ = false; + return settings; + }(); + const auto ddpSettings = [&]() { + auto settings = leggedRobotInterface.ddpSettings(); + settings.algorithm_ = ocs2::ddp::Algorithm::SLQ; + settings.nThreads_ = 1; + settings.displayInfo_ = false; + settings.displayShortSummary_ = false; + settings.checkNumericalStability_ = false; + settings.debugPrintRollout_ = false; + settings.useFeedbackPolicy_ = true; + return settings; + }(); + // create one MPC instance + auto mpcPtr = + std::make_unique(mpcSettings, ddpSettings, leggedRobotInterface.getRollout(), + leggedRobotInterface.getOptimalControlProblem(), leggedRobotInterface.getInitializer()); + mpcPtr->getSolverPtr()->setReferenceManager(leggedRobotInterface.getReferenceManagerPtr()); + return mpcPtr; +} + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp new file mode 100644 index 000000000..f170bde7a --- /dev/null +++ b/ocs2_mpcnet/ocs2_legged_robot_mpcnet/src/LeggedRobotMpcnetPybindings.cpp @@ -0,0 +1,34 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include "ocs2_legged_robot_mpcnet/LeggedRobotMpcnetInterface.h" + +CREATE_ROBOT_MPCNET_PYTHON_BINDINGS(ocs2::legged_robot::LeggedRobotMpcnetInterface, LeggedRobotMpcnetPybindings) diff --git a/ocs2_mpcnet/ocs2_mpcnet/CMakeLists.txt b/ocs2_mpcnet/ocs2_mpcnet/CMakeLists.txt new file mode 100644 index 000000000..a9dba76cb --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_mpcnet) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/ocs2_mpcnet/ocs2_mpcnet/package.xml b/ocs2_mpcnet/ocs2_mpcnet/package.xml new file mode 100644 index 000000000..7eec28861 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet/package.xml @@ -0,0 +1,24 @@ + + + ocs2_mpcnet + 0.0.0 + The ocs2_mpcnet metapackage + + Alexander Reske + + Farbod Farshidian + Alexander Reske + + BSD-3 + + catkin + + ocs2_mpcnet_core + ocs2_ballbot_mpcnet + ocs2_legged_robot_mpcnet + + + + + + diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt b/ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt new file mode 100644 index 000000000..7b91fd000 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_mpcnet_core) + +set(CATKIN_PACKAGE_DEPENDENCIES + pybind11_catkin + ocs2_mpc + ocs2_python_interface + ocs2_ros_interfaces +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(onnxruntime REQUIRED) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################### +## catkin specific configuration ## +################################### + +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS + onnxruntime +) + +########### +## Build ## +########### + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +# main library +add_library(${PROJECT_NAME} + src/control/MpcnetBehavioralController.cpp + src/control/MpcnetOnnxController.cpp + src/dummy/MpcnetDummyLoopRos.cpp + src/dummy/MpcnetDummyObserverRos.cpp + src/rollout/MpcnetDataGeneration.cpp + src/rollout/MpcnetPolicyEvaluation.cpp + src/rollout/MpcnetRolloutBase.cpp + src/rollout/MpcnetRolloutManager.cpp + src/MpcnetInterfaceBase.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + onnxruntime +) + +# python bindings +pybind11_add_module(MpcnetPybindings SHARED + src/MpcnetPybindings.cpp +) +add_dependencies(MpcnetPybindings + ${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(MpcnetPybindings PRIVATE + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +set_target_properties(MpcnetPybindings + PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +catkin_python_setup() + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target ocs2_mpcnet_core") + add_clang_tooling( + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR +) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(TARGETS MpcnetPybindings + ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +############# +## Testing ## +############# + +# TODO(areske) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h new file mode 100644 index 000000000..de322bcc2 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetDefinitionBase.h @@ -0,0 +1,100 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +namespace ocs2 { +namespace mpcnet { + +/** + * Base class for MPC-Net definitions. + */ +class MpcnetDefinitionBase { + public: + /** + * Default constructor. + */ + MpcnetDefinitionBase() = default; + + /** + * Default destructor. + */ + virtual ~MpcnetDefinitionBase() = default; + + /** + * Deleted copy constructor. + */ + MpcnetDefinitionBase(const MpcnetDefinitionBase&) = delete; + + /** + * Deleted copy assignment. + */ + MpcnetDefinitionBase& operator=(const MpcnetDefinitionBase&) = delete; + + /** + * Get the observation. + * @note The observation o is the input to the policy. + * @param[in] t : Absolute time. + * @param[in] x : Robot state. + * @param[in] modeSchedule : Mode schedule. + * @param[in] targetTrajectories : Target trajectories. + * @return The observation. + */ + virtual vector_t getObservation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) = 0; + + /** + * Get the action transformation. + * @note Used for computing the control input u = A * a + b from the action a predicted by the policy. + * @param[in] t : Absolute time. + * @param[in] x : Robot state. + * @param[in] modeSchedule : Mode schedule. + * @param[in] targetTrajectories : Target trajectories. + * @return The action transformation pair {A, b}. + */ + virtual std::pair getActionTransformation(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) = 0; + + /** + * Check if the tuple (t, x, modeSchedule, targetTrajectories) is valid. + * @note E.g., check if the state diverged or if tracking is poor. + * @param[in] t : Absolute time. + * @param[in] x : Robot state. + * @param[in] modeSchedule : Mode schedule. + * @param[in] targetTrajectories : Target trajectories. + * @return True if valid. + */ + virtual bool isValid(scalar_t t, const vector_t& x, const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) = 0; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetInterfaceBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetInterfaceBase.h new file mode 100644 index 000000000..ec4b72d00 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetInterfaceBase.h @@ -0,0 +1,91 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include "ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * Base class for all MPC-Net interfaces between C++ and Python. + */ +class MpcnetInterfaceBase { + public: + /** + * Default destructor. + */ + virtual ~MpcnetInterfaceBase() = default; + + /** + * @see MpcnetRolloutManager::startDataGeneration() + */ + void startDataGeneration(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, + const matrix_t& samplingCovariance, const std::vector& initialObservations, + const std::vector& modeSchedules, const std::vector& targetTrajectories); + + /** + * @see MpcnetRolloutManager::isDataGenerationDone() + */ + bool isDataGenerationDone(); + + /** + * @see MpcnetRolloutManager::getGeneratedData() + */ + data_array_t getGeneratedData(); + + /** + * @see MpcnetRolloutManager::startPolicyEvaluation() + */ + void startPolicyEvaluation(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, + const std::vector& initialObservations, const std::vector& modeSchedules, + const std::vector& targetTrajectories); + + /** + * @see MpcnetRolloutManager::isPolicyEvaluationDone() + */ + bool isPolicyEvaluationDone(); + + /** + * @see MpcnetRolloutManager::getComputedMetrics() + */ + metrics_array_t getComputedMetrics(); + + protected: + /** + * Default constructor. + */ + MpcnetInterfaceBase() = default; + + std::unique_ptr mpcnetRolloutManagerPtr_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h new file mode 100644 index 000000000..632fa4717 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/MpcnetPybindMacros.h @@ -0,0 +1,133 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include + +#include "ocs2_mpcnet_core/rollout/MpcnetData.h" +#include "ocs2_mpcnet_core/rollout/MpcnetMetrics.h" + +using namespace pybind11::literals; + +/** + * Convenience macro to bind general MPC-Net functionalities and other classes with all required vectors. + */ +#define CREATE_MPCNET_PYTHON_BINDINGS(LIB_NAME) \ + /* make vector types opaque so they are not converted to python lists */ \ + PYBIND11_MAKE_OPAQUE(ocs2::size_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::scalar_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::vector_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::matrix_array_t) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(std::vector) \ + PYBIND11_MAKE_OPAQUE(ocs2::mpcnet::data_array_t) \ + PYBIND11_MAKE_OPAQUE(ocs2::mpcnet::metrics_array_t) \ + /* create a python module */ \ + PYBIND11_MODULE(LIB_NAME, m) { \ + /* bind vector types so they can be used natively in python */ \ + VECTOR_TYPE_BINDING(ocs2::size_array_t, "size_array") \ + VECTOR_TYPE_BINDING(ocs2::scalar_array_t, "scalar_array") \ + VECTOR_TYPE_BINDING(ocs2::vector_array_t, "vector_array") \ + VECTOR_TYPE_BINDING(ocs2::matrix_array_t, "matrix_array") \ + VECTOR_TYPE_BINDING(std::vector, "SystemObservationArray") \ + VECTOR_TYPE_BINDING(std::vector, "ModeScheduleArray") \ + VECTOR_TYPE_BINDING(std::vector, "TargetTrajectoriesArray") \ + VECTOR_TYPE_BINDING(ocs2::mpcnet::data_array_t, "DataArray") \ + VECTOR_TYPE_BINDING(ocs2::mpcnet::metrics_array_t, "MetricsArray") \ + /* bind approximation classes */ \ + pybind11::class_(m, "ScalarFunctionQuadraticApproximation") \ + .def_readwrite("f", &ocs2::ScalarFunctionQuadraticApproximation::f) \ + .def_readwrite("dfdx", &ocs2::ScalarFunctionQuadraticApproximation::dfdx) \ + .def_readwrite("dfdu", &ocs2::ScalarFunctionQuadraticApproximation::dfdu) \ + .def_readwrite("dfdxx", &ocs2::ScalarFunctionQuadraticApproximation::dfdxx) \ + .def_readwrite("dfdux", &ocs2::ScalarFunctionQuadraticApproximation::dfdux) \ + .def_readwrite("dfduu", &ocs2::ScalarFunctionQuadraticApproximation::dfduu); \ + /* bind system observation struct */ \ + pybind11::class_(m, "SystemObservation") \ + .def(pybind11::init<>()) \ + .def_readwrite("mode", &ocs2::SystemObservation::mode) \ + .def_readwrite("time", &ocs2::SystemObservation::time) \ + .def_readwrite("state", &ocs2::SystemObservation::state) \ + .def_readwrite("input", &ocs2::SystemObservation::input); \ + /* bind mode schedule struct */ \ + pybind11::class_(m, "ModeSchedule") \ + .def(pybind11::init()) \ + .def_readwrite("eventTimes", &ocs2::ModeSchedule::eventTimes) \ + .def_readwrite("modeSequence", &ocs2::ModeSchedule::modeSequence); \ + /* bind target trajectories class */ \ + pybind11::class_(m, "TargetTrajectories") \ + .def(pybind11::init()) \ + .def_readwrite("timeTrajectory", &ocs2::TargetTrajectories::timeTrajectory) \ + .def_readwrite("stateTrajectory", &ocs2::TargetTrajectories::stateTrajectory) \ + .def_readwrite("inputTrajectory", &ocs2::TargetTrajectories::inputTrajectory); \ + /* bind data point struct */ \ + pybind11::class_(m, "DataPoint") \ + .def(pybind11::init<>()) \ + .def_readwrite("mode", &ocs2::mpcnet::data_point_t::mode) \ + .def_readwrite("t", &ocs2::mpcnet::data_point_t::t) \ + .def_readwrite("x", &ocs2::mpcnet::data_point_t::x) \ + .def_readwrite("u", &ocs2::mpcnet::data_point_t::u) \ + .def_readwrite("observation", &ocs2::mpcnet::data_point_t::observation) \ + .def_readwrite("actionTransformation", &ocs2::mpcnet::data_point_t::actionTransformation) \ + .def_readwrite("hamiltonian", &ocs2::mpcnet::data_point_t::hamiltonian); \ + /* bind metrics struct */ \ + pybind11::class_(m, "Metrics") \ + .def(pybind11::init<>()) \ + .def_readwrite("survivalTime", &ocs2::mpcnet::metrics_t::survivalTime) \ + .def_readwrite("incurredHamiltonian", &ocs2::mpcnet::metrics_t::incurredHamiltonian); \ + } + +/** + * Convenience macro to bind robot MPC-Net interface. + */ +#define CREATE_ROBOT_MPCNET_PYTHON_BINDINGS(MPCNET_INTERFACE, LIB_NAME) \ + /* create a python module */ \ + PYBIND11_MODULE(LIB_NAME, m) { \ + /* import the general MPC-Net module */ \ + pybind11::module::import("ocs2_mpcnet_core.MpcnetPybindings"); \ + /* bind actual MPC-Net interface for specific robot */ \ + pybind11::class_(m, "MpcnetInterface") \ + .def(pybind11::init()) \ + .def("startDataGeneration", &MPCNET_INTERFACE::startDataGeneration, "alpha"_a, "policyFilePath"_a, "timeStep"_a, \ + "dataDecimation"_a, "nSamples"_a, "samplingCovariance"_a.noconvert(), "initialObservations"_a, "modeSchedules"_a, \ + "targetTrajectories"_a) \ + .def("isDataGenerationDone", &MPCNET_INTERFACE::isDataGenerationDone) \ + .def("getGeneratedData", &MPCNET_INTERFACE::getGeneratedData) \ + .def("startPolicyEvaluation", &MPCNET_INTERFACE::startPolicyEvaluation, "alpha"_a, "policyFilePath"_a, "timeStep"_a, \ + "initialObservations"_a, "modeSchedules"_a, "targetTrajectories"_a) \ + .def("isPolicyEvaluationDone", &MPCNET_INTERFACE::isPolicyEvaluationDone) \ + .def("getComputedMetrics", &MPCNET_INTERFACE::getComputedMetrics); \ + } diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetBehavioralController.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetBehavioralController.h new file mode 100644 index 000000000..60ec8a6c6 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetBehavioralController.h @@ -0,0 +1,97 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include + +#include "ocs2_mpcnet_core/control/MpcnetControllerBase.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * A behavioral controller that computes the input based on a mixture of an optimal policy (e.g. implicitly found via MPC) + * and a learned policy (e.g. explicitly represented by a neural network). + * The behavioral policy is pi_behavioral = alpha * pi_optimal + (1 - alpha) * pi_learned with alpha in [0, 1]. + */ +class MpcnetBehavioralController final : public ControllerBase { + public: + /** + * Constructor. + * @param [in] alpha : The mixture parameter. + * @param [in] optimalController : The optimal controller (this class takes ownership of a clone). + * @param [in] learnedController : The learned controller (this class takes ownership of a clone). + */ + MpcnetBehavioralController(scalar_t alpha, const ControllerBase& optimalController, const MpcnetControllerBase& learnedController) + : alpha_(alpha), optimalControllerPtr_(optimalController.clone()), learnedControllerPtr_(learnedController.clone()) {} + + MpcnetBehavioralController() = default; + ~MpcnetBehavioralController() override = default; + MpcnetBehavioralController* clone() const override { return new MpcnetBehavioralController(*this); } + + /** + * Set the mixture parameter. + * @param [in] alpha : The mixture parameter. + */ + void setAlpha(scalar_t alpha) { alpha_ = alpha; } + + /** + * Set the optimal controller. + * @param [in] optimalController : The optimal controller (this class takes ownership of a clone). + */ + void setOptimalController(const ControllerBase& optimalController) { optimalControllerPtr_.reset(optimalController.clone()); } + + /** + * Set the learned controller. + * @param [in] learnedController : The learned controller (this class takes ownership of a clone). + */ + void setLearnedController(const MpcnetControllerBase& learnedController) { learnedControllerPtr_.reset(learnedController.clone()); } + + vector_t computeInput(scalar_t t, const vector_t& x) override; + ControllerType getType() const override { return ControllerType::BEHAVIORAL; } + + int size() const override; + void clear() override; + bool empty() const override; + void concatenate(const ControllerBase* otherController, int index, int length) override; + + private: + MpcnetBehavioralController(const MpcnetBehavioralController& other) + : MpcnetBehavioralController(other.alpha_, *other.optimalControllerPtr_, *other.learnedControllerPtr_) {} + + scalar_t alpha_; + std::unique_ptr optimalControllerPtr_; + std::unique_ptr learnedControllerPtr_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetControllerBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetControllerBase.h new file mode 100644 index 000000000..8a669cf79 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetControllerBase.h @@ -0,0 +1,57 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +namespace ocs2 { +namespace mpcnet { + +/** + * The base class for all controllers that use a MPC-Net policy. + */ +class MpcnetControllerBase : public ControllerBase { + public: + MpcnetControllerBase() = default; + ~MpcnetControllerBase() override = default; + MpcnetControllerBase* clone() const override = 0; + + /** + * Load the model of the policy. + * @param [in] policyFilePath : Path to the file with the model of the policy. + */ + virtual void loadPolicyModel(const std::string& policyFilePath) = 0; + + protected: + MpcnetControllerBase(const MpcnetControllerBase& other) = default; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetOnnxController.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetOnnxController.h new file mode 100644 index 000000000..89c102eb3 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/control/MpcnetOnnxController.h @@ -0,0 +1,111 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include + +#include "ocs2_mpcnet_core/MpcnetDefinitionBase.h" +#include "ocs2_mpcnet_core/control/MpcnetControllerBase.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * Convenience function for creating an environment for ONNX Runtime and getting a corresponding shared pointer. + * @note Only one environment per process can be created. The environment offers some threading and logging options. + * @return Pointer to the environment for ONNX Runtime. + */ +inline std::shared_ptr createOnnxEnvironment() { + return std::make_shared(ORT_LOGGING_LEVEL_WARNING, "MpcnetOnnxController"); +} + +/** + * A neural network controller using ONNX Runtime based on the Open Neural Network Exchange (ONNX) format. + * The model of the policy computes u = model(t, x) with + * t: generalized time (1 x dimensionOfTime), + * x: relative state (1 x dimensionOfState), + * u: predicted input (1 x dimensionOfInput), + * @note The additional first dimension with size 1 for the variables of the model comes from batch processing during training. + */ +class MpcnetOnnxController final : public MpcnetControllerBase { + public: + /** + * Constructor. + * @note The class is not fully instantiated until calling loadPolicyModel(). + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions. + * @param [in] referenceManagerPtr : Pointer to the reference manager. + * @param [in] onnxEnvironmentPtr : Pointer to the environment for ONNX Runtime. + */ + MpcnetOnnxController(std::shared_ptr mpcnetDefinitionPtr, + std::shared_ptr referenceManagerPtr, std::shared_ptr onnxEnvironmentPtr) + : mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), + referenceManagerPtr_(std::move(referenceManagerPtr)), + onnxEnvironmentPtr_(std::move(onnxEnvironmentPtr)) {} + + ~MpcnetOnnxController() override = default; + MpcnetOnnxController* clone() const override { return new MpcnetOnnxController(*this); } + + void loadPolicyModel(const std::string& policyFilePath) override; + + vector_t computeInput(const scalar_t t, const vector_t& x) override; + ControllerType getType() const override { return ControllerType::ONNX; } + + int size() const override { throw std::runtime_error("[MpcnetOnnxController::size] not implemented."); } + void clear() override { throw std::runtime_error("[MpcnetOnnxController::clear] not implemented."); } + bool empty() const override { throw std::runtime_error("[MpcnetOnnxController::empty] not implemented."); } + void concatenate(const ControllerBase* otherController, int index, int length) override { + throw std::runtime_error("[MpcnetOnnxController::concatenate] not implemented."); + } + + private: + using tensor_element_t = float; + + MpcnetOnnxController(const MpcnetOnnxController& other) + : MpcnetOnnxController(other.mpcnetDefinitionPtr_, other.referenceManagerPtr_, other.onnxEnvironmentPtr_) { + if (!other.policyFilePath_.empty()) { + loadPolicyModel(other.policyFilePath_); + } + } + + std::shared_ptr mpcnetDefinitionPtr_; + std::shared_ptr referenceManagerPtr_; + std::shared_ptr onnxEnvironmentPtr_; + std::string policyFilePath_; + std::unique_ptr sessionPtr_; + std::vector inputNames_; + std::vector outputNames_; + std::vector> inputShapes_; + std::vector> outputShapes_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h new file mode 100644 index 000000000..b1e954e28 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h @@ -0,0 +1,112 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include + +#include "ocs2_mpcnet_core/control/MpcnetControllerBase.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * Dummy loop to test a robot controlled by an MPC-Net policy. + */ +class MpcnetDummyLoopRos { + public: + /** + * Constructor. + * @param [in] controlFrequency : Minimum frequency at which the MPC-Net policy should be called. + * @param [in] rosFrequency : Frequency at which the ROS observers are updated. + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] rosReferenceManagerPtr : Pointer to the reference manager to be used (shared ownership). + */ + MpcnetDummyLoopRos(scalar_t controlFrequency, scalar_t rosFrequency, std::unique_ptr mpcnetPtr, + std::unique_ptr rolloutPtr, std::shared_ptr rosReferenceManagerPtr); + + /** + * Default destructor. + */ + virtual ~MpcnetDummyLoopRos() = default; + + /** + * Runs the dummy loop. + * @param [in] systemObservation: The initial system observation. + * @param [in] targetTrajectories: The initial target trajectories. + */ + void run(const SystemObservation& systemObservation, const TargetTrajectories& targetTrajectories); + + /** + * Adds one observer to the vector of observers that need to be informed about the system observation, primal solution and command data. + * Each observer is updated once after running a rollout. + * @param [in] observer : The observer to add. + */ + void addObserver(std::shared_ptr observer); + + /** + * Adds one module to the vector of modules that need to be synchronized with the policy. + * Each module is updated once before calling the policy. + * @param [in] synchronizedModule : The module to add. + */ + void addSynchronizedModule(std::shared_ptr synchronizedModule); + + protected: + /** + * Runs a rollout. + * @param [in] duration : The duration of the run. + * @param [in] initialSystemObservation : The initial system observation. + * @param [out] finalSystemObservation : The final system observation. + */ + void rollout(scalar_t duration, const SystemObservation& initialSystemObservation, SystemObservation& finalSystemObservation); + + /** + * Update the reference manager and the synchronized modules. + * @param [in] time : The current time. + * @param [in] state : The cuurent state. + */ + void preSolverRun(scalar_t time, const vector_t& state); + + private: + scalar_t controlFrequency_; + scalar_t rosFrequency_; + std::unique_ptr mpcnetPtr_; + std::unique_ptr rolloutPtr_; + std::shared_ptr rosReferenceManagerPtr_; + std::vector> observerPtrs_; + std::vector> synchronizedModulePtrs_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h new file mode 100644 index 000000000..a03661bb4 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h @@ -0,0 +1,69 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include + +namespace ocs2 { +namespace mpcnet { + +/** + * Dummy observer that publishes the current system observation that is required for some target command nodes. + */ +class MpcnetDummyObserverRos : public DummyObserver { + public: + /** + * Constructor. + * @param [in] nodeHandle : The ROS node handle. + * @param [in] topicPrefix : The prefix defines the names for the observation's publishing topic "topicPrefix_mpc_observation". + */ + explicit MpcnetDummyObserverRos(ros::NodeHandle& nodeHandle, std::string topicPrefix = "anonymousRobot"); + + /** + * Default destructor. + */ + ~MpcnetDummyObserverRos() override = default; + + /** + * Update and publish. + * @param [in] observation: The current system observation. + * @param [in] primalSolution: The current primal solution. + * @param [in] command: The given command data. + */ + void update(const SystemObservation& observation, const PrimalSolution& primalSolution, const CommandData& command) override; + + private: + ros::Publisher observationPublisher_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h new file mode 100644 index 000000000..7fd63215e --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetData.h @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +#include "ocs2_mpcnet_core/MpcnetDefinitionBase.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * Data point collected during the data generation rollout. + */ +struct DataPoint { + /** Mode of the system. */ + size_t mode; + /** Absolute time. */ + scalar_t t; + /** Observed state. */ + vector_t x; + /** Optimal control input. */ + vector_t u; + /** Observation given as input to the policy. */ + vector_t observation; + /** Action transformation applied to the output of the policy. */ + std::pair actionTransformation; + /** Linear-quadratic approximation of the Hamiltonian, using x and u as development/expansion points. */ + ScalarFunctionQuadraticApproximation hamiltonian; +}; +using data_point_t = DataPoint; +using data_array_t = std::vector; + +/** + * Get a data point. + * @param [in] mpc : The MPC with a pointer to the underlying solver. + * @param [in] mpcnetDefinition : The MPC-Net definitions. + * @param [in] deviation : The state deviation from the nominal state where to get the data point from. + * @return A data point. + */ +inline data_point_t getDataPoint(MPC_BASE& mpc, MpcnetDefinitionBase& mpcnetDefinition, const vector_t& deviation) { + data_point_t dataPoint; + const auto& referenceManager = mpc.getSolverPtr()->getReferenceManager(); + const auto primalSolution = mpc.getSolverPtr()->primalSolution(mpc.getSolverPtr()->getFinalTime()); + dataPoint.t = primalSolution.timeTrajectory_.front(); + dataPoint.x = primalSolution.stateTrajectory_.front() + deviation; + dataPoint.u = primalSolution.controllerPtr_->computeInput(dataPoint.t, dataPoint.x); + dataPoint.mode = primalSolution.modeSchedule_.modeAtTime(dataPoint.t); + dataPoint.observation = mpcnetDefinition.getObservation(dataPoint.t, dataPoint.x, referenceManager.getModeSchedule(), + referenceManager.getTargetTrajectories()); + dataPoint.actionTransformation = mpcnetDefinition.getActionTransformation(dataPoint.t, dataPoint.x, referenceManager.getModeSchedule(), + referenceManager.getTargetTrajectories()); + dataPoint.hamiltonian = mpc.getSolverPtr()->getHamiltonian(dataPoint.t, dataPoint.x, dataPoint.u); + return dataPoint; +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h new file mode 100644 index 000000000..6b0ee2a0e --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h @@ -0,0 +1,95 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include "ocs2_mpcnet_core/rollout/MpcnetData.h" +#include "ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * A class for generating data from a system that is forward simulated with a behavioral controller. + * @note Usually the behavioral controller moves from the MPC policy to the MPC-Net policy throughout the training process. + */ +class MpcnetDataGeneration final : public MpcnetRolloutBase { + public: + /** + * Constructor. + * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). + * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). + */ + MpcnetDataGeneration(std::unique_ptr mpcPtr, std::unique_ptr mpcnetPtr, + std::unique_ptr rolloutPtr, std::shared_ptr mpcnetDefinitionPtr, + std::shared_ptr referenceManagerPtr) + : MpcnetRolloutBase(std::move(mpcPtr), std::move(mpcnetPtr), std::move(rolloutPtr), std::move(mpcnetDefinitionPtr), + std::move(referenceManagerPtr)) {} + + /** + * Default destructor. + */ + ~MpcnetDataGeneration() override = default; + + /** + * Deleted copy constructor. + */ + MpcnetDataGeneration(const MpcnetDataGeneration&) = delete; + + /** + * Deleted copy assignment. + */ + MpcnetDataGeneration& operator=(const MpcnetDataGeneration&) = delete; + + /** + * Run the data generation. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the behavioral controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. + * @param [in] dataDecimation : The integer factor used for downsampling the data signal. + * @param [in] nSamples : The number of samples drawn from a multivariate normal distribution around the nominal states. + * @param [in] samplingCovariance : The covariance matrix used for sampling from a multivariate normal distribution. + * @param [in] initialObservation : The initial system observation to start from (time and state required). + * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + * @return Pointer to the data array with the generated data. + */ + const data_array_t* run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, + const matrix_t& samplingCovariance, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories); + + private: + data_array_t dataArray_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetMetrics.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetMetrics.h new file mode 100644 index 000000000..120da00c7 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetMetrics.h @@ -0,0 +1,50 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +namespace ocs2 { +namespace mpcnet { + +/** + * Metrics computed during the policy evaluation rollout. + */ +struct Metrics { + /** Survival time. */ + scalar_t survivalTime = 0.0; + /** Hamiltonian incurred over time. */ + scalar_t incurredHamiltonian = 0.0; +}; +using metrics_t = Metrics; +using metrics_array_t = std::vector; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h new file mode 100644 index 000000000..24d8ac57e --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h @@ -0,0 +1,88 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include "ocs2_mpcnet_core/rollout/MpcnetMetrics.h" +#include "ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * A class for evaluating a policy for a system that is forward simulated with a behavioral controller. + * @note Usually the behavioral controller is evaluated for the MPC-Net policy (alpha = 0). + */ +class MpcnetPolicyEvaluation final : public MpcnetRolloutBase { + public: + /** + * Constructor. + * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). + * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). + */ + MpcnetPolicyEvaluation(std::unique_ptr mpcPtr, std::unique_ptr mpcnetPtr, + std::unique_ptr rolloutPtr, std::shared_ptr mpcnetDefinitionPtr, + std::shared_ptr referenceManagerPtr) + : MpcnetRolloutBase(std::move(mpcPtr), std::move(mpcnetPtr), std::move(rolloutPtr), std::move(mpcnetDefinitionPtr), + std::move(referenceManagerPtr)) {} + + /** + * Default destructor. + */ + ~MpcnetPolicyEvaluation() override = default; + + /** + * Deleted copy constructor. + */ + MpcnetPolicyEvaluation(const MpcnetPolicyEvaluation&) = delete; + + /** + * Deleted copy assignment. + */ + MpcnetPolicyEvaluation& operator=(const MpcnetPolicyEvaluation&) = delete; + + /** + * Run the policy evaluation. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the behavioral controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. + * @param [in] initialObservation : The initial system observation to start from (time and state required). + * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + * @return The computed metrics. + */ + metrics_t run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories); +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h new file mode 100644 index 000000000..23c16f1d2 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h @@ -0,0 +1,117 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "ocs2_mpcnet_core/MpcnetDefinitionBase.h" +#include "ocs2_mpcnet_core/control/MpcnetBehavioralController.h" +#include "ocs2_mpcnet_core/control/MpcnetControllerBase.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * The base class for doing rollouts for a system that is forward simulated with a behavioral controller. + * The behavioral policy is a mixture of an MPC policy and an MPC-Net policy (e.g. a neural network). + */ +class MpcnetRolloutBase { + public: + /** + * Constructor. + * @param [in] mpcPtr : Pointer to the MPC solver to be used (this class takes ownership). + * @param [in] mpcnetPtr : Pointer to the MPC-Net policy to be used (this class takes ownership). + * @param [in] rolloutPtr : Pointer to the rollout to be used (this class takes ownership). + * @param [in] mpcnetDefinitionPtr : Pointer to the MPC-Net definitions to be used (shared ownership). + * @param [in] referenceManagerPtr : Pointer to the reference manager to be used (shared ownership). + */ + MpcnetRolloutBase(std::unique_ptr mpcPtr, std::unique_ptr mpcnetPtr, + std::unique_ptr rolloutPtr, std::shared_ptr mpcnetDefinitionPtr, + std::shared_ptr referenceManagerPtr) + : mpcPtr_(std::move(mpcPtr)), + mpcnetPtr_(std::move(mpcnetPtr)), + rolloutPtr_(std::move(rolloutPtr)), + mpcnetDefinitionPtr_(std::move(mpcnetDefinitionPtr)), + referenceManagerPtr_(std::move(referenceManagerPtr)), + behavioralControllerPtr_(new MpcnetBehavioralController()) {} + + /** + * Default destructor. + */ + virtual ~MpcnetRolloutBase() = default; + + /** + * Deleted copy constructor. + */ + MpcnetRolloutBase(const MpcnetRolloutBase&) = delete; + + /** + * Deleted copy assignment. + */ + MpcnetRolloutBase& operator=(const MpcnetRolloutBase&) = delete; + + protected: + /** + * (Re)set system components. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the controller. + * @param [in] initialObservation : The initial system observation to start from (time and state required). + * @param [in] modeSchedule : The mode schedule providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + */ + void set(scalar_t alpha, const std::string& policyFilePath, const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories); + + /** + * Simulate the system one step forward. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. + */ + void step(scalar_t timeStep); + + std::unique_ptr mpcPtr_; + std::shared_ptr mpcnetDefinitionPtr_; + std::unique_ptr behavioralControllerPtr_; + SystemObservation systemObservation_; + PrimalSolution primalSolution_; + + private: + std::unique_ptr mpcnetPtr_; + std::unique_ptr rolloutPtr_; + std::shared_ptr referenceManagerPtr_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h new file mode 100644 index 000000000..c1862e9bc --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/include/ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h @@ -0,0 +1,137 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include "ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h" +#include "ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h" + +namespace ocs2 { +namespace mpcnet { + +/** + * A class to manage the data generation and policy evaluation rollouts for MPC-Net. + */ +class MpcnetRolloutManager { + public: + /** + * Constructor. + * @note The first nDataGenerationThreads pointers will be used for the data generation and the next nPolicyEvaluationThreads pointers for + * the policy evaluation. + * @param [in] nDataGenerationThreads : Number of data generation threads. + * @param [in] nPolicyEvaluationThreads : Number of policy evaluation threads. + * @param [in] mpcPtrs : Pointers to the MPC solvers to be used. + * @param [in] mpcnetPtrs : Pointers to the MPC-Net policies to be used. + * @param [in] rolloutPtrs : Pointers to the rollouts to be used. + * @param [in] mpcnetDefinitionPtrs : Pointers to the MPC-Net definitions to be used. + * @param [in] referenceManagerPtrs : Pointers to the reference managers to be used. + */ + MpcnetRolloutManager(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, std::vector> mpcPtrs, + std::vector> mpcnetPtrs, std::vector> rolloutPtrs, + std::vector> mpcnetDefinitionPtrs, + std::vector> referenceManagerPtrs); + + /** + * Default destructor. + */ + virtual ~MpcnetRolloutManager() = default; + + /** + * Starts the data genration forward simulated by a behavioral controller. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the behavioral controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. + * @param [in] dataDecimation : The integer factor used for downsampling the data signal. + * @param [in] nSamples : The number of samples drawn from a multivariate normal distribution around the nominal states. + * @param [in] samplingCovariance : The covariance matrix used for sampling from a multivariate normal distribution. + * @param [in] initialObservations : The initial system observations to start from (time and state required). + * @param [in] modeSchedules : The mode schedules providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + */ + void startDataGeneration(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, size_t nSamples, + const matrix_t& samplingCovariance, const std::vector& initialObservations, + const std::vector& modeSchedules, const std::vector& targetTrajectories); + + /** + * Check if data generation is done. + * @return True if done. + */ + bool isDataGenerationDone(); + + /** + * Get the data generated from the data generation rollout. + * @return The generated data. + */ + const data_array_t& getGeneratedData(); + + /** + * Starts the policy evaluation forward simulated by a behavioral controller. + * @param [in] alpha : The mixture parameter for the behavioral controller. + * @param [in] policyFilePath : The path to the file with the learned policy for the behavioral controller. + * @param [in] timeStep : The time step for the forward simulation of the system with the behavioral controller. + * @param [in] initialObservations : The initial system observations to start from (time and state required). + * @param [in] modeSchedules : The mode schedules providing the event times and mode sequence. + * @param [in] targetTrajectories : The target trajectories to be tracked. + */ + void startPolicyEvaluation(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, + const std::vector& initialObservations, const std::vector& modeSchedules, + const std::vector& targetTrajectories); + + /** + * Check if policy evaluation is done. + * @return True if done. + */ + bool isPolicyEvaluationDone(); + + /** + * Get the metrics computed from the policy evaluation rollout. + * @return The computed metrics. + */ + metrics_array_t getComputedMetrics(); + + private: + // data generation variables + size_t nDataGenerationThreads_; + std::atomic_int nDataGenerationTasksDone_; + std::unique_ptr dataGenerationThreadPoolPtr_; + std::vector> dataGenerationPtrs_; + std::vector> dataGenerationFtrs_; + data_array_t dataArray_; + // policy evaluation variables + size_t nPolicyEvaluationThreads_; + std::atomic_int nPolicyEvaluationTasksDone_; + std::unique_ptr policyEvaluationThreadPoolPtr_; + std::vector> policyEvaluationPtrs_; + std::vector> policyEvaluationFtrs_; +}; + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeConfig.cmake b/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeConfig.cmake new file mode 100644 index 000000000..28cba4e94 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeConfig.cmake @@ -0,0 +1,28 @@ +# Custom cmake config file to enable find_package(onnxruntime) without modifying LIBRARY_PATH and LD_LIBRARY_PATH +# +# This will define the following variables: +# onnxruntime_FOUND -- True if the system has the onnxruntime library +# onnxruntime_INCLUDE_DIRS -- The include directories for onnxruntime +# onnxruntime_LIBRARIES -- Libraries to link against +# onnxruntime_CXX_FLAGS -- Additional (required) compiler flags + +include(FindPackageHandleStandardArgs) + +# Assume we are in /share/cmake/onnxruntime/onnxruntimeConfig.cmake +get_filename_component(CMAKE_CURRENT_LIST_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH) +get_filename_component(onnxruntime_INSTALL_PREFIX "${CMAKE_CURRENT_LIST_DIR}/../../../" ABSOLUTE) + +set(onnxruntime_INCLUDE_DIRS ${onnxruntime_INSTALL_PREFIX}/include) +set(onnxruntime_LIBRARIES onnxruntime) +set(onnxruntime_CXX_FLAGS "") # no flags needed + +find_library(onnxruntime_LIBRARY onnxruntime + PATHS "${onnxruntime_INSTALL_PREFIX}/lib" +) + +add_library(onnxruntime SHARED IMPORTED) +set_property(TARGET onnxruntime PROPERTY IMPORTED_LOCATION "${onnxruntime_LIBRARY}") +set_property(TARGET onnxruntime PROPERTY INTERFACE_INCLUDE_DIRECTORIES "${onnxruntime_INCLUDE_DIRS}") +set_property(TARGET onnxruntime PROPERTY INTERFACE_COMPILE_OPTIONS "${onnxruntime_CXX_FLAGS}") + +find_package_handle_standard_args(onnxruntime DEFAULT_MSG onnxruntime_LIBRARY onnxruntime_INCLUDE_DIRS) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeVersion.cmake b/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeVersion.cmake new file mode 100644 index 000000000..8dbbb0498 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/misc/onnxruntime/cmake/onnxruntimeVersion.cmake @@ -0,0 +1,13 @@ +# Custom cmake version file + +set(PACKAGE_VERSION "1.7.0") + +# Check whether the requested PACKAGE_FIND_VERSION is compatible +if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}") + set(PACKAGE_VERSION_COMPATIBLE FALSE) +else() + set(PACKAGE_VERSION_COMPATIBLE TRUE) + if("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}") + set(PACKAGE_VERSION_EXACT TRUE) + endif() +endif() diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/package.xml b/ocs2_mpcnet/ocs2_mpcnet_core/package.xml new file mode 100644 index 000000000..48eb0efea --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/package.xml @@ -0,0 +1,24 @@ + + + ocs2_mpcnet_core + 0.0.0 + The ocs2_mpcnet_core package + + Alexander Reske + + Farbod Farshidian + Alexander Reske + + BSD-3 + + catkin + + cmake_clang_tools + + pybind11_catkin + + ocs2_mpc + ocs2_python_interface + ocs2_ros_interfaces + + diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/__init__.py new file mode 100644 index 000000000..0b2bd554f --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/__init__.py @@ -0,0 +1,7 @@ +from ocs2_mpcnet_core.MpcnetPybindings import size_array, scalar_array, vector_array, matrix_array +from ocs2_mpcnet_core.MpcnetPybindings import ScalarFunctionQuadraticApproximation +from ocs2_mpcnet_core.MpcnetPybindings import SystemObservation, SystemObservationArray +from ocs2_mpcnet_core.MpcnetPybindings import ModeSchedule, ModeScheduleArray +from ocs2_mpcnet_core.MpcnetPybindings import TargetTrajectories, TargetTrajectoriesArray +from ocs2_mpcnet_core.MpcnetPybindings import DataPoint, DataArray +from ocs2_mpcnet_core.MpcnetPybindings import Metrics, MetricsArray diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py new file mode 100644 index 000000000..1943898aa --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/config.py @@ -0,0 +1,70 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Configuration class. + +Provides a class that handles the configuration parameters. +""" + +import yaml +import torch + + +class Config: + """Config. + + Loads configuration parameters from a YAML file and provides access to them as attributes of this class. + + Attributes: + DTYPE: The PyTorch data type. + DEVICE: The PyTorch device to select. + """ + + def __init__(self, config_file_path: str) -> None: + """Initializes the Config class. + + Initializes the Config class by setting fixed attributes and by loading attributes from a YAML file. + + Args: + config_file_path: A string with the path to the configuration file. + """ + # + # class config + # + # data type for tensor elements + self.DTYPE = torch.float + # device on which tensors will be allocated + self.DEVICE = torch.device("cuda") + # + # yaml config + # + with open(config_file_path, "r") as stream: + config = yaml.safe_load(stream) + for key, value in config.items(): + setattr(self, key, value) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py new file mode 100644 index 000000000..345e7d96f --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/helper.py @@ -0,0 +1,309 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Helper functions. + +Provides helper functions, such as convenience functions for batch-wise operations or access to OCC2 types. +""" + +import torch +import numpy as np +from typing import Tuple, Dict + +from ocs2_mpcnet_core import ( + size_array, + scalar_array, + vector_array, + SystemObservation, + SystemObservationArray, + ModeSchedule, + ModeScheduleArray, + TargetTrajectories, + TargetTrajectoriesArray, +) + + +def bdot(bv1: torch.Tensor, bv2: torch.Tensor) -> torch.Tensor: + """Batch-wise dot product. + + Performs a batch-wise dot product between two batches of vectors with batch size B and dimension N. Supports + broadcasting for the batch dimension. + + Args: + bv1: A (B,N) tensor containing a batch of vectors. + bv2: A (B,N) tensor containing a batch of vectors. + + Returns: + A (B) tensor containing the batch-wise dot product. + """ + return torch.sum(torch.mul(bv1, bv2), dim=1) + + +def bmv(bm: torch.Tensor, bv: torch.Tensor) -> torch.Tensor: + """Batch-wise matrix-vector product. + + Performs a batch-wise matrix-vector product between a batch of MxN matrices and a batch of vectors of dimension N, + each with batch size B. Supports broadcasting for the batch dimension. + + Args: + bm: A (B,M,N) tensor containing a batch of matrices. + bv: A (B,N) tensor containing a batch of vectors. + + Returns: + A (B,M) tensor containing the batch-wise matrix-vector product. + """ + return torch.matmul(bm, bv.unsqueeze(dim=2)).squeeze(dim=2) + + +def bmm(bm1: torch.Tensor, bm2: torch.Tensor) -> torch.Tensor: + """Batch-wise matrix-matrix product. + + Performs a batch-wise matrix-matrix product between a batch of MxK matrices and a batch of KxN matrices, each with + batch size B. Supports broadcasting for the batch dimension (unlike torch.bmm). + + Args: + bm1: A (B,M,K) tensor containing a batch of matrices. + bm2: A (B,K,N) tensor containing a batch of matrices. + + Returns: + A (B,M,N) tensor containing the batch-wise matrix-matrix product. + """ + return torch.matmul(bm1, bm2) + + +def get_size_array(data: np.ndarray) -> size_array: + """Get an OCS2 size array. + + Creates an OCS2 size array and fills it with integer data from a NumPy array. + + Args: + data: A NumPy array of shape (N) containing integers. + + Returns: + An OCS2 size array of length N. + """ + my_size_array = size_array() + my_size_array.resize(len(data)) + for i in range(len(data)): + my_size_array[i] = data[i] + return my_size_array + + +def get_scalar_array(data: np.ndarray) -> scalar_array: + """Get an OCS2 scalar array. + + Creates an OCS2 scalar array and fills it with float data from a NumPy array. + + Args: + data: A NumPy array of shape (N) containing floats. + + Returns: + An OCS2 scalar array of length N. + """ + my_scalar_array = scalar_array() + my_scalar_array.resize(len(data)) + for i in range(len(data)): + my_scalar_array[i] = data[i] + return my_scalar_array + + +def get_vector_array(data: np.ndarray) -> vector_array: + """Get an OCS2 vector array. + + Creates an OCS2 vector array and fills it with float data from a NumPy array. + + Args: + data: A NumPy array of shape (M,N) containing floats. + + Returns: + An OCS2 vector array of length M with vectors of dimension N. + """ + my_vector_array = vector_array() + my_vector_array.resize(len(data)) + for i in range(len(data)): + my_vector_array[i] = data[i] + return my_vector_array + + +def get_system_observation(mode: int, time: float, state: np.ndarray, input: np.ndarray) -> SystemObservation: + """Get an OCS2 system observation object. + + Creates an OCS2 system observation object and fills it with data. + + Args: + mode: The observed mode given by an integer. + time: The observed time given by a float. + state: The observed state given by a NumPy array of shape (M) containing floats. + input: The observed input given by a NumPy array of shape (N) containing floats. + + Returns: + An OCS2 system observation object. + """ + system_observation = SystemObservation() + system_observation.mode = mode + system_observation.time = time + system_observation.state = state + system_observation.input = input + return system_observation + + +def get_system_observation_array(length: int) -> SystemObservationArray: + """Get an OCS2 system observation array. + + Creates an OCS2 system observation array but does not fill it with data. + + Args: + length: The length that the array should have given by an integer. + + Returns: + An OCS2 system observation array of the desired length. + """ + system_observation_array = SystemObservationArray() + system_observation_array.resize(length) + return system_observation_array + + +def get_target_trajectories( + time_trajectory: np.ndarray, state_trajectory: np.ndarray, input_trajectory: np.ndarray +) -> TargetTrajectories: + """Get an OCS2 target trajectories object. + + Creates an OCS2 target trajectories object and fills it with data. + + Args: + time_trajectory: The target time trajectory given by a NumPy array of shape (K) containing floats. + state_trajectory: The target state trajectory given by a NumPy array of shape (K,M) containing floats. + input_trajectory: The target input trajectory given by a NumPy array of shape (K,N) containing floats. + + Returns: + An OCS2 target trajectories object. + """ + time_trajectory_array = get_scalar_array(time_trajectory) + state_trajectory_array = get_vector_array(state_trajectory) + input_trajectory_array = get_vector_array(input_trajectory) + return TargetTrajectories(time_trajectory_array, state_trajectory_array, input_trajectory_array) + + +def get_target_trajectories_array(length: int) -> TargetTrajectoriesArray: + """Get an OCS2 target trajectories array. + + Creates an OCS2 target trajectories array but does not fill it with data. + + Args: + length: The length that the array should have given by an integer. + + Returns: + An OCS2 target trajectories array of the desired length. + """ + target_trajectories_array = TargetTrajectoriesArray() + target_trajectories_array.resize(length) + return target_trajectories_array + + +def get_mode_schedule(event_times: np.ndarray, mode_sequence: np.ndarray) -> ModeSchedule: + """Get an OCS2 mode schedule object. + + Creates an OCS2 mode schedule object and fills it with data. + + Args: + event_times: The event times given by a NumPy array of shape (K-1) containing floats. + mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + + Returns: + An OCS2 mode schedule object. + """ + event_times_array = get_scalar_array(event_times) + mode_sequence_array = get_size_array(mode_sequence) + return ModeSchedule(event_times_array, mode_sequence_array) + + +def get_mode_schedule_array(length: int) -> ModeScheduleArray: + """Get an OCS2 mode schedule array. + + Creates an OCS2 mode schedule array but does not fill it with data. + + Args: + length: The length that the array should have given by an integer. + + Returns: + An OCS2 mode schedule array of the desired length. + """ + mode_schedule_array = ModeScheduleArray() + mode_schedule_array.resize(length) + return mode_schedule_array + + +def get_event_times_and_mode_sequence( + default_mode: int, duration: float, event_times_template: np.ndarray, mode_sequence_template: np.ndarray +) -> Tuple[np.ndarray, np.ndarray]: + """Get the event times and mode sequence describing a mode schedule. + + Creates the event times and mode sequence for a certain time duration from a template (e.g. a gait). + + Args: + default_mode: The default mode prepended and appended to the mode schedule and given by an integer. + duration: The duration of the mode schedule given by a float. + event_times_template: The event times template given by a NumPy array of shape (T) containing floats. + mode_sequence_template: The mode sequence template given by a NumPy array of shape (T) containing integers. + + Returns: + A tuple containing the components of the mode schedule. + - event_times: The event times given by a NumPy array of shape (K-1) containing floats. + - mode_sequence: The mode sequence given by a NumPy array of shape (K) containing integers. + """ + gait_cycle_duration = event_times_template[-1] + num_gait_cycles = int(np.floor(duration / gait_cycle_duration)) + event_times = np.array([0.0], dtype=np.float64) + mode_sequence = np.array([default_mode], dtype=np.uintp) + for _ in range(num_gait_cycles): + event_times = np.append( + event_times, event_times[-1] * np.ones(len(event_times_template)) + event_times_template + ) + mode_sequence = np.append(mode_sequence, mode_sequence_template) + mode_sequence = np.append(mode_sequence, np.array([default_mode], dtype=np.uintp)) + return event_times, mode_sequence + + +def get_one_hot(mode: int, expert_number: int, expert_for_mode: Dict[int, int]) -> np.ndarray: + """Get one hot encoding of mode. + + Get a one hot encoding of a mode represented by a discrete probability distribution, where the sample space is the + set of P individually identified items given by the set of E individually identified experts. + + Args: + mode: The mode of the system given by an integer. + expert_number: The number of experts given by an integer. + expert_for_mode: A dictionary that assigns modes to experts. + + Returns: + p: Discrete probability distribution given by a NumPy array of shape (P) containing floats. + """ + one_hot = np.zeros(expert_number) + one_hot[expert_for_mode[mode]] = 1.0 + return one_hot diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py new file mode 100644 index 000000000..c343a60cb --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/__init__.py @@ -0,0 +1,6 @@ +from .base import BaseLoss +from .behavioral_cloning import BehavioralCloningLoss +from .cross_entropy import CrossEntropyLoss +from .hamiltonian import HamiltonianLoss + +__all__ = ["BaseLoss", "BehavioralCloningLoss", "CrossEntropyLoss", "HamiltonianLoss"] diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py new file mode 100644 index 000000000..329c58089 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/base.py @@ -0,0 +1,94 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Base loss. + +Provides a base class for all loss classes. +""" + +import torch +from abc import ABCMeta, abstractmethod + +from ocs2_mpcnet_core.config import Config + + +class BaseLoss(metaclass=ABCMeta): + """Base loss. + + Provides the interface to all loss classes. + """ + + def __init__(self, config: Config) -> None: + """Initializes the BaseLoss class. + + Initializes the BaseLoss class. + + Args: + config: An instance of the configuration class. + """ + pass + + @abstractmethod + def __call__( + self, + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + p_query: torch.Tensor, + p_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: + """Computes the loss. + + Computes the mean loss for a batch. + + Args: + x_query: A (B,X) tensor with the query (e.g. predicted) states. + x_nominal: A (B,X) tensor with the nominal (e.g. target) states. + u_query: A (B,U) tensor with the query (e.g. predicted) inputs. + u_nominal: A (B,U) tensor with the nominal (e.g. target) inputs. + p_query: A (B,P) tensor with the query (e.g. predicted) discrete probability distributions. + p_nominal: A (B,P) tensor with the nominal (e.g. target) discrete probability distributions. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the nominal points. + + Returns: + A (1) tensor containing the mean loss. + """ + pass diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py new file mode 100644 index 000000000..5dda2c2d3 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/behavioral_cloning.py @@ -0,0 +1,119 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Behavioral cloning loss. + +Provides a class that implements a simple behavioral cloning loss for benchmarking MPC-Net. +""" + +import torch + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.base import BaseLoss +from ocs2_mpcnet_core.helper import bdot, bmv + + +class BehavioralCloningLoss(BaseLoss): + r"""Behavioral cloning loss. + + Uses a simple quadratic function as loss: + + .. math:: + + BC(u) = \delta u^T R \, \delta u, + + where the input u is of dimension U. + + Attributes: + R: A (1,U,U) tensor with the input cost matrix. + """ + + def __init__(self, config: Config) -> None: + """Initializes the BehavioralCloningLoss class. + + Initializes the BehavioralCloningLoss class by setting fixed attributes. + + Args: + config: An instance of the configuration class. + """ + super().__init__(config) + self.R = torch.tensor(config.R, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + + def __call__( + self, + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + p_query: torch.Tensor, + p_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: + """Computes the loss. + + Computes the mean loss for a batch. + + Args: + x_query: A (B,X) tensor with the query (e.g. predicted) states. + x_nominal: A (B,X) tensor with the nominal (e.g. target) states. + u_query: A (B,U) tensor with the query (e.g. predicted) inputs. + u_nominal: A (B,U) tensor with the nominal (e.g. target) inputs. + p_query: A (B,P) tensor with the query (e.g. predicted) discrete probability distributions. + p_nominal: A (B,P) tensor with the nominal (e.g. target) discrete probability distributions. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the nominal points. + + Returns: + A (1) tensor containing the mean loss. + """ + return torch.mean(self.compute(u_query, u_nominal)) + + def compute(self, u_predicted: torch.Tensor, u_target: torch.Tensor) -> torch.Tensor: + """Computes the behavioral cloning losses for a batch. + + Computes the behavioral cloning losses for a batch of size B using the cost matrix. + + Args: + u_predicted: A (B, U) tensor with the predicted inputs. + u_target: A (B, U) tensor with the target inputs. + + Returns: + A (B) tensor containing the behavioral cloning losses. + """ + du = torch.sub(u_predicted, u_target) + return bdot(du, bmv(self.R, du)) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py new file mode 100644 index 000000000..93cc706c0 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/cross_entropy.py @@ -0,0 +1,118 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Cross entropy loss. + +Provides a class that implements the cross entropy loss for training a gating network of a mixture of experts network. +""" + +import torch + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.base import BaseLoss +from ocs2_mpcnet_core.helper import bdot, bmv + + +class CrossEntropyLoss(BaseLoss): + r"""Cross entropy loss. + + Uses the cross entropy between two discrete probability distributions as loss: + + .. math:: + + CE(p_{target}, p_{predicted}) = - \sum_{i=1}^{P} (p_{target,i} \log(p_{predicted,i} + \varepsilon)), + + where the sample space is the set of P individually identified items. + + Attributes: + epsilon: A (1) tensor with a small epsilon used to stabilize the logarithm. + """ + + def __init__(self, config: Config) -> None: + """Initializes the CrossEntropyLoss class. + + Initializes the CrossEntropyLoss class by setting fixed attributes. + + Args: + config: An instance of the configuration class. + """ + super().__init__(config) + self.epsilon = torch.tensor(config.EPSILON, device=config.DEVICE, dtype=config.DTYPE) + + def __call__( + self, + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + p_query: torch.Tensor, + p_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: + """Computes the loss. + + Computes the mean loss for a batch. + + Args: + x_query: A (B,X) tensor with the query (e.g. predicted) states. + x_nominal: A (B,X) tensor with the nominal (e.g. target) states. + u_query: A (B,U) tensor with the query (e.g. predicted) inputs. + u_nominal: A (B,U) tensor with the nominal (e.g. target) inputs. + p_query: A (B,P) tensor with the query (e.g. predicted) discrete probability distributions. + p_nominal: A (B,P) tensor with the nominal (e.g. target) discrete probability distributions. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the nominal points. + + Returns: + A (1) tensor containing the mean loss. + """ + return torch.mean(self.compute(p_query, p_nominal)) + + def compute(self, p_predicted: torch.Tensor, p_target: torch.Tensor) -> torch.Tensor: + """Computes the cross entropy losses for a batch. + + Computes the cross entropy losses for a batch, where the logarithm is stabilized by a small epsilon. + + Args: + p_predicted: A (B,P) tensor with the predicted discrete probability distributions. + p_target: A (B,P) tensor with the target discrete probability distributions. + + Returns: + A (B) tensor containing the cross entropy losses. + """ + return -bdot(p_target, torch.log(p_predicted + self.epsilon)) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py new file mode 100644 index 000000000..3def1f538 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/loss/hamiltonian.py @@ -0,0 +1,151 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Hamiltonian loss. + +Provides a class that implements the Hamiltonian loss for MPC-Net. +""" + +import torch + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss.base import BaseLoss +from ocs2_mpcnet_core.helper import bdot, bmv + + +class HamiltonianLoss(BaseLoss): + r"""Hamiltonian loss. + + Uses the linear quadratic approximation of the Hamiltonian as loss: + + .. math:: + + H(x,u) = \frac{1}{2} \delta x^T \partial H_{xx} \delta x +\delta u^T \partial H_{ux} \delta x + \frac{1}{2} + \delta u^T \partial H_{uu} \delta u + \partial H_{x}^T \delta x + \partial H_{u}^T \delta u + H, + + where the state x is of dimension X and the input u is of dimension U. + """ + + def __init__(self, config: Config) -> None: + """Initializes the HamiltonianLoss class. + + Initializes the HamiltonianLoss class. + + Args: + config: An instance of the configuration class. + """ + super().__init__(config) + + def __call__( + self, + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + p_query: torch.Tensor, + p_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: + """Computes the loss. + + Computes the mean loss for a batch. + + Args: + x_query: A (B,X) tensor with the query (e.g. predicted) states. + x_nominal: A (B,X) tensor with the nominal (e.g. target) states. + u_query: A (B,U) tensor with the query (e.g. predicted) inputs. + u_nominal: A (B,U) tensor with the nominal (e.g. target) inputs. + p_query: A (B,P) tensor with the query (e.g. predicted) discrete probability distributions. + p_nominal: A (B,P) tensor with the nominal (e.g. target) discrete probability distributions. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the nominal points. + + Returns: + A (1) tensor containing the mean loss. + """ + return torch.mean(self.compute(x_query, x_nominal, u_query, u_nominal, dHdxx, dHdux, dHduu, dHdx, dHdu, H)) + + @staticmethod + def compute( + x_query: torch.Tensor, + x_nominal: torch.Tensor, + u_query: torch.Tensor, + u_nominal: torch.Tensor, + dHdxx: torch.Tensor, + dHdux: torch.Tensor, + dHduu: torch.Tensor, + dHdx: torch.Tensor, + dHdu: torch.Tensor, + H: torch.Tensor, + ) -> torch.Tensor: + """Computes the Hamiltonian losses for a batch. + + Computes the Hamiltonian losses for a batch of size B using the provided linear quadratic approximations. + + Args: + x_query: A (B,X) tensor with the states the Hamiltonian loss should be computed for. + x_nominal: A (B,X) tensor with the states that were used as development/expansion points. + u_query: A (B,U) tensor with the inputs the Hamiltonian loss should be computed for. + u_nominal: A (B,U) tensor with the inputs that were used as development/expansion point. + dHdxx: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + dHdux: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + dHduu: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + dHdx: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + dHdu: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + H: A (B) tensor with the Hamiltonians at the development/expansion points. + + Returns: + A (B) tensor containing the computed Hamiltonian losses. + """ + if torch.equal(x_query, x_nominal): + du = torch.sub(u_query, u_nominal) + return 0.5 * bdot(du, bmv(dHduu, du)) + bdot(dHdu, du) + H + elif torch.equal(u_query, u_nominal): + dx = torch.sub(x_query, x_nominal) + return 0.5 * bdot(dx, bmv(dHdxx, dx)) + bdot(dHdx, dx) + H + else: + dx = torch.sub(x_query, x_nominal) + du = torch.sub(u_query, u_nominal) + return ( + 0.5 * bdot(dx, bmv(dHdxx, dx)) + + bdot(du, bmv(dHdux, dx)) + + 0.5 * bdot(du, bmv(dHduu, du)) + + bdot(dHdx, dx) + + bdot(dHdu, du) + + H + ) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py new file mode 100644 index 000000000..7d777af72 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/__init__.py @@ -0,0 +1,4 @@ +from .base import BaseMemory +from .circular import CircularMemory + +__all__ = ["BaseMemory", "CircularMemory"] diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py new file mode 100644 index 000000000..5c1b26fec --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/base.py @@ -0,0 +1,122 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Base memory. + +Provides a base class for all memory classes. +""" + +import torch +import numpy as np +from typing import Tuple, List +from abc import ABCMeta, abstractmethod + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core import ScalarFunctionQuadraticApproximation + + +class BaseMemory(metaclass=ABCMeta): + """Base memory. + + Provides the interface to all memory classes. + """ + + def __init__(self, config: Config) -> None: + """Initializes the BaseMemory class. + + Initializes the BaseMemory class. + + Args: + config: An instance of the configuration class. + """ + pass + + @abstractmethod + def push( + self, + t: float, + x: np.ndarray, + u: np.ndarray, + p: np.ndarray, + observation: np.ndarray, + action_transformation: List[np.ndarray], + hamiltonian: ScalarFunctionQuadraticApproximation, + ) -> None: + """Pushes data into the memory. + + Pushes one data sample into the memory. + + Args: + t: A float with the time. + x: A NumPy array of shape (X) with the observed state. + u: A NumPy array of shape (U) with the optimal input. + p: A NumPy array of shape (P) tensor for the observed discrete probability distributions of the modes. + observation: A NumPy array of shape (O) with the generalized times. + action_transformation: A list containing NumPy arrays of shape (U,A) and (U) with the action transformation. + hamiltonian: An OCS2 scalar function quadratic approximation representing the Hamiltonian around x and u. + """ + pass + + @abstractmethod + def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: + """Samples data from the memory. + + Samples a batch of data from the memory. + + Args: + batch_size: An integer defining the batch size B. + + Returns: + A tuple containing the sampled batch of data. + - t_batch: A (B) tensor with the times. + - x_batch: A (B,X) tensor with the observed states. + - u_batch: A (B,U) tensor with the optimal inputs. + - p_batch: A (B,P) tensor with the observed discrete probability distributions of the modes. + - observation_batch: A (B,O) tensor with the observations. + - action_transformation_matrix_batch: A (B,U,A) tensor with the action transformation matrices. + - action_transformation_vector_batch: A (B,U) tensor with the action transformation vectors. + - dHdxx_batch: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + - dHdux_batch: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + - dHduu_batch: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + - dHdx_batch: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + - dHdu_batch: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + - H_batch: A (B) tensor with the Hamiltonians at the development/expansion points. + """ + pass + + @abstractmethod + def __len__(self) -> int: + """The length of the memory. + + Return the length of the memory given by the current size. + + Returns: + An integer describing the length of the memory. + """ + pass diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py new file mode 100644 index 000000000..70decc31b --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/memory/circular.py @@ -0,0 +1,218 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Circular memory. + +Provides a class that implements a circular memory. +""" + +import torch +import numpy as np +from typing import Tuple, List + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.memory.base import BaseMemory +from ocs2_mpcnet_core import ScalarFunctionQuadraticApproximation + + +class CircularMemory(BaseMemory): + """Circular memory. + + Stores data in a circular memory that overwrites old data if the size of the memory reaches its capacity. + + Attributes: + capacity: An integer defining the capacity of the memory. + size: An integer giving the current size of the memory. + position: An integer giving the current position in the memory. + t: A (C) tensor for the times. + x: A (C,X) tensor for the observed states. + u: A (C,U) tensor for the optimal inputs. + p: A (C,P) tensor for the observed discrete probability distributions of the modes. + observation: A (C,O) tensor for the observations. + action_transformation_matrix: A (C,U,A) tensor for the action transformation matrices. + action_transformation_vector: A (C,U) tensor for the action transformation vectors. + dHdxx: A (C,X,X) tensor for the state-state Hessians of the Hamiltonian approximations. + dHdux: A (C,U,X) tensor for the input-state Hessians of the Hamiltonian approximations. + dHduu: A (C,U,U) tensor for the input-input Hessians of the Hamiltonian approximations. + dHdx: A (C,X) tensor for the state gradients of the Hamiltonian approximations. + dHdu: A (C,U) tensor for the input gradients of the Hamiltonian approximations. + H: A (C) tensor for the Hamiltonians at the development/expansion points. + """ + + def __init__(self, config: Config) -> None: + """Initializes the CircularMemory class. + + Initializes the CircularMemory class by setting fixed attributes, initializing variable attributes and + pre-allocating memory. + + Args: + config: An instance of the configuration class. + """ + # init variables + self.device = config.DEVICE + self.capacity = config.CAPACITY + self.size = 0 + self.position = 0 + # pre-allocate memory + self.t = torch.zeros(config.CAPACITY, device=config.DEVICE, dtype=config.DTYPE) + self.x = torch.zeros(config.CAPACITY, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE) + self.u = torch.zeros(config.CAPACITY, config.INPUT_DIM, device=config.DEVICE, dtype=config.DTYPE) + self.p = torch.zeros(config.CAPACITY, config.EXPERT_NUM, device=config.DEVICE, dtype=config.DTYPE) + self.observation = torch.zeros( + config.CAPACITY, config.OBSERVATION_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.action_transformation_matrix = torch.zeros( + config.CAPACITY, config.INPUT_DIM, config.ACTION_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.action_transformation_vector = torch.zeros( + config.CAPACITY, config.INPUT_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.dHdxx = torch.zeros( + config.CAPACITY, config.STATE_DIM, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.dHdux = torch.zeros( + config.CAPACITY, config.INPUT_DIM, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.dHduu = torch.zeros( + config.CAPACITY, config.INPUT_DIM, config.INPUT_DIM, device=config.DEVICE, dtype=config.DTYPE + ) + self.dHdx = torch.zeros(config.CAPACITY, config.STATE_DIM, device=config.DEVICE, dtype=config.DTYPE) + self.dHdu = torch.zeros(config.CAPACITY, config.INPUT_DIM, device=config.DEVICE, dtype=config.DTYPE) + self.H = torch.zeros(config.CAPACITY, device=config.DEVICE, dtype=config.DTYPE) + + def push( + self, + t: float, + x: np.ndarray, + u: np.ndarray, + p: np.ndarray, + observation: np.ndarray, + action_transformation: List[np.ndarray], + hamiltonian: ScalarFunctionQuadraticApproximation, + ) -> None: + """Pushes data into the memory. + + Pushes one data sample into the memory. + + Args: + t: A float with the time. + x: A NumPy array of shape (X) with the observed state. + u: A NumPy array of shape (U) with the optimal input. + p: A NumPy array of shape (P) tensor for the observed discrete probability distributions of the modes. + observation: A NumPy array of shape (O) with the generalized times. + action_transformation: A list containing NumPy arrays of shape (U,A) and (U) with the action transformation. + hamiltonian: An OCS2 scalar function quadratic approximation representing the Hamiltonian around x and u. + """ + # push data into memory + # note: - torch.as_tensor: no copy as data is a ndarray of the corresponding dtype and the device is the cpu + # - torch.Tensor.copy_: copy performed together with potential dtype and device change + self.t[self.position].copy_(torch.as_tensor(t, dtype=None, device=torch.device("cpu"))) + self.x[self.position].copy_(torch.as_tensor(x, dtype=None, device=torch.device("cpu"))) + self.u[self.position].copy_(torch.as_tensor(u, dtype=None, device=torch.device("cpu"))) + self.p[self.position].copy_(torch.as_tensor(p, dtype=None, device=torch.device("cpu"))) + self.observation[self.position].copy_(torch.as_tensor(observation, dtype=None, device=torch.device("cpu"))) + self.action_transformation_matrix[self.position].copy_( + torch.as_tensor(action_transformation[0], dtype=None, device=torch.device("cpu")) + ) + self.action_transformation_vector[self.position].copy_( + torch.as_tensor(action_transformation[1], dtype=None, device=torch.device("cpu")) + ) + self.dHdxx[self.position].copy_(torch.as_tensor(hamiltonian.dfdxx, dtype=None, device=torch.device("cpu"))) + self.dHdux[self.position].copy_(torch.as_tensor(hamiltonian.dfdux, dtype=None, device=torch.device("cpu"))) + self.dHduu[self.position].copy_(torch.as_tensor(hamiltonian.dfduu, dtype=None, device=torch.device("cpu"))) + self.dHdx[self.position].copy_(torch.as_tensor(hamiltonian.dfdx, dtype=None, device=torch.device("cpu"))) + self.dHdu[self.position].copy_(torch.as_tensor(hamiltonian.dfdu, dtype=None, device=torch.device("cpu"))) + self.H[self.position].copy_(torch.as_tensor(hamiltonian.f, dtype=None, device=torch.device("cpu"))) + # update size and position + self.size = min(self.size + 1, self.capacity) + self.position = (self.position + 1) % self.capacity + + def sample(self, batch_size: int) -> Tuple[torch.Tensor, ...]: + """Samples data from the memory. + + Samples a batch of data from the memory. + + Args: + batch_size: An integer defining the batch size B. + + Returns: + A tuple containing the sampled batch of data. + - t_batch: A (B) tensor with the times. + - x_batch: A (B,X) tensor with the observed states. + - u_batch: A (B,U) tensor with the optimal inputs. + - p_batch: A (B,P) tensor with the observed discrete probability distributions of the modes. + - observation_batch: A (B,O) tensor with the observations. + - action_transformation_matrix_batch: A (B,U,A) tensor with the action transformation matrices. + - action_transformation_vector_batch: A (B,U) tensor with the action transformation vectors. + - dHdxx_batch: A (B,X,X) tensor with the state-state Hessians of the Hamiltonian approximations. + - dHdux_batch: A (B,U,X) tensor with the input-state Hessians of the Hamiltonian approximations. + - dHduu_batch: A (B,U,U) tensor with the input-input Hessians of the Hamiltonian approximations. + - dHdx_batch: A (B,X) tensor with the state gradients of the Hamiltonian approximations. + - dHdu_batch: A (B,U) tensor with the input gradients of the Hamiltonian approximations. + - H_batch: A (B) tensor with the Hamiltonians at the development/expansion points. + """ + indices = torch.randint(low=0, high=self.size, size=(batch_size,), device=self.device) + t_batch = self.t[indices] + x_batch = self.x[indices] + u_batch = self.u[indices] + p_batch = self.p[indices] + observation_batch = self.observation[indices] + action_transformation_matrix_batch = self.action_transformation_matrix[indices] + action_transformation_vector_batch = self.action_transformation_vector[indices] + dHdxx_batch = self.dHdxx[indices] + dHdux_batch = self.dHdux[indices] + dHduu_batch = self.dHduu[indices] + dHdx_batch = self.dHdx[indices] + dHdu_batch = self.dHdu[indices] + H_batch = self.H[indices] + return ( + t_batch, + x_batch, + u_batch, + p_batch, + observation_batch, + action_transformation_matrix_batch, + action_transformation_vector_batch, + dHdxx_batch, + dHdux_batch, + dHduu_batch, + dHdx_batch, + dHdu_batch, + H_batch, + ) + + def __len__(self) -> int: + """The length of the memory. + + Return the length of the memory given by the current size. + + Returns: + An integer describing the length of the memory. + """ + return self.size diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py new file mode 100644 index 000000000..a79fbc9d7 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/mpcnet.py @@ -0,0 +1,329 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""MPC-Net class. + +Provides a class that handles the MPC-Net training. +""" + +import os +import time +import datetime +import torch +import numpy as np +from typing import Optional, Tuple +from abc import ABCMeta, abstractmethod +from torch.utils.tensorboard import SummaryWriter + + +from ocs2_mpcnet_core import helper +from ocs2_mpcnet_core import SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.loss import BaseLoss +from ocs2_mpcnet_core.memory import BaseMemory +from ocs2_mpcnet_core.policy import BasePolicy + + +class Mpcnet(metaclass=ABCMeta): + """MPC-Net. + + Implements the main methods for the MPC-Net training. + + Takes a specific configuration, interface, memory, policy and loss function(s). + The task formulation has to be implemented in a robot-specific class derived from this class. + Provides the main training loop for MPC-Net. + """ + + def __init__( + self, + root_dir: str, + config: Config, + interface: object, + memory: BaseMemory, + policy: BasePolicy, + experts_loss: BaseLoss, + gating_loss: Optional[BaseLoss] = None, + ) -> None: + """Initializes the Mpcnet class. + + Initializes the Mpcnet class by setting fixed and variable attributes. + + Args: + root_dir: The absolute path to the root directory. + config: An instance of the configuration class. + interface: An instance of the interface class. + memory: An instance of a memory class. + policy: An instance of a policy class. + experts_loss: An instance of a loss class used as experts loss. + gating_loss: An instance of a loss class used as gating loss. + """ + # config + self.config = config + # interface + self.interface = interface + # logging + timestamp = datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + self.log_dir = os.path.join(root_dir, "runs", f"{timestamp}_{config.NAME}_{config.DESCRIPTION}") + self.writer = SummaryWriter(self.log_dir) + # loss + self.experts_loss = experts_loss + self.gating_loss = gating_loss + # memory + self.memory = memory + # policy + self.policy = policy + self.policy.to(config.DEVICE) + self.dummy_observation = torch.randn(1, config.OBSERVATION_DIM, device=config.DEVICE, dtype=config.DTYPE) + # optimizer + self.optimizer = torch.optim.Adam(self.policy.parameters(), lr=config.LEARNING_RATE) + + @abstractmethod + def get_tasks( + self, tasks_number: int, duration: float + ) -> Tuple[SystemObservationArray, ModeScheduleArray, TargetTrajectoriesArray]: + """Get tasks. + + Get a random set of task that should be executed by the data generation or policy evaluation. + + Args: + tasks_number: Number of tasks given by an integer. + duration: Duration of each task given by a float. + + Returns: + A tuple containing the components of the task. + - initial_observations: The initial observations given by an OCS2 system observation array. + - mode_schedules: The desired mode schedules given by an OCS2 mode schedule array. + - target_trajectories: The desired target trajectories given by an OCS2 target trajectories array. + """ + pass + + def start_data_generation(self, policy: BasePolicy, alpha: float = 1.0): + """Start data generation. + + Start the data generation rollouts to receive new data. + + Args: + policy: The current learned policy. + alpha: The weight of the MPC policy in the rollouts. + """ + policy_file_path = "/tmp/data_generation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=self.dummy_observation, f=policy_file_path) + initial_observations, mode_schedules, target_trajectories = self.get_tasks( + self.config.DATA_GENERATION_TASKS, self.config.DATA_GENERATION_DURATION + ) + self.interface.startDataGeneration( + alpha, + policy_file_path, + self.config.DATA_GENERATION_TIME_STEP, + self.config.DATA_GENERATION_DATA_DECIMATION, + self.config.DATA_GENERATION_SAMPLES, + np.diag(np.power(np.array(self.config.DATA_GENERATION_SAMPLING_VARIANCE), 2)), + initial_observations, + mode_schedules, + target_trajectories, + ) + + def start_policy_evaluation(self, policy: BasePolicy, alpha: float = 0.0): + """Start policy evaluation. + + Start the policy evaluation rollouts to validate the current performance. + + Args: + policy: The current learned policy. + alpha: The weight of the MPC policy in the rollouts. + """ + policy_file_path = "/tmp/policy_evaluation_" + datetime.datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + ".onnx" + torch.onnx.export(model=policy, args=self.dummy_observation, f=policy_file_path) + initial_observations, mode_schedules, target_trajectories = self.get_tasks( + self.config.POLICY_EVALUATION_TASKS, self.config.POLICY_EVALUATION_DURATION + ) + self.interface.startPolicyEvaluation( + alpha, + policy_file_path, + self.config.POLICY_EVALUATION_TIME_STEP, + initial_observations, + mode_schedules, + target_trajectories, + ) + + def train(self) -> None: + """Train. + + Run the main training loop of MPC-Net. + """ + try: + # save initial policy + save_path = self.log_dir + "/initial_policy" + torch.onnx.export(model=self.policy, args=self.dummy_observation, f=save_path + ".onnx") + torch.save(obj=self.policy, f=save_path + ".pt") + + print("==============\nWaiting for first data.\n==============") + self.start_data_generation(self.policy) + self.start_policy_evaluation(self.policy) + while not self.interface.isDataGenerationDone(): + time.sleep(1.0) + + print("==============\nStarting training.\n==============") + for iteration in range(self.config.LEARNING_ITERATIONS): + alpha = 1.0 - 1.0 * iteration / self.config.LEARNING_ITERATIONS + + # data generation + if self.interface.isDataGenerationDone(): + # get generated data + data = self.interface.getGeneratedData() + for i in range(len(data)): + # push t, x, u, p, observation, action transformation, Hamiltonian into memory + self.memory.push( + data[i].t, + data[i].x, + data[i].u, + helper.get_one_hot(data[i].mode, self.config.EXPERT_NUM, self.config.EXPERT_FOR_MODE), + data[i].observation, + data[i].actionTransformation, + data[i].hamiltonian, + ) + # logging + self.writer.add_scalar("data/new_data_points", len(data), iteration) + self.writer.add_scalar("data/total_data_points", len(self.memory), iteration) + print("iteration", iteration, "received data points", len(data), "requesting with alpha", alpha) + # start new data generation + self.start_data_generation(self.policy, alpha) + + # policy evaluation + if self.interface.isPolicyEvaluationDone(): + # get computed metrics + metrics = self.interface.getComputedMetrics() + survival_time = np.mean([metrics[i].survivalTime for i in range(len(metrics))]) + incurred_hamiltonian = np.mean([metrics[i].incurredHamiltonian for i in range(len(metrics))]) + # logging + self.writer.add_scalar("metric/survival_time", survival_time, iteration) + self.writer.add_scalar("metric/incurred_hamiltonian", incurred_hamiltonian, iteration) + print( + "iteration", + iteration, + "received metrics:", + "incurred_hamiltonian", + incurred_hamiltonian, + "survival_time", + survival_time, + ) + # start new policy evaluation + self.start_policy_evaluation(self.policy) + + # save intermediate policy + if (iteration % int(0.1 * self.config.LEARNING_ITERATIONS) == 0) and (iteration > 0): + save_path = self.log_dir + "/intermediate_policy_" + str(iteration) + torch.onnx.export(model=self.policy, args=self.dummy_observation, f=save_path + ".onnx") + torch.save(obj=self.policy, f=save_path + ".pt") + + # extract batch from memory + ( + t, + x, + u, + p, + observation, + action_transformation_matrix, + action_transformation_vector, + dHdxx, + dHdux, + dHduu, + dHdx, + dHdu, + H, + ) = self.memory.sample(self.config.BATCH_SIZE) + + # normal closure only evaluating the experts loss + def normal_closure(): + # clear the gradients + self.optimizer.zero_grad() + # prediction + action = self.policy(observation)[0] + input = helper.bmv(action_transformation_matrix, action) + action_transformation_vector + # compute the empirical loss + empirical_loss = self.experts_loss(x, x, input, u, p, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + # compute the gradients + empirical_loss.backward() + # clip the gradients + if self.config.GRADIENT_CLIPPING: + torch.nn.utils.clip_grad_norm_(self.policy.parameters(), self.config.GRADIENT_CLIPPING_VALUE) + # logging + self.writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) + # return empirical loss + return empirical_loss + + # cheating closure also adding the gating loss (only relevant for mixture of experts networks) + def cheating_closure(): + # clear the gradients + self.optimizer.zero_grad() + # prediction + action, weights = self.policy(observation)[:2] + input = helper.bmv(action_transformation_matrix, action) + action_transformation_vector + # compute the empirical loss + empirical_experts_loss = self.experts_loss(x, x, input, u, p, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_gating_loss = self.gating_loss(x, x, u, u, weights, p, dHdxx, dHdux, dHduu, dHdx, dHdu, H) + empirical_loss = empirical_experts_loss + self.config.LAMBDA * empirical_gating_loss + # compute the gradients + empirical_loss.backward() + # clip the gradients + if self.config.GRADIENT_CLIPPING: + torch.nn.utils.clip_grad_norm_(self.policy.parameters(), self.config.GRADIENT_CLIPPING_VALUE) + # logging + self.writer.add_scalar("objective/empirical_experts_loss", empirical_experts_loss.item(), iteration) + self.writer.add_scalar("objective/empirical_gating_loss", empirical_gating_loss.item(), iteration) + self.writer.add_scalar("objective/empirical_loss", empirical_loss.item(), iteration) + # return empirical loss + return empirical_loss + + # take an optimization step + if self.config.CHEATING: + self.optimizer.step(cheating_closure) + else: + self.optimizer.step(normal_closure) + + # let data generation and policy evaluation finish in last iteration (to avoid a segmentation fault) + if iteration == self.config.LEARNING_ITERATIONS - 1: + while (not self.interface.isDataGenerationDone()) or (not self.interface.isPolicyEvaluationDone()): + time.sleep(1.0) + + print("==============\nTraining completed.\n==============") + + # save final policy + save_path = self.log_dir + "/final_policy" + torch.onnx.export(model=self.policy, args=self.dummy_observation, f=save_path + ".onnx") + torch.save(obj=self.policy, f=save_path + ".pt") + + except KeyboardInterrupt: + # let data generation and policy evaluation finish (to avoid a segmentation fault) + while (not self.interface.isDataGenerationDone()) or (not self.interface.isPolicyEvaluationDone()): + time.sleep(1.0) + print("==============\nTraining interrupted.\n==============") + pass + + self.writer.close() diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py new file mode 100644 index 000000000..3e178b783 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/__init__.py @@ -0,0 +1,13 @@ +from .base import BasePolicy +from .linear import LinearPolicy +from .mixture_of_linear_experts import MixtureOfLinearExpertsPolicy +from .mixture_of_nonlinear_experts import MixtureOfNonlinearExpertsPolicy +from .nonlinear import NonlinearPolicy + +__all__ = [ + "BasePolicy", + "LinearPolicy", + "MixtureOfLinearExpertsPolicy", + "MixtureOfNonlinearExpertsPolicy", + "NonlinearPolicy", +] diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py new file mode 100644 index 000000000..7b9c584c2 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/base.py @@ -0,0 +1,107 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Base policy. + +Provides a base class for all policy classes. +""" + +import torch +from typing import Tuple +from abc import ABCMeta, abstractmethod + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.helper import bmv + + +class BasePolicy(torch.nn.Module, metaclass=ABCMeta): + """Base policy. + + Provides the interface to all policy classes. + + Attributes: + observation_scaling: A (1,O,O) tensor for the observation scaling. + action_scaling: A (1,A,A) tensor for the action scaling. + """ + + def __init__(self, config: Config) -> None: + """Initializes the BasePolicy class. + + Initializes the BasePolicy class. + + Args: + config: An instance of the configuration class. + """ + super().__init__() + self.observation_scaling = ( + torch.tensor(config.OBSERVATION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) + self.action_scaling = ( + torch.tensor(config.ACTION_SCALING, device=config.DEVICE, dtype=config.DTYPE).diag().unsqueeze(dim=0) + ) + + @abstractmethod + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, ...]: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + observation: A (B,O) tensor with the observations. + + Returns: + tuple: A tuple with the predictions, e.g. containing a (B,A) tensor with the predicted actions. + """ + pass + + def scale_observation(self, observation: torch.Tensor) -> torch.Tensor: + """Scale observation. + + Scale the observation with a fixed diagonal matrix. + + Args: + observation: A (B,O) tensor with the observations. + + Returns: + scaled_observation: A (B,O) tensor with the scaled observations. + """ + return bmv(self.observation_scaling, observation) + + def scale_action(self, action: torch.Tensor) -> torch.Tensor: + """Scale action. + + Scale the action with a fixed diagonal matrix. + + Args: + action: A (B,A) tensor with the actions. + + Returns: + scaled_action: A (B,A) tensor with the scaled actions. + """ + return bmv(self.action_scaling, action) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py new file mode 100644 index 000000000..ba9c9fe9c --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/linear.py @@ -0,0 +1,82 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Linear policy. + +Provides a class that implements a linear policy. +""" + +import torch +from typing import Tuple + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.policy.base import BasePolicy + + +class LinearPolicy(BasePolicy): + """Linear policy. + + Class for a simple linear neural network policy. + + Attributes: + name: A string with the name of the policy. + observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. + action_dimension: An integer defining the action (i.e. output) dimension of the policy. + linear: The linear neural network layer. + """ + + def __init__(self, config: Config) -> None: + """Initializes the LinearPolicy class. + + Initializes the LinearPolicy class by setting fixed and variable attributes. + + Args: + config: An instance of the configuration class. + """ + super().__init__(config) + self.name = "LinearPolicy" + self.observation_dimension = config.OBSERVATION_DIM + self.action_dimension = config.ACTION_DIM + self.linear = torch.nn.Linear(self.observation_dimension, self.action_dimension) + + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor]: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + observation: A (B,O) tensor with the observations. + + Returns: + action: A (B,A) tensor with the predicted actions. + """ + scaled_observation = self.scale_observation(observation) + unscaled_action = self.linear(scaled_observation) + action = self.scale_action(unscaled_action) + return (action,) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py new file mode 100644 index 000000000..2527d5e67 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_linear_experts.py @@ -0,0 +1,140 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Mixture of linear experts policy. + +Provides classes that implement a mixture of linear experts policy. +""" + +import torch +from typing import Tuple + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.policy.base import BasePolicy +from ocs2_mpcnet_core.helper import bmv + + +class MixtureOfLinearExpertsPolicy(BasePolicy): + """Mixture of linear experts policy. + + Class for a mixture of experts neural network policy with linear experts. + + Attributes: + name: A string with the name of the policy. + observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. + action_dimension: An integer defining the action (i.e. output) dimension of the policy. + expert_number: An integer defining the number of experts. + gating_net: The gating network. + expert_nets: The expert networks. + """ + + def __init__(self, config: Config) -> None: + """Initializes the MixtureOfLinearExpertsPolicy class. + + Initializes the MixtureOfLinearExpertsPolicy class by setting fixed and variable attributes. + + Args: + config: An instance of the configuration class. + """ + super().__init__(config) + self.name = "MixtureOfLinearExpertsPolicy" + self.observation_dimension = config.OBSERVATION_DIM + self.action_dimension = config.ACTION_DIM + self.expert_number = config.EXPERT_NUM + # gating + self.gating_net = torch.nn.Sequential( + torch.nn.Linear(self.observation_dimension, self.expert_number), torch.nn.Softmax(dim=1) + ) + # experts + self.expert_nets = torch.nn.ModuleList( + [_LinearExpert(i, self.observation_dimension, self.action_dimension) for i in range(self.expert_number)] + ) + + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + observation: A (B,O) tensor with the observations. + + Returns: + action: A (B,A) tensor with the predicted actions. + expert_weights: A (B,E) tensor with the predicted expert weights. + """ + scaled_observation = self.scale_observation(observation) + expert_weights = self.gating_net(scaled_observation) + expert_actions = torch.stack( + [self.expert_nets[i](scaled_observation) for i in range(self.expert_number)], dim=2 + ) + unscaled_action = bmv(expert_actions, expert_weights) + action = self.scale_action(unscaled_action) + return action, expert_weights + + +class _LinearExpert(torch.nn.Module): + """Linear expert. + + Class for a simple linear neural network expert. + + Attributes: + name: A string with the name of the expert. + input_dimension: An integer defining the input dimension of the expert. + output_dimension: An integer defining the output dimension of the expert. + linear: The linear neural network layer. + """ + + def __init__(self, index: int, input_dimension: int, output_dimension: int) -> None: + """Initializes the _LinearExpert class. + + Initializes the _LinearExpert class by setting fixed and variable attributes. + + Args: + index: An integer with the index of the expert. + input_dimension: An integer defining the input dimension of the expert. + output_dimension: An integer defining the output dimension of the expert. + """ + super().__init__() + self.name = "LinearExpert" + str(index) + self.input_dimension = input_dimension + self.output_dimension = output_dimension + self.linear = torch.nn.Linear(self.input_dimension, self.output_dimension) + + def forward(self, input: torch.Tensor) -> torch.Tensor: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + input: A (B,I) tensor with the inputs. + + Returns: + output: A (B,O) tensor with the outputs. + """ + return self.linear(input) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py new file mode 100644 index 000000000..6fed0da20 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/mixture_of_nonlinear_experts.py @@ -0,0 +1,159 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Mixture of nonlinear experts policy. + +Provides classes that implement a mixture of nonlinear experts policy. +""" + +import torch +from typing import Tuple + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.policy.base import BasePolicy +from ocs2_mpcnet_core.helper import bmv + + +class MixtureOfNonlinearExpertsPolicy(BasePolicy): + """Mixture of nonlinear experts policy. + + Class for a mixture of experts neural network policy with nonlinear experts, where the hidden layer dimension is the + mean of the input and output dimensions. + + Attributes: + name: A string with the name of the policy. + observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. + gating_hidden_dimension: An integer defining the dimension of the hidden layer for the gating network. + expert_hidden_dimension: An integer defining the dimension of the hidden layer for the expert networks. + action_dimension: An integer defining the action (i.e. output) dimension of the policy. + expert_number: An integer defining the number of experts. + gating_net: The gating network. + expert_nets: The expert networks. + """ + + def __init__(self, config: Config) -> None: + """Initializes the MixtureOfNonlinearExpertsPolicy class. + + Initializes the MixtureOfNonlinearExpertsPolicy class by setting fixed and variable attributes. + + Args: + config: An instance of the configuration class. + """ + super().__init__(config) + self.name = "MixtureOfNonlinearExpertsPolicy" + self.observation_dimension = config.OBSERVATION_DIM + self.gating_hidden_dimension = int((config.OBSERVATION_DIM + config.EXPERT_NUM) / 2) + self.expert_hidden_dimension = int((config.OBSERVATION_DIM + config.ACTION_DIM) / 2) + self.action_dimension = config.ACTION_DIM + self.expert_number = config.EXPERT_NUM + # gating + self.gating_net = torch.nn.Sequential( + torch.nn.Linear(self.observation_dimension, self.gating_hidden_dimension), + torch.nn.Tanh(), + torch.nn.Linear(self.gating_hidden_dimension, self.expert_number), + torch.nn.Softmax(dim=1), + ) + # experts + self.expert_nets = torch.nn.ModuleList( + [ + _NonlinearExpert(i, self.observation_dimension, self.expert_hidden_dimension, self.action_dimension) + for i in range(self.expert_number) + ] + ) + + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor, torch.Tensor]: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + observation: A (B,O) tensor with the observations. + + Returns: + action: A (B,A) tensor with the predicted actions. + expert_weights: A (B,E) tensor with the predicted expert weights. + """ + scaled_observation = self.scale_observation(observation) + expert_weights = self.gating_net(scaled_observation) + expert_actions = torch.stack( + [self.expert_nets[i](scaled_observation) for i in range(self.expert_number)], dim=2 + ) + unscaled_action = bmv(expert_actions, expert_weights) + action = self.scale_action(unscaled_action) + return action, expert_weights + + +class _NonlinearExpert(torch.nn.Module): + """Nonlinear expert. + + Class for a simple nonlinear neural network expert, where the hidden layer dimension is the mean of the input and + output dimensions. + + Attributes: + name: A string with the name of the expert. + input_dimension: An integer defining the input dimension of the expert. + hidden_dimension: An integer defining the dimension of the hidden layer. + output_dimension: An integer defining the output dimension of the expert. + linear1: The first linear neural network layer. + activation: The activation to get the hidden layer. + linear2: The second linear neural network layer. + """ + + def __init__(self, index: int, input_dimension: int, hidden_dimension: int, output_dimension: int) -> None: + """Initializes the _NonlinearExpert class. + + Initializes the _NonlinearExpert class by setting fixed and variable attributes. + + Args: + index: An integer with the index of the expert. + input_dimension: An integer defining the input dimension of the expert. + hidden_dimension: An integer defining the dimension of the hidden layer. + output_dimension: An integer defining the output dimension of the expert. + """ + super().__init__() + self.name = "NonlinearExpert" + str(index) + self.input_dimension = input_dimension + self.hidden_dimension = hidden_dimension + self.output_dimension = output_dimension + self.linear1 = torch.nn.Linear(self.input_dimension, self.hidden_dimension) + self.activation = torch.nn.Tanh() + self.linear2 = torch.nn.Linear(self.hidden_dimension, self.output_dimension) + + def forward(self, input: torch.Tensor) -> torch.Tensor: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + input: A (B,I) tensor with the inputs. + + Returns: + output: A (B,O) tensor with the outputs. + """ + return self.linear2(self.activation(self.linear1(input))) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py new file mode 100644 index 000000000..f2f9f95e1 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/python/ocs2_mpcnet_core/policy/nonlinear.py @@ -0,0 +1,89 @@ +############################################################################### +# Copyright (c) 2022, Farbod Farshidian. All rights reserved. +# +# 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. +# +# * Neither the name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# 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. +############################################################################### + +"""Nonlinear policy. + +Provides a class that implements a nonlinear policy. +""" + +import torch +from typing import Tuple + +from ocs2_mpcnet_core.config import Config +from ocs2_mpcnet_core.policy.base import BasePolicy + + +class NonlinearPolicy(BasePolicy): + """Nonlinear policy. + + Class for a simple nonlinear neural network policy, where the hidden layer dimension is the mean of the input and + output dimensions. + + Attributes: + name: A string with the name of the policy. + observation_dimension: An integer defining the observation (i.e. input) dimension of the policy. + hidden_dimension: An integer defining the dimension of the hidden layer. + action_dimension: An integer defining the action (i.e. output) dimension of the policy. + linear1: The first linear neural network layer. + activation: The activation to get the hidden layer. + linear2: The second linear neural network layer. + """ + + def __init__(self, config: Config) -> None: + """Initializes the NonlinearPolicy class. + + Initializes the NonlinearPolicy class by setting fixed and variable attributes. + + Args: + config: An instance of the configuration class. + """ + super().__init__(config) + self.name = "NonlinearPolicy" + self.observation_dimension = config.OBSERVATION_DIM + self.hidden_dimension = int((config.OBSERVATION_DIM + config.ACTION_DIM) / 2) + self.action_dimension = config.ACTION_DIM + self.linear1 = torch.nn.Linear(self.observation_dimension, self.hidden_dimension) + self.activation = torch.nn.Tanh() + self.linear2 = torch.nn.Linear(self.hidden_dimension, self.action_dimension) + + def forward(self, observation: torch.Tensor) -> Tuple[torch.Tensor]: + """Forward method. + + Defines the computation performed at every call. Computes the output tensors from the input tensors. + + Args: + observation: A (B,O) tensor with the observations. + + Returns: + action: A (B,A) tensor with the predicted actions. + """ + scaled_observation = self.scale_observation(observation) + unscaled_action = self.linear2(self.activation(self.linear1(scaled_observation))) + action = self.scale_action(unscaled_action) + return (action,) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt b/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt new file mode 100644 index 000000000..243bf94a5 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/requirements.txt @@ -0,0 +1,14 @@ +# +####### requirements.txt ####### +# +###### Requirements without version specifiers ###### +black +numpy +pyyaml +tensorboard +torch +# +###### Requirements with version specifiers ###### +# +###### Refer to other requirements files ###### +# \ No newline at end of file diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/setup.py b/ocs2_mpcnet/ocs2_mpcnet_core/setup.py new file mode 100644 index 000000000..61478769f --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/setup.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python + +from setuptools import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup(packages=["ocs2_mpcnet_core"], package_dir={"": "python"}) + +setup(**setup_args) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp new file mode 100644 index 000000000..e02f861d9 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetInterfaceBase.cpp @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/MpcnetInterfaceBase.h" + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetInterfaceBase::startDataGeneration(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, + size_t nSamples, const matrix_t& samplingCovariance, + const std::vector& initialObservations, + const std::vector& modeSchedules, + const std::vector& targetTrajectories) { + mpcnetRolloutManagerPtr_->startDataGeneration(alpha, policyFilePath, timeStep, dataDecimation, nSamples, samplingCovariance, + initialObservations, modeSchedules, targetTrajectories); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetInterfaceBase::isDataGenerationDone() { + return mpcnetRolloutManagerPtr_->isDataGenerationDone(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +data_array_t MpcnetInterfaceBase::getGeneratedData() { + return mpcnetRolloutManagerPtr_->getGeneratedData(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetInterfaceBase::startPolicyEvaluation(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, + const std::vector& initialObservations, + const std::vector& modeSchedules, + const std::vector& targetTrajectories) { + mpcnetRolloutManagerPtr_->startPolicyEvaluation(alpha, policyFilePath, timeStep, initialObservations, modeSchedules, targetTrajectories); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetInterfaceBase::isPolicyEvaluationDone() { + return mpcnetRolloutManagerPtr_->isPolicyEvaluationDone(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +metrics_array_t MpcnetInterfaceBase::getComputedMetrics() { + return mpcnetRolloutManagerPtr_->getComputedMetrics(); +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp new file mode 100644 index 000000000..38fec2cf0 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/MpcnetPybindings.cpp @@ -0,0 +1,34 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/MpcnetPybindMacros.h" + +#include "ocs2_mpcnet_core/MpcnetInterfaceBase.h" + +CREATE_MPCNET_PYTHON_BINDINGS(MpcnetPybindings) diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp new file mode 100644 index 000000000..3c3ef8437 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetBehavioralController.cpp @@ -0,0 +1,110 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/control/MpcnetBehavioralController.h" + +#include + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t MpcnetBehavioralController::computeInput(scalar_t t, const vector_t& x) { + if (optimalControllerPtr_ != nullptr && learnedControllerPtr_ != nullptr) { + if (numerics::almost_eq(alpha_, 0.0)) { + return learnedControllerPtr_->computeInput(t, x); + } else if (numerics::almost_eq(alpha_, 1.0)) { + return optimalControllerPtr_->computeInput(t, x); + } else { + return alpha_ * optimalControllerPtr_->computeInput(t, x) + (1 - alpha_) * learnedControllerPtr_->computeInput(t, x); + } + } else { + throw std::runtime_error( + "[MpcnetBehavioralController::computeInput] cannot compute input, since optimal and/or learned controller not set."); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +int MpcnetBehavioralController::size() const { + if (optimalControllerPtr_ != nullptr && learnedControllerPtr_ != nullptr) { + return std::max(optimalControllerPtr_->size(), learnedControllerPtr_->size()); + } else if (optimalControllerPtr_ != nullptr) { + return optimalControllerPtr_->size(); + } else if (learnedControllerPtr_ != nullptr) { + return learnedControllerPtr_->size(); + } else { + return 0; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetBehavioralController::clear() { + if (optimalControllerPtr_ != nullptr) { + optimalControllerPtr_->clear(); + } + if (learnedControllerPtr_ != nullptr) { + learnedControllerPtr_->clear(); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetBehavioralController::empty() const { + if (optimalControllerPtr_ != nullptr && learnedControllerPtr_ != nullptr) { + return optimalControllerPtr_->empty() && learnedControllerPtr_->empty(); + } else if (optimalControllerPtr_ != nullptr) { + return optimalControllerPtr_->empty(); + } else if (learnedControllerPtr_ != nullptr) { + return learnedControllerPtr_->empty(); + } else { + return true; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetBehavioralController::concatenate(const ControllerBase* otherController, int index, int length) { + if (optimalControllerPtr_ != nullptr) { + optimalControllerPtr_->concatenate(otherController, index, length); + } + if (learnedControllerPtr_ != nullptr) { + learnedControllerPtr_->concatenate(otherController, index, length); + } +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp new file mode 100644 index 000000000..b77c73d2b --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/control/MpcnetOnnxController.cpp @@ -0,0 +1,89 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/control/MpcnetOnnxController.h" + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetOnnxController::loadPolicyModel(const std::string& policyFilePath) { + policyFilePath_ = policyFilePath; + // create session + Ort::SessionOptions sessionOptions; + sessionOptions.SetIntraOpNumThreads(1); + sessionOptions.SetInterOpNumThreads(1); + sessionPtr_.reset(new Ort::Session(*onnxEnvironmentPtr_, policyFilePath_.c_str(), sessionOptions)); + // get input and output info + inputNames_.clear(); + outputNames_.clear(); + inputShapes_.clear(); + outputShapes_.clear(); + Ort::AllocatorWithDefaultOptions allocator; + for (int i = 0; i < sessionPtr_->GetInputCount(); i++) { + inputNames_.push_back(sessionPtr_->GetInputName(i, allocator)); + inputShapes_.push_back(sessionPtr_->GetInputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); + } + for (int i = 0; i < sessionPtr_->GetOutputCount(); i++) { + outputNames_.push_back(sessionPtr_->GetOutputName(i, allocator)); + outputShapes_.push_back(sessionPtr_->GetOutputTypeInfo(i).GetTensorTypeAndShapeInfo().GetShape()); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t MpcnetOnnxController::computeInput(const scalar_t t, const vector_t& x) { + if (sessionPtr_ == nullptr) { + throw std::runtime_error("[MpcnetOnnxController::computeInput] cannot compute input, since policy model is not loaded."); + } + // create input tensor object + Eigen::Matrix observation = + mpcnetDefinitionPtr_->getObservation(t, x, referenceManagerPtr_->getModeSchedule(), referenceManagerPtr_->getTargetTrajectories()) + .cast(); + Ort::MemoryInfo memoryInfo = Ort::MemoryInfo::CreateCpu(OrtAllocatorType::OrtArenaAllocator, OrtMemType::OrtMemTypeDefault); + std::vector inputValues; + inputValues.push_back(Ort::Value::CreateTensor(memoryInfo, observation.data(), observation.size(), + inputShapes_[0].data(), inputShapes_[0].size())); + // run inference + Ort::RunOptions runOptions; + std::vector outputValues = sessionPtr_->Run(runOptions, inputNames_.data(), inputValues.data(), 1, outputNames_.data(), 1); + // evaluate output tensor object + Eigen::Map> action(outputValues[0].GetTensorMutableData(), + outputShapes_[0][1], outputShapes_[0][0]); + std::pair actionTransformation = mpcnetDefinitionPtr_->getActionTransformation( + t, x, referenceManagerPtr_->getModeSchedule(), referenceManagerPtr_->getTargetTrajectories()); + // transform action + return actionTransformation.first * action.cast() + actionTransformation.second; +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp new file mode 100644 index 000000000..80e138143 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyLoopRos.cpp @@ -0,0 +1,151 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/dummy/MpcnetDummyLoopRos.h" + +#include + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetDummyLoopRos::MpcnetDummyLoopRos(scalar_t controlFrequency, scalar_t rosFrequency, std::unique_ptr mpcnetPtr, + std::unique_ptr rolloutPtr, std::shared_ptr rosReferenceManagerPtr) + : controlFrequency_(controlFrequency), + rosFrequency_(rosFrequency), + mpcnetPtr_(std::move(mpcnetPtr)), + rolloutPtr_(std::move(rolloutPtr)), + rosReferenceManagerPtr_(std::move(rosReferenceManagerPtr)) {} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::run(const SystemObservation& systemObservation, const TargetTrajectories& targetTrajectories) { + ros::WallRate rosRate(rosFrequency_); + scalar_t duration = 1.0 / rosFrequency_; + + // initialize + SystemObservation initialSystemObservation = systemObservation; + SystemObservation finalSystemObservation = systemObservation; + rosReferenceManagerPtr_->setTargetTrajectories(targetTrajectories); + + // start of while loop + while (::ros::ok() && ::ros::master::check()) { + // update system observation + swap(initialSystemObservation, finalSystemObservation); + + // update reference manager and synchronized modules + preSolverRun(initialSystemObservation.time, initialSystemObservation.state); + + // rollout + rollout(duration, initialSystemObservation, finalSystemObservation); + + // update observers + PrimalSolution primalSolution; + primalSolution.timeTrajectory_ = {finalSystemObservation.time}; + primalSolution.stateTrajectory_ = {finalSystemObservation.state}; + primalSolution.inputTrajectory_ = {finalSystemObservation.input}; + primalSolution.modeSchedule_ = rosReferenceManagerPtr_->getModeSchedule(); + primalSolution.controllerPtr_ = std::unique_ptr(mpcnetPtr_->clone()); + CommandData commandData; + commandData.mpcInitObservation_ = initialSystemObservation; + commandData.mpcTargetTrajectories_ = rosReferenceManagerPtr_->getTargetTrajectories(); + for (auto& observer : observerPtrs_) { + observer->update(finalSystemObservation, primalSolution, commandData); + } + + // process callbacks and sleep + ::ros::spinOnce(); + rosRate.sleep(); + } // end of while loop +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::addObserver(std::shared_ptr observer) { + observerPtrs_.push_back(std::move(observer)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::addSynchronizedModule(std::shared_ptr synchronizedModule) { + synchronizedModulePtrs_.push_back(std::move(synchronizedModule)); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::rollout(scalar_t duration, const SystemObservation& initialSystemObservation, + SystemObservation& finalSystemObservation) { + scalar_t timeStep = 1.0 / controlFrequency_; + + // initial time, state and input + scalar_t time = initialSystemObservation.time; + vector_t state = initialSystemObservation.state; + vector_t input = initialSystemObservation.input; + + // start of while loop + while (time <= initialSystemObservation.time + duration) { + // forward simulate system + ModeSchedule modeSchedule = rosReferenceManagerPtr_->getModeSchedule(); + scalar_array_t timeTrajectory; + size_array_t postEventIndicesStock; + vector_array_t stateTrajectory; + vector_array_t inputTrajectory; + rolloutPtr_->run(time, state, time + timeStep, mpcnetPtr_.get(), modeSchedule, timeTrajectory, postEventIndicesStock, stateTrajectory, + inputTrajectory); + + // update time, state and input + time = timeTrajectory.back(); + state = stateTrajectory.back(); + input = inputTrajectory.back(); + } // end of while loop + + // final time, state and input + finalSystemObservation.time = time; + finalSystemObservation.state = state; + finalSystemObservation.input = input; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyLoopRos::preSolverRun(scalar_t time, const vector_t& state) { + rosReferenceManagerPtr_->preSolverRun(time, time + scalar_t(1.0), state); + for (auto& module : synchronizedModulePtrs_) { + module->preSolverRun(time, time + scalar_t(1.0), state, *rosReferenceManagerPtr_); + } +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp new file mode 100644 index 000000000..6005b62c9 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/dummy/MpcnetDummyObserverRos.cpp @@ -0,0 +1,56 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/dummy/MpcnetDummyObserverRos.h" + +#include + +#include + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetDummyObserverRos::MpcnetDummyObserverRos(ros::NodeHandle& nodeHandle, std::string topicPrefix) { + observationPublisher_ = nodeHandle.advertise(topicPrefix + "_mpc_observation", 1); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetDummyObserverRos::update(const SystemObservation& observation, const PrimalSolution& primalSolution, + const CommandData& command) { + auto observationMsg = ros_msg_conversions::createObservationMsg(observation); + observationPublisher_.publish(observationMsg); +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp new file mode 100644 index 000000000..0cfc884eb --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetDataGeneration.cpp @@ -0,0 +1,94 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/rollout/MpcnetDataGeneration.h" + +#include + +#include "ocs2_mpcnet_core/control/MpcnetBehavioralController.h" + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +const data_array_t* MpcnetDataGeneration::run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, + size_t nSamples, const matrix_t& samplingCovariance, + const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + // clear data array + dataArray_.clear(); + + // set system + set(alpha, policyFilePath, initialObservation, modeSchedule, targetTrajectories); + + // set up scalar standard normal generator and compute Cholesky decomposition of covariance matrix + std::random_device randomDevice; + std::default_random_engine pseudoRandomNumberGenerator(randomDevice()); + std::normal_distribution standardNormalDistribution(scalar_t(0.0), scalar_t(1.0)); + auto standardNormalNullaryOp = [&](scalar_t) -> scalar_t { return standardNormalDistribution(pseudoRandomNumberGenerator); }; + const matrix_t L = samplingCovariance.llt().matrixL(); + + // run data generation + int iteration = 0; + try { + while (systemObservation_.time <= targetTrajectories.timeTrajectory.back()) { + // step system + step(timeStep); + + // downsample the data signal by an integer factor + if (iteration % dataDecimation == 0) { + // get nominal data point + const vector_t deviation = vector_t::Zero(primalSolution_.stateTrajectory_.front().size()); + dataArray_.push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); + + // get samples around nominal data point + for (int i = 0; i < nSamples; i++) { + const vector_t deviation = L * vector_t::NullaryExpr(primalSolution_.stateTrajectory_.front().size(), standardNormalNullaryOp); + dataArray_.push_back(getDataPoint(*mpcPtr_, *mpcnetDefinitionPtr_, deviation)); + } + } + + // update iteration + ++iteration; + } + } catch (const std::exception& e) { + // print error for exceptions + std::cerr << "[MpcnetDataGeneration::run] a standard exception was caught, with message: " << e.what() << "\n"; + // this data generation run failed, clear data + dataArray_.clear(); + } + + // return pointer to the data array + return &dataArray_; +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp new file mode 100644 index 000000000..d1847fc04 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetPolicyEvaluation.cpp @@ -0,0 +1,74 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/rollout/MpcnetPolicyEvaluation.h" + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +metrics_t MpcnetPolicyEvaluation::run(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, + const SystemObservation& initialObservation, const ModeSchedule& modeSchedule, + const TargetTrajectories& targetTrajectories) { + // declare metrics + metrics_t metrics; + + // set system + set(alpha, policyFilePath, initialObservation, modeSchedule, targetTrajectories); + + // run policy evaluation + try { + while (systemObservation_.time <= targetTrajectories.timeTrajectory.back()) { + // step system + step(timeStep); + + // incurred quantities + const scalar_t time = primalSolution_.timeTrajectory_.front(); + const vector_t state = primalSolution_.stateTrajectory_.front(); + const vector_t input = behavioralControllerPtr_->computeInput(time, state); + metrics.incurredHamiltonian += mpcPtr_->getSolverPtr()->getHamiltonian(time, state, input).f * timeStep; + } + } catch (const std::exception& e) { + // print error for exceptions + std::cerr << "[MpcnetPolicyEvaluation::run] a standard exception was caught, with message: " << e.what() << "\n"; + // this policy evaluation run failed, incurred quantities are not reported + metrics.incurredHamiltonian = std::numeric_limits::quiet_NaN(); + } + + // report survival time + metrics.survivalTime = systemObservation_.time; + + // return metrics + return metrics; +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp new file mode 100644 index 000000000..8037e5541 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutBase.cpp @@ -0,0 +1,101 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/rollout/MpcnetRolloutBase.h" + +#include "ocs2_mpcnet_core/control/MpcnetBehavioralController.h" + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetRolloutBase::set(scalar_t alpha, const std::string& policyFilePath, const SystemObservation& initialObservation, + const ModeSchedule& modeSchedule, const TargetTrajectories& targetTrajectories) { + // init system observation + systemObservation_ = initialObservation; + + // reset mpc + mpcPtr_->reset(); + + // prepare learned controller + mpcnetPtr_->loadPolicyModel(policyFilePath); + + // reset rollout, i.e. reset the internal simulator state (e.g. relevant for RaiSim) + rolloutPtr_->resetRollout(); + + // update the reference manager + referenceManagerPtr_->setModeSchedule(modeSchedule); + referenceManagerPtr_->setTargetTrajectories(targetTrajectories); + + // set up behavioral controller with mixture parameter alpha and learned controller + behavioralControllerPtr_->setAlpha(alpha); + behavioralControllerPtr_->setLearnedController(*mpcnetPtr_); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetRolloutBase::step(scalar_t timeStep) { + // run mpc + if (!mpcPtr_->run(systemObservation_.time, systemObservation_.state)) { + throw std::runtime_error("[MpcnetRolloutBase::step] main routine of MPC returned false."); + } + + // update primal solution + primalSolution_ = mpcPtr_->getSolverPtr()->primalSolution(mpcPtr_->getSolverPtr()->getFinalTime()); + + // update behavioral controller with MPC controller + behavioralControllerPtr_->setOptimalController(*primalSolution_.controllerPtr_); + + // forward simulate system with behavioral controller + scalar_array_t timeTrajectory; + size_array_t postEventIndicesStock; + vector_array_t stateTrajectory; + vector_array_t inputTrajectory; + rolloutPtr_->run(primalSolution_.timeTrajectory_.front(), primalSolution_.stateTrajectory_.front(), + primalSolution_.timeTrajectory_.front() + timeStep, behavioralControllerPtr_.get(), primalSolution_.modeSchedule_, + timeTrajectory, postEventIndicesStock, stateTrajectory, inputTrajectory); + + // update system observation + systemObservation_.time = timeTrajectory.back(); + systemObservation_.state = stateTrajectory.back(); + systemObservation_.input = inputTrajectory.back(); + systemObservation_.mode = primalSolution_.modeSchedule_.modeAtTime(systemObservation_.time); + + // check forward simulated system + if (!mpcnetDefinitionPtr_->isValid(systemObservation_.time, systemObservation_.state, referenceManagerPtr_->getModeSchedule(), + referenceManagerPtr_->getTargetTrajectories())) { + throw std::runtime_error("MpcnetDataGeneration::run Tuple (time, state, modeSchedule, targetTrajectories) is not valid."); + } +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp new file mode 100644 index 000000000..7c170fb33 --- /dev/null +++ b/ocs2_mpcnet/ocs2_mpcnet_core/src/rollout/MpcnetRolloutManager.cpp @@ -0,0 +1,243 @@ +/****************************************************************************** +Copyright (c) 2022, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_mpcnet_core/rollout/MpcnetRolloutManager.h" + +namespace ocs2 { +namespace mpcnet { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +MpcnetRolloutManager::MpcnetRolloutManager(size_t nDataGenerationThreads, size_t nPolicyEvaluationThreads, + std::vector> mpcPtrs, + std::vector> mpcnetPtrs, + std::vector> rolloutPtrs, + std::vector> mpcnetDefinitionPtrs, + std::vector> referenceManagerPtrs) { + // data generation + nDataGenerationThreads_ = nDataGenerationThreads; + if (nDataGenerationThreads_ > 0) { + dataGenerationThreadPoolPtr_.reset(new ThreadPool(nDataGenerationThreads_)); + dataGenerationPtrs_.reserve(nDataGenerationThreads); + for (int i = 0; i < nDataGenerationThreads; i++) { + dataGenerationPtrs_.push_back( + std::make_unique(std::move(mpcPtrs.at(i)), std::move(mpcnetPtrs.at(i)), std::move(rolloutPtrs.at(i)), + std::move(mpcnetDefinitionPtrs.at(i)), referenceManagerPtrs.at(i))); + } + } + + // policy evaluation + nPolicyEvaluationThreads_ = nPolicyEvaluationThreads; + if (nPolicyEvaluationThreads_ > 0) { + policyEvaluationThreadPoolPtr_.reset(new ThreadPool(nPolicyEvaluationThreads_)); + policyEvaluationPtrs_.reserve(nPolicyEvaluationThreads_); + for (int i = nDataGenerationThreads_; i < (nDataGenerationThreads_ + nPolicyEvaluationThreads_); i++) { + policyEvaluationPtrs_.push_back( + std::make_unique(std::move(mpcPtrs.at(i)), std::move(mpcnetPtrs.at(i)), std::move(rolloutPtrs.at(i)), + std::move(mpcnetDefinitionPtrs.at(i)), referenceManagerPtrs.at(i))); + } + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetRolloutManager::startDataGeneration(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, size_t dataDecimation, + size_t nSamples, const matrix_t& samplingCovariance, + const std::vector& initialObservations, + const std::vector& modeSchedules, + const std::vector& targetTrajectories) { + if (nDataGenerationThreads_ <= 0) { + throw std::runtime_error("[MpcnetRolloutManager::startDataGeneration] cannot work without at least one data generation thread."); + } + + // reset variables + dataGenerationFtrs_.clear(); + nDataGenerationTasksDone_ = 0; + + // push tasks into pool + for (int i = 0; i < initialObservations.size(); i++) { + dataGenerationFtrs_.push_back(dataGenerationThreadPoolPtr_->run([=](int threadNumber) { + const auto* result = + dataGenerationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, dataDecimation, nSamples, samplingCovariance, + initialObservations.at(i), modeSchedules.at(i), targetTrajectories.at(i)); + nDataGenerationTasksDone_++; + // print thread and task number + std::cerr << "Data generation thread " << threadNumber << " finished task " << nDataGenerationTasksDone_ << "\n"; + return result; + })); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetRolloutManager::isDataGenerationDone() { + if (nDataGenerationThreads_ <= 0) { + throw std::runtime_error("[MpcnetRolloutManager::isDataGenerationDone] cannot work without at least one data generation thread."); + } + if (dataGenerationFtrs_.size() <= 0) { + throw std::runtime_error( + "[MpcnetRolloutManager::isDataGenerationDone] cannot return if startDataGeneration has not been triggered once."); + } + + // check if done + if (nDataGenerationTasksDone_ < dataGenerationFtrs_.size()) { + return false; + } else if (nDataGenerationTasksDone_ == dataGenerationFtrs_.size()) { + return true; + } else { + throw std::runtime_error("[MpcnetRolloutManager::isDataGenerationDone] error since more tasks done than futures available."); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +const data_array_t& MpcnetRolloutManager::getGeneratedData() { + if (nDataGenerationThreads_ <= 0) { + throw std::runtime_error("[MpcnetRolloutManager::getGeneratedData] cannot work without at least one data generation thread."); + } + if (!isDataGenerationDone()) { + throw std::runtime_error("[MpcnetRolloutManager::getGeneratedData] cannot get data when data generation is not done."); + } + + // clear data array + dataArray_.clear(); + + // get pointers to data + std::vector dataPtrs; + dataPtrs.reserve(dataGenerationFtrs_.size()); + for (auto& dataGenerationFtr : dataGenerationFtrs_) { + try { + // get results from futures of the tasks + dataPtrs.push_back(dataGenerationFtr.get()); + } catch (const std::exception& e) { + // print error for exceptions + std::cerr << "[MpcnetRolloutManager::getGeneratedData] a standard exception was caught, with message: " << e.what() << "\n"; + } + } + + // find number of data points + int nDataPoints = 0; + for (int i = 0; i < dataPtrs.size(); i++) { + nDataPoints += dataPtrs[i]->size(); + } + + // fill data array + dataArray_.reserve(nDataPoints); + for (const auto dataPtr : dataPtrs) { + dataArray_.insert(dataArray_.end(), dataPtr->begin(), dataPtr->end()); + } + + // return data array + return dataArray_; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void MpcnetRolloutManager::startPolicyEvaluation(scalar_t alpha, const std::string& policyFilePath, scalar_t timeStep, + const std::vector& initialObservations, + const std::vector& modeSchedules, + const std::vector& targetTrajectories) { + if (nPolicyEvaluationThreads_ <= 0) { + throw std::runtime_error("[MpcnetRolloutManager::startPolicyEvaluation] cannot work without at least one policy evaluation thread."); + } + + // reset variables + policyEvaluationFtrs_.clear(); + nPolicyEvaluationTasksDone_ = 0; + + // push tasks into pool + for (int i = 0; i < initialObservations.size(); i++) { + policyEvaluationFtrs_.push_back(policyEvaluationThreadPoolPtr_->run([=](int threadNumber) { + const auto result = policyEvaluationPtrs_[threadNumber]->run(alpha, policyFilePath, timeStep, initialObservations.at(i), + modeSchedules.at(i), targetTrajectories.at(i)); + nPolicyEvaluationTasksDone_++; + // print thread and task number + std::cerr << "Policy evaluation thread " << threadNumber << " finished task " << nPolicyEvaluationTasksDone_ << "\n"; + return result; + })); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool MpcnetRolloutManager::isPolicyEvaluationDone() { + if (nPolicyEvaluationThreads_ <= 0) { + throw std::runtime_error("[MpcnetRolloutManager::isPolicyEvaluationDone] cannot work without at least one policy evaluation thread."); + } + if (policyEvaluationFtrs_.size() <= 0) { + throw std::runtime_error( + "[MpcnetRolloutManager::isPolicyEvaluationDone] cannot return if startPolicyEvaluation has not been triggered once."); + } + + // check if done + if (nPolicyEvaluationTasksDone_ < policyEvaluationFtrs_.size()) { + return false; + } else if (nPolicyEvaluationTasksDone_ == policyEvaluationFtrs_.size()) { + return true; + } else { + throw std::runtime_error("[MpcnetRolloutManager::isPolicyEvaluationDone] error since more tasks done than futures available."); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +metrics_array_t MpcnetRolloutManager::getComputedMetrics() { + if (nPolicyEvaluationThreads_ <= 0) { + throw std::runtime_error("[MpcnetRolloutManager::getComputedMetrics] cannot work without at least one policy evaluation thread."); + } + if (!isPolicyEvaluationDone()) { + throw std::runtime_error("[MpcnetRolloutManager::getComputedMetrics] cannot get metrics when policy evaluation is not done."); + } + + // get metrics and fill metrics array + metrics_array_t metricsArray; + metricsArray.reserve(policyEvaluationFtrs_.size()); + for (auto& policyEvaluationFtr : policyEvaluationFtrs_) { + try { + // get results from futures of the tasks + metricsArray.push_back(policyEvaluationFtr.get()); + } catch (const std::exception& e) { + // print error for exceptions + std::cerr << "[MpcnetRolloutManager::getComputedMetrics] a standard exception was caught, with message: " << e.what() << "\n"; + } + } + + // return metrics array + return metricsArray; +} + +} // namespace mpcnet +} // namespace ocs2 diff --git a/ocs2_msgs/CMakeLists.txt b/ocs2_msgs/CMakeLists.txt index d36f92f81..4aed95b53 100644 --- a/ocs2_msgs/CMakeLists.txt +++ b/ocs2_msgs/CMakeLists.txt @@ -19,6 +19,7 @@ add_message_files( mpc_flattened_controller.msg lagrangian_metrics.msg multiplier.msg + constraint.msg ) add_service_files( diff --git a/ocs2_msgs/msg/constraint.msg b/ocs2_msgs/msg/constraint.msg new file mode 100644 index 000000000..fe48dd804 --- /dev/null +++ b/ocs2_msgs/msg/constraint.msg @@ -0,0 +1,4 @@ +# MPC constraint + +float32 time +float32[] value \ No newline at end of file diff --git a/ocs2_oc/CMakeLists.txt b/ocs2_oc/CMakeLists.txt index db09a027a..4ba703f79 100644 --- a/ocs2_oc/CMakeLists.txt +++ b/ocs2_oc/CMakeLists.txt @@ -49,12 +49,23 @@ include_directories( add_library(${PROJECT_NAME} src/approximate_model/ChangeOfInputVariables.cpp src/approximate_model/LinearQuadraticApproximator.cpp + src/multiple_shooting/Helpers.cpp + src/multiple_shooting/Initialization.cpp + src/multiple_shooting/LagrangianEvaluation.cpp + src/multiple_shooting/MetricsComputation.cpp + src/multiple_shooting/PerformanceIndexComputation.cpp + src/multiple_shooting/ProjectionMultiplierCoefficients.cpp + src/multiple_shooting/Transcription.cpp src/oc_data/LoopshapingPrimalSolution.cpp + src/oc_data/PerformanceIndex.cpp + src/oc_data/TimeDiscretization.cpp src/oc_problem/OptimalControlProblem.cpp src/oc_problem/LoopshapingOptimalControlProblem.cpp src/oc_problem/OptimalControlProblemHelperFunction.cpp + src/oc_problem/OcpSize.cpp + src/oc_problem/OcpToKkt.cpp src/oc_solver/SolverBase.cpp - src/oc_problem/OptimalControlProblem.cpp + src/precondition/Ruzi.cpp src/rollout/PerformanceIndicesRollout.cpp src/rollout/RolloutBase.cpp src/rollout/RootFinder.cpp @@ -65,7 +76,8 @@ add_library(${PROJECT_NAME} src/synchronized_module/ReferenceManager.cpp src/synchronized_module/LoopshapingReferenceManager.cpp src/synchronized_module/LoopshapingSynchronizedModule.cpp - src/synchronized_module/AugmentedLagrangianObserver.cpp + src/synchronized_module/SolverObserver.cpp + src/search_strategy/FilterLinesearch.cpp src/trajectory_adjustment/TrajectorySpreading.cpp ) target_link_libraries(${PROJECT_NAME} @@ -73,7 +85,6 @@ target_link_libraries(${PROJECT_NAME} ) target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) - add_executable(${PROJECT_NAME}_lintTarget src/lintTarget.cpp ) @@ -102,11 +113,9 @@ install( LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) - install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) - install(DIRECTORY test/include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) @@ -121,42 +130,83 @@ install(DIRECTORY test/include/${PROJECT_NAME}/ ## to see the summary of unit test results run ## $ catkin_test_results ../../../build/ocs2_oc -catkin_add_gtest(test_time_triggered_rollout - test/rollout/testTimeTriggeredRollout.cpp +catkin_add_gtest(test_${PROJECT_NAME}_multiple_shooting + test/multiple_shooting/testProjectionMultiplierCoefficients.cpp + test/multiple_shooting/testTranscriptionMetrics.cpp + test/multiple_shooting/testTranscriptionPerformanceIndex.cpp +) +add_dependencies(test_${PROJECT_NAME}_multiple_shooting + ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(test_time_triggered_rollout +target_link_libraries(test_${PROJECT_NAME}_multiple_shooting ${PROJECT_NAME} ${catkin_LIBRARIES} - ${Boost_LIBRARIES} gtest_main ) -catkin_add_gtest(test_state_triggered_rollout - test/rollout/testStateTriggeredRollout.cpp +catkin_add_gtest(test_${PROJECT_NAME}_data + test/oc_data/testTimeDiscretization.cpp +) +add_dependencies(test_${PROJECT_NAME}_data + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(test_${PROJECT_NAME}_data + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main +) + +catkin_add_gtest(test_${PROJECT_NAME}_rollout + test/rollout/testTimeTriggeredRollout.cpp + test/rollout/testStateTriggeredRollout.cpp +) +add_dependencies(test_${PROJECT_NAME}_rollout + ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(test_state_triggered_rollout +target_link_libraries(test_${PROJECT_NAME}_rollout ${PROJECT_NAME} ${catkin_LIBRARIES} - ${Boost_LIBRARIES} gtest_main ) catkin_add_gtest(test_change_of_variables test/testChangeOfInputVariables.cpp ) +add_dependencies(test_change_of_variables + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(test_change_of_variables ${PROJECT_NAME} ${catkin_LIBRARIES} - ${Boost_LIBRARIES} gtest_main ) catkin_add_gtest(test_trajectory_spreading test/trajectory_adjustment/TrajectorySpreadingTest.cpp ) +add_dependencies(test_trajectory_spreading + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(test_trajectory_spreading ${PROJECT_NAME} ${catkin_LIBRARIES} - ${Boost_LIBRARIES} + gtest_main +) + +catkin_add_gtest(test_ocp_to_kkt + test/oc_problem/testOcpToKkt.cpp +) +target_link_libraries(test_ocp_to_kkt + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main +) + +catkin_add_gtest(test_precondition + test/precondition/testPrecondition.cpp +) +target_link_libraries(test_precondition + ${PROJECT_NAME} + ${catkin_LIBRARIES} gtest_main ) diff --git a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h index 55d574eac..9c4074231 100644 --- a/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h +++ b/ocs2_oc/include/ocs2_oc/approximate_model/LinearQuadraticApproximator.h @@ -161,7 +161,23 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr const vector_t& state); /** - * Compute the intermediate-time MetricsCollection (i.e. cost, softConstraints, and constraints). + * Compute the intermediate-time Metrics (i.e. cost, softConstraints, and constraints). + * + * @note It is assumed that the precomputation request is already made. + * problem.preComputationPtr->request(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x, u) + * + * @param [in] problem: The optimal control probelm + * @param [in] time: The current time. + * @param [in] state: The current state. + * @param [in] input: The current input. + * @param [in] dynamicsViolation: The violation of dynamics. It depends on the transcription method. + * @return The output Metrics. + */ +Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, + vector_t&& dynamicsViolation = vector_t()); + +/** + * Compute the intermediate-time Metrics (i.e. cost, softConstraints, and constraints). * * @note It is assumed that the precomputation request is already made. * problem.preComputationPtr->request(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x, u) @@ -171,13 +187,29 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr * @param [in] state: The current state. * @param [in] input: The current input. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. - * @return The output MetricsCollection. + * @param [in] dynamicsViolation: The violation of dynamics. It depends on the transcription method. + * @return The output Metrics. */ -MetricsCollection computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const vector_t& input, const MultiplierCollection& multipliers); +Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, + const MultiplierCollection& multipliers, vector_t&& dynamicsViolation = vector_t()); /** - * Compute the event-time MetricsCollection based on pre-jump state value (i.e. cost, softConstraints, and constraints). + * Compute the event-time Metrics based on pre-jump state value (i.e. cost, softConstraints, and constraints). + * + * @note It is assumed that the precomputation request is already made. + * problem.preComputationPtr->requestPreJump(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x) + * + * @param [in] problem: The optimal control probelm + * @param [in] time: The current time. + * @param [in] state: The current state. + * @param [in] dynamicsViolation: The violation of dynamics. It depends on the transcription method. + * @return The output Metrics. + */ +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + vector_t&& dynamicsViolation = vector_t()); + +/** + * Compute the event-time Metrics based on pre-jump state value (i.e. cost, softConstraints, and constraints). * * @note It is assumed that the precomputation request is already made. * problem.preComputationPtr->requestPreJump(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x) @@ -186,13 +218,27 @@ MetricsCollection computeIntermediateMetrics(OptimalControlProblem& problem, con * @param [in] time: The current time. * @param [in] state: The current state. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. - * @return The output MetricsCollection. + * @param [in] dynamicsViolation: The violation of dynamics. It depends on the transcription method. + * @return The output Metrics. + */ +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers, vector_t&& dynamicsViolation = vector_t()); + +/** + * Compute the final-time Metrics (i.e. cost, softConstraints, and constraints). + * + * @note It is assumed that the precomputation request is already made. + * problem.preComputationPtr->requestFinal(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x) + * + * @param [in] problem: The optimal control probelm + * @param [in] time: The current time. + * @param [in] state: The current state. + * @return The output Metrics. */ -MetricsCollection computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers); +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state); /** - * Compute the final-time MetricsCollection (i.e. cost, softConstraints, and constraints). + * Compute the final-time Metrics (i.e. cost, softConstraints, and constraints). * * @note It is assumed that the precomputation request is already made. * problem.preComputationPtr->requestFinal(Request::Cost + Request::Constraint + Request::SoftConstraint, t, x) @@ -201,9 +247,9 @@ MetricsCollection computePreJumpMetrics(OptimalControlProblem& problem, const sc * @param [in] time: The current time. * @param [in] state: The current state. * @param [in] multipliers: The current multipliers associated to the equality and inequality Lagrangians. - * @return The output MetricsCollection. + * @return The output Metrics. */ -MetricsCollection computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers); +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers); } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h new file mode 100644 index 000000000..529b198f7 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Helpers.h @@ -0,0 +1,115 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include "ocs2_oc/oc_data/PerformanceIndex.h" +#include "ocs2_oc/oc_data/PrimalSolution.h" +#include "ocs2_oc/oc_data/ProblemMetrics.h" +#include "ocs2_oc/oc_data/TimeDiscretization.h" + +namespace ocs2 { +namespace multiple_shooting { + +/** Compute 2-norm of the trajectory: sqrt(sum_i v[i]^2) */ +inline scalar_t trajectoryNorm(const vector_array_t& v) { + scalar_t norm = 0.0; + for (const auto& vi : v) { + norm += vi.squaredNorm(); + } + return std::sqrt(norm); +} + +/** Increment the given trajectory as: vNew[i] = v[i] + alpha * dv[i]. It assumes that vNew is already resized to size of v. */ +template +void incrementTrajectory(const std::vector& v, const std::vector& dv, const scalar_t alpha, std::vector& vNew) { + assert(v.size() == dv.size()); + if (v.size() != vNew.size()) { + throw std::runtime_error("[incrementTrajectory] Resize vNew to the size of v!"); + } + + for (int i = 0; i < v.size(); i++) { + if (dv[i].size() > 0) { // account for absence of inputs at events. + vNew[i] = v[i] + alpha * dv[i]; + } else { + vNew[i] = Type(); + } + } +} + +/** + * Re-map the projected input back to the original space. + * + * @param [in] constraintsProjection: The constraints projection. + * @param [in] deltaXSol: The state trajectory of the QP subproblem solution. + * @param [in, out] deltaUSol: The input trajectory of the QP subproblem solution. + */ +void remapProjectedInput(const std::vector& constraintsProjection, const vector_array_t& deltaXSol, + vector_array_t& deltaUSol); + +void remapProjectedGain(const std::vector& constraintsProjection, matrix_array_t& KMatrices); + +/** + * Constructs a primal solution (with a feedforward controller) based the LQ subproblem solution. + * + * @param [in] time : The annotated time trajectory + * @param [in] modeSchedule: The mode schedule. + * @param [in] x: The state trajectory of the QP subproblem solution. + * @param [in] u: The input trajectory of the QP subproblem solution. + * @return The primal solution. + */ +PrimalSolution toPrimalSolution(const std::vector& time, ModeSchedule&& modeSchedule, vector_array_t&& x, + vector_array_t&& u); + +/** + * Constructs a primal solution (with a linear controller) based the LQ subproblem solution. + * + * @param [in] time : The annotated time trajectory + * @param [in] modeSchedule: The mode schedule. + * @param [in] x: The state trajectory of the QP subproblem solution. + * @param [in] u: The input trajectory of the QP subproblem solution. + * @param [in] KMatrices: The LQR gain trajectory of the QP subproblem solution. + * @return The primal solution. + */ +PrimalSolution toPrimalSolution(const std::vector& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, + matrix_array_t&& KMatrices); + +/** + * Constructs a ProblemMetrics from an array of metrics. + * + * @param [in] time : The annotated time trajectory + * @param [in] metrics: The metrics array. + * @return The ProblemMetrics. + */ +ProblemMetrics toProblemMetrics(const std::vector& time, std::vector&& metrics); + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Initialization.h similarity index 68% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h rename to ocs2_oc/include/ocs2_oc/multiple_shooting/Initialization.h index ad5137a4d..c13464e84 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingInitialization.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Initialization.h @@ -31,7 +31,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include + +#include "ocs2_oc/oc_data/PrimalSolution.h" +#include "ocs2_oc/oc_data/TimeDiscretization.h" namespace ocs2 { namespace multiple_shooting { @@ -57,10 +60,12 @@ inline std::pair initializeIntermediateNode(Initializer& ini * @param primalSolution : previous solution * @param t : Start of the discrete interval * @param tNext : End time of te discrete interval - * @param x : Starting state of the discrete interval * @return {u(t), x(tNext)} : input and state transition */ -std::pair initializeIntermediateNode(PrimalSolution& primalSolution, scalar_t t, scalar_t tNext, const vector_t& x); +inline std::pair initializeIntermediateNode(const PrimalSolution& primalSolution, scalar_t t, scalar_t tNext) { + return {LinearInterpolation::interpolate(t, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_), + LinearInterpolation::interpolate(tNext, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_)}; +} /** * Initialize the state jump at an event node. @@ -74,5 +79,20 @@ inline vector_t initializeEventNode(scalar_t t, const vector_t& x) { return x; } +/** + * Initializes for the state-input trajectories. It interpolates the primalSolution for the starting intersecting time period and then uses + * initializer for the tail. + * + * @param [in] initState : Initial state + * @param [in] timeDiscretization : The annotated time trajectory + * @param [in] primalSolution : previous solution + * @param [in] initializer : System initializer + * @param [out] stateTrajectory : The initialized state trajectory + * @param [out] inputTrajectory : The initialized input trajectory + */ +void initializeStateInputTrajectories(const vector_t& initState, const std::vector& timeDiscretization, + const PrimalSolution& primalSolution, Initializer& initializer, vector_array_t& stateTrajectory, + vector_array_t& inputTrajectory); + } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h new file mode 100644 index 000000000..27bdce4c3 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/LagrangianEvaluation.h @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +namespace ocs2 { +namespace multiple_shooting { + +/** + * Computes the SSE of the residual in the dual feasibilities. + * + * @param lagrangian : Quadratic approximation of the Lagrangian for a single node. + * @return SSE of the residual in the dual feasibilities + */ +inline scalar_t evaluateDualFeasibilities(const ScalarFunctionQuadraticApproximation& lagrangian) { + return lagrangian.dfdx.squaredNorm() + lagrangian.dfdu.squaredNorm(); +} + +/** + * Evaluates the quadratic approximation of the Lagrangian for a single intermediate node. + * + * @param lmd : Costate at start of the interval + * @param lmd_next : Costate at the end of the interval + * @param nu : Lagrange multiplier of the projection constraint. + * @param cost : Quadratic approximation of the cost. + * @param dynamics : Linear approximation of the dynamics + * @param stateInputEqConstraints : Linear approximation of the state-input equality constraints + * @return Quadratic approximation of the Lagrangian. + */ +ScalarFunctionQuadraticApproximation evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu, + ScalarFunctionQuadraticApproximation&& cost, + const VectorFunctionLinearApproximation& dynamics, + const VectorFunctionLinearApproximation& stateInputEqConstraints); + +/** + * Evaluates the quadratic approximation of the Lagrangian for the terminal node. + * + * @param lmd : Costate at start of the interval + * @param cost : Quadratic approximation of the cost. + * @return Quadratic approximation of the Lagrangian. + */ +ScalarFunctionQuadraticApproximation evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation&& cost); + +/** + * Evaluates the quadratic approximation of the Lagrangian for the event node. + * + * @param lmd : Costate at start of the interval + * @param lmd_next : Costate at the end of the the interval + * @param cost : Quadratic approximation of the cost. + * @param dynamics : Dynamics + * @return Quadratic approximation of the Lagrangian. + */ +ScalarFunctionQuadraticApproximation evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next, + ScalarFunctionQuadraticApproximation&& cost, + const VectorFunctionLinearApproximation& dynamics); + +} // namespace multiple_shooting +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h new file mode 100644 index 000000000..fe1203406 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/MetricsComputation.h @@ -0,0 +1,97 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include + +#include "ocs2_oc/multiple_shooting/Transcription.h" +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" + +namespace ocs2 { +namespace multiple_shooting { + +/** + * Compute the Metrics for a single intermediate node. + * @param transcription: multiple shooting transcription for an intermediate node. + * @return Metrics for a single intermediate node. + */ +Metrics computeMetrics(const Transcription& transcription); + +/** + * Compute the Metrics for the event node. + * @param transcription: multiple shooting transcription for event node. + * @return Metrics for a event node. + */ +Metrics computeMetrics(const EventTranscription& transcription); + +/** + * Compute the Metrics for the terminal node. + * @param transcription: multiple shooting transcription for terminal node. + * @return Metrics for a terminal node. + */ +Metrics computeMetrics(const TerminalTranscription& transcription); + +/** + * Compute the Metrics for a single intermediate node. + * @param optimalControlProblem : Definition of the optimal control problem + * @param discretizer : Integrator to use for creating the discrete dynamics. + * @param t : Start of the discrete interval + * @param dt : Duration of the interval + * @param x : State at start of the interval + * @param x_next : State at the end of the interval + * @param u : Input, taken to be constant across the interval. + * @return Metrics for a single intermediate node. + */ +Metrics computeIntermediateMetrics(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, scalar_t dt, + const vector_t& x, const vector_t& x_next, const vector_t& u); + +/** + * Compute the Metrics for the event node. + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the event node + * @param x : Pre-event state + * @param x_next : Post-event state + * @return Metrics for the event node. + */ +Metrics computeEventMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); + +/** + * Compute the Metrics for the terminal node. + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the terminal node + * @param x : Terminal state + * @return Metrics for the terminal node. + */ +Metrics computeTerminalMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h new file mode 100644 index 000000000..50456edd8 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/PerformanceIndexComputation.h @@ -0,0 +1,102 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +#include "ocs2_oc/multiple_shooting/Transcription.h" +#include "ocs2_oc/oc_data/PerformanceIndex.h" +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" + +namespace ocs2 { +namespace multiple_shooting { + +/** + * Compute the performance index from the transcription for an intermediate node. + * @param transcription: multiple shooting transcription for an intermediate node. + * @param dt : Duration of the interval + * @return Performance index for a single intermediate node. + */ +PerformanceIndex computePerformanceIndex(const Transcription& transcription, scalar_t dt); + +/** + * Compute the performance index from the transcription for the event node. + * @param transcription: multiple shooting transcription for this node. + * @return Performance index for the event node. + */ +PerformanceIndex computePerformanceIndex(const EventTranscription& transcription); + +/** + * Compute the performance index from the transcription for the terminal node. + * @param transcription: multiple shooting transcription for this node. + * @return Performance index for the terminal node. + */ +PerformanceIndex computePerformanceIndex(const TerminalTranscription& transcription); + +/** + * Compute only the performance index for a single intermediate node. + * Corresponds to the performance index computed from the Transcription returned by "multiple_shooting::setupIntermediateNode". + * @param optimalControlProblem : Definition of the optimal control problem + * @param discretizer : Integrator to use for creating the discrete dynamics. + * @param t : Start of the discrete interval + * @param dt : Duration of the interval + * @param x : State at start of the interval + * @param x_next : State at the end of the interval + * @param u : Input, taken to be constant across the interval. + * @return Performance index for a single intermediate node. + */ +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); + +/** + * Compute only the performance index for the event node. + * Corresponds to the performance index computed from EventTranscription returned by "multiple_shooting::setEventNode". + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the event node + * @param x : Pre-event state + * @param x_next : Post-event state + * @return Performance index for the event node. + */ +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next); + +/** + * Compute only the performance index for the terminal node. + * Corresponds to the performance index computed from TerminalTranscription returned by "multiple_shooting::setTerminalNode". + * @param optimalControlProblem : Definition of the optimal control problem + * @param t : Time at the terminal node + * @param x : Terminal state + * @return Performance index for the terminal node. + */ +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h new file mode 100644 index 000000000..10e378c78 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h @@ -0,0 +1,60 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +namespace ocs2 { +namespace multiple_shooting { + +/** + * Coefficients to compute the Newton step of the Lagrange multiplier associated with the state-input equality constraint such that + * dfdx * dx + dfdu * du + dfdcostate * dcostate + f + */ +struct ProjectionMultiplierCoefficients { + matrix_t dfdx; + matrix_t dfdu; + matrix_t dfdcostate; + vector_t f; + + /** + * Computes the coefficients of the Lagrange multiplier associated with the state-input equality constraint. + * + * @param cost : The cost quadratic approximation. + * @param dynamics : The dynamics linear approximation. + * @param constraintProjection : The constraint projection. + * @param pseudoInverse : Left pseudo-inverse of D^T of the state-input linearized equality constraint (C dx + D du + e = 0). + */ + void compute(const ScalarFunctionQuadraticApproximation& cost, const VectorFunctionLinearApproximation& dynamics, + const VectorFunctionLinearApproximation& constraintProjection, const matrix_t& pseudoInverse); +}; + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h similarity index 65% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h rename to ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h index 92985b915..51e10b60a 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingTranscription.h +++ b/ocs2_oc/include/ocs2_oc/multiple_shooting/Transcription.h @@ -31,21 +31,36 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include + +#include "ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h" +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" namespace ocs2 { namespace multiple_shooting { +/** + * The size of each constraint terms for each type. + */ +struct ConstraintsSize { + size_array_t stateEq; + size_array_t stateInputEq; + size_array_t stateIneq; + size_array_t stateInputIneq; +}; + /** * Results of the transcription at an intermediate node */ struct Transcription { - PerformanceIndex performance; - VectorFunctionLinearApproximation dynamics; + ConstraintsSize constraintsSize; ScalarFunctionQuadraticApproximation cost; - VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation dynamics; + VectorFunctionLinearApproximation stateEqConstraints; + VectorFunctionLinearApproximation stateInputEqConstraints; + VectorFunctionLinearApproximation stateIneqConstraints; + VectorFunctionLinearApproximation stateInputIneqConstraints; VectorFunctionLinearApproximation constraintsProjection; + ProjectionMultiplierCoefficients projectionMultiplierCoefficients; }; /** @@ -53,7 +68,6 @@ struct Transcription { * * @param optimalControlProblem : Definition of the optimal control problem * @param sensitivityDiscretizer : Integrator to use for creating the discrete dynamics. - * @param projectStateInputEqualityConstraints * @param t : Start of the discrete interval * @param dt : Duration of the interval * @param x : State at start of the interval @@ -61,24 +75,25 @@ struct Transcription { * @param u : Input, taken to be constant across the interval. * @return multiple shooting transcription for this node. */ -Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, - DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, +Transcription setupIntermediateNode(OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); /** - * Compute only the performance index for a single intermediate node. - * Corresponds to the performance index returned by "setupIntermediateNode" + * Apply the state-input equality constraint projection for a single intermediate node transcription. + * + * @param transcription : Transcription for a single intermediate node + * @param extractProjectionMultiplier : Whether to extract the projection multiplier. */ -PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u); +void projectTranscription(Transcription& transcription, bool extractProjectionMultiplier = false); /** * Results of the transcription at a terminal node */ struct TerminalTranscription { - PerformanceIndex performance; + ConstraintsSize constraintsSize; ScalarFunctionQuadraticApproximation cost; - VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation eqConstraints; + VectorFunctionLinearApproximation ineqConstraints; }; /** @@ -89,22 +104,17 @@ struct TerminalTranscription { * @param x : Terminal state * @return multiple shooting transcription for the terminal node. */ -TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); - -/** - * Compute only the performance index for the terminal node. - * Corresponds to the performance index returned by "setTerminalNode" - */ -PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); +TerminalTranscription setupTerminalNode(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x); /** * Results of the transcription at an event */ struct EventTranscription { - PerformanceIndex performance; - VectorFunctionLinearApproximation dynamics; + ConstraintsSize constraintsSize; ScalarFunctionQuadraticApproximation cost; - VectorFunctionLinearApproximation constraints; + VectorFunctionLinearApproximation dynamics; + VectorFunctionLinearApproximation eqConstraints; + VectorFunctionLinearApproximation ineqConstraints; }; /** @@ -116,15 +126,7 @@ struct EventTranscription { * @param x_next : Post-event state * @return multiple shooting transcription for the event node. */ -EventTranscription setupEventNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next); - -/** - * Compute only the performance index for the event node. - * Corresponds to the performance index returned by "setupEventNode" - */ -PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next); +EventTranscription setupEventNode(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next); } // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h similarity index 61% rename from ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h rename to ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h index ee45978ee..13fdf42a2 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/PerformanceIndex.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/PerformanceIndex.h @@ -29,10 +29,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once -#include #include #include +#include namespace ocs2 { @@ -46,6 +46,13 @@ struct PerformanceIndex { /** The total cost of a rollout. */ scalar_t cost = 0.0; + /** Sum of Squared Error (SSE) of the dual feasibilities: + * - Final: squared norm of violation in the dual feasibilities + * - PreJumps: sum of squared norm of violation in the dual feasibilities + * - Intermediates: sum of squared norm of violation in the dual feasibilities + */ + scalar_t dualFeasibilitiesSSE = 0.0; + /** Sum of Squared Error (SSE) of system dynamics violation */ scalar_t dynamicsViolationSSE = 0.0; @@ -56,6 +63,13 @@ struct PerformanceIndex { */ scalar_t equalityConstraintsSSE = 0.0; + /** Sum of Squared Error (SSE) of inequality constraints: + * - Final: squared norm of violation in state inequality constraints + * - PreJumps: sum of squared norm of violation in state inequality constraints + * - Intermediates: Integral of squared norm violation in state/state-input inequality constraints + */ + scalar_t inequalityConstraintsSSE = 0.0; + /** Sum of equality Lagrangians: * - Final: penalty for violation in state equality constraints * - PreJumps: penalty for violation in state equality constraints @@ -70,51 +84,44 @@ struct PerformanceIndex { */ scalar_t inequalityLagrangian = 0.0; - /** Add performance indices */ - PerformanceIndex& operator+=(const PerformanceIndex& rhs) { - this->merit += rhs.merit; - this->cost += rhs.cost; - this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; - this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; - this->equalityLagrangian += rhs.equalityLagrangian; - this->inequalityLagrangian += rhs.inequalityLagrangian; - return *this; - } + /** Add performance indices. */ + PerformanceIndex& operator+=(const PerformanceIndex& rhs); + + /** Multiply by a scalar. */ + PerformanceIndex& operator*=(const scalar_t c); + + /** Returns true if *this is approximately equal to other, within the precision determined by prec. */ + bool isApprox(const PerformanceIndex& other, const scalar_t prec = 1e-8) const; }; +/** Add performance indices. */ inline PerformanceIndex operator+(PerformanceIndex lhs, const PerformanceIndex& rhs) { - lhs += rhs; // Copied lhs, add rhs to it. + lhs += rhs; // copied lhs, add rhs to it. return lhs; } -/** Swaps performance indices */ -inline void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { - std::swap(lhs.merit, rhs.merit); - std::swap(lhs.cost, rhs.cost); - std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); - std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); - std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); - std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); +/** Multiply by a scalar. */ +inline PerformanceIndex operator*(PerformanceIndex lhs, const scalar_t c) { + lhs *= c; // copied lhs + return lhs; } -inline std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex) { - const size_t tabSpace = 12; - const auto indentation = stream.width(); - stream << std::left; // fill from left +/** Multiply by a scalar. */ +inline PerformanceIndex operator*(const scalar_t c, PerformanceIndex rhs) { + rhs *= c; // copied rhs + return rhs; +} - stream << std::setw(indentation) << ""; - stream << "Rollout Merit: " << std::setw(tabSpace) << performanceIndex.merit; - stream << "Rollout Cost: " << std::setw(tabSpace) << performanceIndex.cost << '\n'; +/** Swaps performance indices. */ +void swap(PerformanceIndex& lhs, PerformanceIndex& rhs); - stream << std::setw(indentation) << ""; - stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE; - stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE << '\n'; +/** Computes the PerformanceIndex based on a given continuous-time Metrics. */ +PerformanceIndex toPerformanceIndex(const Metrics& m); - stream << std::setw(indentation) << ""; - stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; - stream << "Inequality Lagrangian: " << std::setw(tabSpace) << performanceIndex.inequalityLagrangian; +/** Computes the PerformanceIndex based on a given discrete-time Metrics. */ +PerformanceIndex toPerformanceIndex(const Metrics& m, const scalar_t dt); - return stream; -} +/** Overloads the stream insertion operator. */ +std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex); } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/oc_data/ProblemMetrics.h b/ocs2_oc/include/ocs2_oc/oc_data/ProblemMetrics.h index c5c26b8d0..8368cc447 100644 --- a/ocs2_oc/include/ocs2_oc/oc_data/ProblemMetrics.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/ProblemMetrics.h @@ -35,9 +35,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { struct ProblemMetrics { - MetricsCollection final; - std::vector preJumps; - std::vector intermediates; + Metrics final; + std::vector preJumps; + std::vector intermediates; /** Exchanges the content of ProblemMetrics */ void swap(ProblemMetrics& other) { diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h b/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h similarity index 80% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h rename to ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h index e235115a0..dc0a70422 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/TimeDiscretization.h +++ b/ocs2_oc/include/ocs2_oc/oc_data/TimeDiscretization.h @@ -61,7 +61,7 @@ scalar_t getIntervalEnd(const AnnotatedTime& end); scalar_t getIntervalDuration(const AnnotatedTime& start, const AnnotatedTime& end); /** - * Decides on time discretization along the horizon. Tries to makes step of dt, but will also ensure that eventtimes are part of the + * Decides on time discretization along the horizon. Tries to makes step of dt, but will also ensure that event times are part of the * discretization. * * @param initTime : start time. @@ -76,4 +76,28 @@ std::vector timeDiscretizationWithEvents(scalar_t initTime, scala const scalar_array_t& eventTimes, scalar_t dt_min = 10.0 * numeric_traits::limitEpsilon()); +/** + * Extracts the time trajectory from the annotated time trajectory. + * + * @param annotatedTime : Annotated time trajectory. + * @return The time trajectory. + */ +scalar_array_t toTime(const std::vector& annotatedTime); + +/** + * Extracts the time trajectory from the annotated time trajectory respecting interpolation rules around event times. + * + * @param annotatedTime : Annotated time trajectory. + * @return The time trajectory. + */ +scalar_array_t toInterpolationTime(const std::vector& annotatedTime); + +/** + * Extracts the array of indices indicating the post-event times from the annotated time trajectory. + * + * @param annotatedTime : Annotated time trajectory. + * @return The post-event indices. + */ +size_array_t toPostEventIndices(const std::vector& annotatedTime); + } // namespace ocs2 diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h similarity index 98% rename from ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h rename to ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h index 425644c90..efad15ec4 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/OcpSize.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpSize.h @@ -34,8 +34,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include namespace ocs2 { -namespace hpipm_interface { - /** * Size of the optimal control problem to be solved with the HpipmInterface * @@ -88,5 +86,4 @@ OcpSize extractSizesFromProblem(const std::vector& cost, const std::vector* constraints); -} // namespace hpipm_interface } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h b/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h new file mode 100644 index 000000000..2fa79343e --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OcpToKkt.h @@ -0,0 +1,166 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include + +#include "ocs2_oc/oc_problem/OcpSize.h" + +namespace ocs2 { + +/** + * Constructs concatenated linear approximation of the constraints from the dynamics and constraints arrays w.r.t. + * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * G Z = g + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] x0: The initial state. + * @param[in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param[out] res: The resulting constraints approxmation. Here we misused dfdx field to store the jacobian w.r.t. Z. + */ +void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector& dynamics, + const std::vector* constraints, const vector_array_t* scalingVectorsPtr, + VectorFunctionLinearApproximation& res); + +/** + * Constructs concatenated linear approximation of the constraints from the dynamics and constraints arrays w.r.t. + * Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * G Z = g + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] x0: The initial state. + * @param[in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param[in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param[in] scalingVectorsPtr: Vector representatoin for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param[out] G: The jacobian of the concatenated constraints w.r.t. Z. + * @param[out] g: The concatenated constraints value. + */ +void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector& dynamics, + const std::vector* constraints, const vector_array_t* scalingVectorsPtr, + Eigen::SparseMatrix& G, vector_t& g); + +/** + * Constructs concatenated quadratic approximation of the total cost w.r.t. Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * totalCost = 0.5 Z' H Z + Z' h + h0 + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] x0: The initial state. + * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @param[out] res: The resulting cost approxmation. Here we misused dfdx and dfdxx fields to store the jacobian and the hessian + * matrices w.r.t. Z. + */ +void getCostMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector& cost, + ScalarFunctionQuadraticApproximation& res); + +/** + * Constructs concatenated the jacobian and the hessian matrices of the total cost w.r.t. Z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * totalCost = 0.5 Z' H Z + Z' h + h0 + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * @param[in] ocpSize: The size of optimal control problem. + * @param[in] x0: The initial state. + * @param[in] cost: Quadratic approximation of the cost over the time horizon. + * @param[out] H: The concatenated hessian matrix w.r.t. Z. + * @param[out] h: The concatenated jacobian vector w.r.t. Z. + */ +void getCostMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector& cost, + Eigen::SparseMatrix& H, vector_t& h); + +/** + * Deserializes the stacked solution to state-input trajecotries. Note that the initial state is not part of the stacked solution. + * + * @param [in] ocpSize : Optimal control problem sizes. + * @param [in] stackedSolution : Defined as [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * @param [in] x0 : The initial state. + * @param [out] xTrajectory : State tarjectory. + * @param [out] uTrajectory : Input trajecotry. + */ +void toOcpSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, + vector_array_t& uTrajectory); + +/** + * Serializes the state-input trajecotries. Note that the initial state is not considered as a decision variable. + * + * @param [in] xTrajectory : State tarjectory. + * @param [in] UTrajectory : Input trajecotry. + * @param [out] stackedSolution : [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + */ +void toKktSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution); + +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h index 64e53b6c1..b20fe2ec2 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblem.h @@ -76,6 +76,16 @@ struct OptimalControlProblem { /** Final equality constraints */ std::unique_ptr finalEqualityConstraintPtr; + /* Inequality Constraints */ + /** Intermediate inequality constraints */ + std::unique_ptr inequalityConstraintPtr; + /** Intermediate state-only inequality constraints */ + std::unique_ptr stateInequalityConstraintPtr; + /** Pre-jump inequality constraints */ + std::unique_ptr preJumpInequalityConstraintPtr; + /** Final inequality constraints */ + std::unique_ptr finalInequalityConstraintPtr; + /* Lagrangians */ /** Lagrangian for intermediate equality constraints */ std::unique_ptr equalityLagrangianPtr; diff --git a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h index c6320105b..80a4e7eb3 100644 --- a/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h +++ b/ocs2_oc/include/ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h @@ -29,6 +29,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + #include #include @@ -93,81 +95,108 @@ void updateDualSolution(const OptimalControlProblem& ocp, const PrimalSolution& /** * Updates in-place final MultiplierCollection for equality and inequality Lagrangians. - * Moreover it also updates the penalties of MetricsCollection based on the update of multipliers. + * Moreover it also updates the penalties of Metrics based on the update of multipliers. * * @param [in] ocp : A const reference to the optimal control problem. * @param [in] time : Final time. * @param [in] state : Final state. - * @param [in, out] metricsCollection: The final MetricsCollection. Its penalties will be updated based on - * the update of multiplierCollection. - * @param [out] multiplierCollection : The updated final MultiplierCollection. + * @param [in, out] metrics: The final Metrics. Its penalties will be updated based on the update of multiplierCollection. + * @param [out] multipliers : The updated final MultiplierCollection. */ -void updateFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection); +void updateFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, Metrics& metrics, + MultiplierCollection& multipliers); /** * Updates in-place pre-jump MultiplierCollection for equality and inequality Lagrangians. - * Moreover it also updates the penalties of MetricsCollection based on the update of multipliers. + * Moreover it also updates the penalties of Metrics based on the update of multipliers. * * @param [in] ocp : A const reference to the optimal control problem. * @param [in] time : Pre-jump time. * @param [in] state : Pre-jump state. - * @param [in, out] metricsCollection: The pre-jump MetricsCollection. Its penalties will be updated based on - * the update of multiplierCollection. - * @param [out] multiplierCollection : The updated pre-jump MultiplierCollection. + * @param [in, out] metrics: The pre-jump Metrics. Its penalties will be updated based on the update of multiplierCollection. + * @param [out] multipliers : The updated pre-jump MultiplierCollection. */ -void updatePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection); +void updatePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, Metrics& metrics, + MultiplierCollection& multipliers); /** * Updates in-place intermediate MultiplierCollection for equality and inequality Lagrangians. - * Moreover it also updates the penalties of MetricsCollection based on the update of multipliers. + * Moreover it also updates the penalties of Metrics based on the update of multipliers. * * @param [in] ocp : A const reference to the optimal control problem. * @param [in] time : Intermediate time. * @param [in] state : Intermediate state. * @param [in] input : Intermediate input. - * @param [in, out] metricsCollection: The intermediate MetricsCollection. Its penalties will be updated based on - * the update of multiplierCollection. - * @param [out] multiplierCollection : The updated Intermediate MultiplierCollection. + * @param [in, out] metrics: The intermediate Metrics. Its penalties will be updated based on the update of multiplierCollection. + * @param [out] multipliers : The updated Intermediate MultiplierCollection. */ void updateIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, const vector_t& input, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection); + Metrics& metrics, MultiplierCollection& multipliers); /** - * Extracts a requested final Term LagrangianMetrics from the input MetricsCollection. + * Extracts a requested final constraint term vector from the input Metrics. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsColl : MetricsCollection. - * @param [out] metrics : A const pointer to the term LagrangianMetrics. - * @return True if the term found in the final collection. + * @param [in] metrics : Metrics. + * @return A const pointer to the constraint term value. + */ +const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const Metrics& metrics); + +/** + * Extracts a requested final term LagrangianMetrics from the input Metrics. + * @param [in] ocp : A const reference to the optimal control problem. + * @param [in] name : The name of the term. + * @param [in] metrics : Metrics. + * @return A const pointer to the term LagrangianMetrics. */ -const LagrangianMetrics* extractFinalTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const MetricsCollection& metricsColl); +const LagrangianMetrics* extractFinalTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const Metrics& metrics); /** - * Extracts a requested pre-jump Term LagrangianMetrics array from the input MetricsCollection array. + * Extracts an array of the requested pre-jump constraint term from the input Metrics array. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsCollArray : MetricsCollection array. - * @param [out] metricsArray : Array of const references to term LagrangianMetrics. + * @param [in] metricsArray : Metrics array. + * @param [out] constraintArray : Array of const references to constraint term values. * @return True if the term found in the pre-jump collection. */ -bool extractPreJumpTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector& metricsCollArray, - std::vector& metricsArray); +bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const std::vector& metricsArray, + std::vector>& constraintArray); + +/** + * Extracts a requested pre-jump term LagrangianMetrics array from the input Metrics array. + * @param [in] ocp : A const reference to the optimal control problem. + * @param [in] name : The name of the term. + * @param [in] metricsArray : Metrics array. + * @param [out] lagrangianMetricsArray : Array of const references to term LagrangianMetrics. + * @return True if the term found in the pre-jump collection. + */ +bool extractPreJumpTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const std::vector& metricsArray, + std::vector& lagrangianMetricsArray); + +/** + * Extracts a trajectory of the requested intermediate constraint term from the input Metrics trajectory. + * @param [in] ocp : A const reference to the optimal control problem. + * @param [in] name : The name of the term. + * @param [in] metricsTraj : Metrics trajectory. + * @param [out] constraintTraj : Trajectory of const references to constraint term values. + * @return True if the term found in the intermediate collection. + */ +bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const std::vector& metricsTraj, + std::vector>& constraintTraj); /** - * Extracts a requested intermediate Term LagrangianMetrics trajectory from the input MetricsCollection trajectory. + * Extracts a requested intermediate term LagrangianMetrics trajectory from the input Metrics trajectory. * @param [in] ocp : A const reference to the optimal control problem. * @param [in] name : The name of the term. - * @param [in] metricsCollTraj : MetricsCollection trajectory. - * @param [out] metricsTrajectory : Trajectory of const references to term LagrangianMetrics. + * @param [in] metricsTraj : Metrics trajectory. + * @param [out] lagrangianMetricsTraj : Trajectory of const references to term LagrangianMetrics. * @return True if the term found in the intermediate collection. */ -bool extractIntermediateTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector& metricsCollTraj, - std::vector& metricsTrajectory); +bool extractIntermediateTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const std::vector& metricsTraj, + std::vector& lagrangianMetricsTraj); /** * Extracts a requested final Term Multiplier from the input MultiplierCollection. diff --git a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h index 753aa9461..65c8df349 100644 --- a/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h +++ b/ocs2_oc/include/ocs2_oc/oc_solver/SolverBase.h @@ -37,14 +37,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include -#include -#include -#include -#include -#include "ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h" +#include "ocs2_oc/oc_data/DualSolution.h" +#include "ocs2_oc/oc_data/PerformanceIndex.h" +#include "ocs2_oc/oc_data/PrimalSolution.h" +#include "ocs2_oc/oc_data/ProblemMetrics.h" +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" +#include "ocs2_oc/synchronized_module/ReferenceManagerInterface.h" +#include "ocs2_oc/synchronized_module/SolverObserver.h" +#include "ocs2_oc/synchronized_module/SolverSynchronizedModule.h" namespace ocs2 { @@ -134,12 +134,10 @@ class SolverBase { } /** - * Adds one observer to the vector of modules that observe solver's dual solution and optimized metrics. - * @note: These observer can slow down the MPC. Only employ them during debugging and remove them for deployment. + * Adds an observer to probe the dual solution or optimized metrics. + * @note: Observers will slow down the MPC. Only employ them during debugging and remove them for deployment. */ - void addAugmentedLagrangianObserver(std::unique_ptr observerModule) { - augmentedLagrangianObservers_.push_back(std::move(observerModule)); - } + void addSolverObserver(std::unique_ptr observerModule) { solverObservers_.push_back(std::move(observerModule)); } /** * @brief Returns a const reference to the definition of optimal control problem. @@ -197,7 +195,7 @@ class SolverBase { * * @return: The dual problem's solution. */ - virtual const DualSolution& getDualSolution() const = 0; + virtual const DualSolution* getDualSolution() const { return nullptr; } /** * @brief Returns the optimized value of the Metrics. @@ -271,7 +269,7 @@ class SolverBase { mutable std::mutex outputDisplayGuardMutex_; std::shared_ptr referenceManagerPtr_; // this pointer cannot be nullptr std::vector> synchronizedModules_; - std::vector> augmentedLagrangianObservers_; + std::vector> solverObservers_; }; } // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/precondition/Ruzi.h b/ocs2_oc/include/ocs2_oc/precondition/Ruzi.h new file mode 100644 index 000000000..7f37a3a3f --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/precondition/Ruzi.h @@ -0,0 +1,176 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. + ******************************************************************************/ + +#pragma once + +#include + +#include +#include + +#include "ocs2_oc/oc_problem/OcpSize.h" + +namespace ocs2 { +namespace precondition { + +/** + * Calculates the pre-conditioning factors D, E, and c, and scale the input dynamics, and cost data in place and parallel. + * + * There are a few pre-conditioning methods aiming to shape different aspects of the problem. To balance the performance and + * computational effort, we choose a modified Ruzi equilibration Algorithm. Interested readers can find the original Ruiz + * equilibration in: "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices". + * + * This pre-conditioning transforms the following minimization with z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}] + * + * min_{z} 1/2 z' H y + y' h + * s.t. G z = g + * + * to the follwoing one with y := inv(D) z + * + * min_{y} c/2 y' (D H D) y + c y' (D h) + * s.t. (E G D) y = E g + * + * The KKT matrices H, h, G, and g are defined as + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * @param [in] threadPool : The external thread pool. + * @param [in] x0 : The initial state. + * @param [in] ocpSize : The size of the oc problem. + * @param [in] iteration : Number of iterations. + * @param [in, out] dynamics : The dynamics array of all time points. + * @param [in, out] cost : The cost array of all time points. + * @param [out] DOut : The matrix D decomposed for each time step. + * @param [out] EOut : The matrix E decomposed for each time step. + * @param [out] scalingVectors : Vector representation for the identity parts of the dynamics constraints inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal components + * of this type of matrix for every timestamp. + * @param [out] cOut : Scaling factor c. + */ +void ocpDataInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, + std::vector& dynamics, + std::vector& cost, vector_array_t& DOut, vector_array_t& EOut, + vector_array_t& scalingVectors, scalar_t& cOut); + +/** + * Calculates the pre-conditioning factors D, E, and c, and scale the input dynamics, and cost data in place in place. + * + * There are a few pre-conditioning methods aiming to shape different aspects of the problem. To balance the performance and + * computational effort, we choose a modified Ruzi equilibration Algorithm. Interested readers can find the original Ruiz + * equilibration in: "Ruiz, D., 2001. A scaling algorithm to equilibrate both rows and columns norms in matrices". + * + * This pre-conditioning transforms the following minimization with z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}] + * + * min_{z} 1/2 z' H y + y' h + * s.t. G z = g + * + * to the follwoing one with y := inv(D) z + * + * min_{y} c/2 y' (D H D) y + c y' (D h) + * s.t. (E G D) y = E g + * + * The KKT matrices H, h, G, and g are defined as + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * For constructing H, h, G, and g, refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * @param [in] iteration : Number of iterations. + * @param [in, out] H : The hessian matrix of the total cost. + * @param [in, out] h : The jacobian vector of the total cost. + * @param [in, out] G : The jacobian matrix of the constarinst. + * @param [in, out] g : The constraints vector. + * @param [out] DOut : The matrix D decomposed for each time step. + * @param [out] EOut : The matrix E decomposed for each time step. + * @param [out] cOut : Scaling factor c. + */ +void kktMatrixInPlace(int iteration, Eigen::SparseMatrix& H, vector_t& h, Eigen::SparseMatrix& G, vector_t& g, + vector_t& DOut, vector_t& EOut, scalar_t& cOut); + +/** + * Scales the dynamics and cost array in place and construct scaling vector array from the given scaling factors E, D and c. + * + * @param [in] ocpSize : The size of the oc problem. + * @param [in] D : Scaling factor D + * @param [in] E : Scaling factor E + * @param [in] c : Scaling factor c + * @param [in, out] dynamics : The dynamics array of all time points. + * @param [in, out] cost : The cost array of all time points. + * @param [out] scalingVectors : Vector representation for the identity parts of the dynamics inside the constraint matrix. + * After scaling, they become arbitrary diagonal matrices. scalingVectors store the diagonal + * components of this type of matrix for every timestamp. + */ +void scaleOcpData(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector& dynamics, std::vector& cost, + std::vector& scalingVectors); + +/** + * Descales the solution. Note that the initial state is not considered a decision variable; therefore, it is not scaled. + * This pre-conditioning transforms the following decision vector z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}] to y := inv(D) z. + * + * @param [in] D : Scaling factor D + * @param [in, out] xTrajectory : The state trajectory of the length (N + 1). + * @param [in, out] uTrajectory : The input trajectory of the length N. + */ +void descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory); + +} // namespace precondition +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h b/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h new file mode 100644 index 000000000..a89721d50 --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/search_strategy/FilterLinesearch.h @@ -0,0 +1,83 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +namespace ocs2 { + +/* + * Filter linesearch based on: + * "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" + * https://link.springer.com/article/10.1007/s10107-004-0559-y + * + * step acceptance criteria with c = costs, g = the norm of constraint violation, and w = [x; u] + */ +struct FilterLinesearch { + enum class StepType { UNKNOWN, CONSTRAINT, DUAL, COST, ZERO }; + + scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} + scalar_t g_min = 1e-6; // (2): ELSE IF (g{i} < g_min AND g{i+1} < g_min AND dc/dw'{i} * delta_w < 0) REQUIRE Armijo condition + scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} + scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * armijoDescentMetric{i} + + /** + * Checks that the step is accepted. + * + * @param [in] baselinePerformance : The zero step PerformanceIndex + * @param [in] stepPerformance : The step PerformanceIndex + * @param [in] armijoDescentMetric : The step Armijo descent metric defined as dc/dw' * delta_w + */ + std::pair acceptStep(const PerformanceIndex& baselinePerformance, const PerformanceIndex& stepPerformance, + scalar_t armijoDescentMetric) const; + + /** Compute total constraint violation */ + static scalar_t totalConstraintViolation(const PerformanceIndex& performance) { + return std::sqrt(performance.dynamicsViolationSSE + performance.equalityConstraintsSSE); + } +}; + +/** Transforms the StepType to string */ +std::string toString(const FilterLinesearch::StepType& stepType); + +/** + * Computes the Armijo descent metric that determines if the solution is a descent direction for the cost. It calculates sum of + * gradient(cost).dot([dx; du]) over the trajectory. + * + * @param [in] cost: The quadratic approximation of the cost. + * @param [in] deltaXSol: The state trajectory of the QP subproblem solution. + * @param [in] deltaUSol: The input trajectory of the QP subproblem solution. + * @return The Armijo descent metric. + */ +scalar_t armijoDescentMetric(const std::vector& cost, const vector_array_t& deltaXSol, + const vector_array_t& deltaUSol); + +} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h b/ocs2_oc/include/ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h deleted file mode 100644 index 6d5ca524c..000000000 --- a/ocs2_oc/include/ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h +++ /dev/null @@ -1,107 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -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. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -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. -******************************************************************************/ - -#pragma once - -#include -#include - -#include -#include - -#include "ocs2_oc/oc_data/DualSolution.h" -#include "ocs2_oc/oc_data/ProblemMetrics.h" -#include "ocs2_oc/oc_problem/OptimalControlProblem.h" - -namespace ocs2 { - -/** - * A solver observer module that extract the metrics and multiplier corresponding to the requested term. - */ -class AugmentedLagrangianObserver { - public: - /** Input arguments are time stamp and a const reference array of the term's LagrangianMetrics. */ - using metrics_callback_t = std::function&)>; - /** Input arguments are time stamp and a const reference array of the term's Multiplier. */ - using multiplier_callback_t = std::function&)>; - - /** - * Constructor. - * @param [in] termsName: The name of the term used to add it to OptimalControlProblem. - * @note The name is case sensitive. - */ - explicit AugmentedLagrangianObserver(std::string termName) : termName_(std::move(termName)) {} - - /** - * Sets the callback for processing extracted metrics array. - * The callback should have the following signature: - * void callback(const scalar_array_t& timeTrajectory, const std::vector& termMetrics) - */ - template - void setMetricsCallback(CallbackType&& callback) { - metricsCallback_ = std::forward(callback); - } - - /** - * Sets the callback for processing extracted multiplier array. - * The callback should have the following signature: - * void callback(const scalar_array_t& timeTrajectory, const std::vector& termMultiplierArray) - */ - template - void setMultiplierCallback(CallbackType&& callback) { - multiplierCallback_ = std::forward(callback); - } - - /** - * Extracts the metrics associated to the requested term from the given problemMetrics. - * - * @param [in] ocp : The optimal control problem. - * @param [in] primalSolution : The primal solution. - * @param [in] problemMetrics: The metrics of current solution - */ - void extractTermMetrics(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, const ProblemMetrics& problemMetrics); - - /** - * Extracts the multipliers associated to the requested term from the given dualSolution. - * - * @param [in] ocp : The optimal control problem. - * @param [in] dualSolution: The dual solution - */ - void extractTermMultipliers(const OptimalControlProblem& ocp, const DualSolution& dualSolution); - - private: - const std::string termName_; - std::vector termMetricsArray_{}; - std::vector termMultiplierArray_{}; - - metrics_callback_t metricsCallback_; - multiplier_callback_t multiplierCallback_; -}; - -} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/synchronized_module/SolverObserver.h b/ocs2_oc/include/ocs2_oc/synchronized_module/SolverObserver.h new file mode 100644 index 000000000..5772d1f2d --- /dev/null +++ b/ocs2_oc/include/ocs2_oc/synchronized_module/SolverObserver.h @@ -0,0 +1,180 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include + +#include "ocs2_oc/oc_data/DualSolution.h" +#include "ocs2_oc/oc_data/ProblemMetrics.h" +#include "ocs2_oc/oc_problem/OptimalControlProblem.h" + +namespace ocs2 { + +/** + * A solver observer module that extract the metrics and multiplier corresponding to the requested term. + */ +class SolverObserver { + private: + struct PrivateToken {}; + + public: + /** The public constructor which cannot be used for instantiation. For that, use the following factory methods: + * - SolverObserver::ConstraintTermObserver + * - SolverObserver::LagrangianTermObserver + */ + explicit SolverObserver(PrivateToken token) {} + + /** The time of the metric that will be observed. */ + enum class Type { + Final, + PreJump, + Intermediate, + }; + + /** Input arguments are time stamp and a const reference array of the constraint term's value. */ + using constraint_callback_t = std::function>&)>; + /** Input arguments are time stamp and a const reference array of the term's LagrangianMetrics. */ + using lagrangian_callback_t = std::function&)>; + /** Input arguments are time stamp and a const reference array of the term's Multiplier. */ + using multiplier_callback_t = std::function&)>; + + /** + * Sets the callback for processing extracted a constraint term. + * + * @param [in] type: It could be either at Final, PreJump, or Intermediate. + * @param [in] termsName: The name of the term used to add it to OptimalControlProblem. Note that the name is case sensitive. + * @param [in] constraintCallback: The callback for processing extracted constraint vector array. The callback should have the following + * signature: void callback(const scalar_array_t& timeStamp, const std::vector>& termMetrics). It + * could any C-style function pointer, lambda, or std::function. For an example check constraint_callback_t. + */ + template + static std::unique_ptr ConstraintTermObserver(Type type, const std::string& termName, + ConstraintCallbackType&& constraintCallback); + + /** + * Sets the callback for processing extracted a term's LagrangianMetrics and/or Multiplier. + * + * @param [in] type: It could be either at Final, PreJump, or Intermediate. + * @param [in] termsName: The name of the term used to add it to OptimalControlProblem. Note that the name is case sensitive. + * @param [in] lagrangianCallback: The callback for processing extracted LagrangianMetrics array. The callback should have the following + * signature: void callback(const scalar_array_t& timeStamp, std::vector& termMetrics). It could any C-style + * function pointer, lambda, or std::function. For an example check lagrangian_callback_t). + * @param [in] multiplierCallback: The callback for processing extracted Multiplier array. The callback should have the following + * signature: void callback(const scalar_array_t& timeStamp, const std::vector& termMultiplier). It could any C-style + * function pointer, lambda, or std::function. For an example check multiplier_callback_t. + */ + template + static std::unique_ptr LagrangianTermObserver(Type type, const std::string& termName, + LagrangianCallbackType&& lagrangianCallback, + MultiplierCallbackType&& multiplierCallback = multiplier_callback_t()); + + private: + /** + * Constructor. + * @param [in] termsName: The name of the term used to add it to OptimalControlProblem. + * @note The name is case sensitive. + */ + template + SolverObserver(Type type, std::string termName, ConstraintCallbackType constraintCallback, LagrangianCallbackType lagrangianCallback, + MultiplierCallbackType multiplierCallback) + : type_(type), + termName_(std::move(termName)), + constraintCallback_(std::move(constraintCallback)), + lagrangianCallback_(std::move(lagrangianCallback)), + multiplierCallback_(std::move(multiplierCallback)) {} + + /** + * Extracts the constraint value associated to the requested term from the given problemMetrics. + * + * @param [in] ocp : The optimal control problem. + * @param [in] primalSolution : The primal solution. + * @param [in] problemMetrics: The metrics of current solution + */ + void extractTermConstraint(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, const ProblemMetrics& problemMetrics); + + /** + * Extracts the LagrangianMetrics associated to the requested term from the given problemMetrics. + * + * @param [in] ocp : The optimal control problem. + * @param [in] primalSolution : The primal solution. + * @param [in] problemMetrics: The metrics of current solution + */ + void extractTermLagrangianMetrics(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, + const ProblemMetrics& problemMetrics); + + /** + * Extracts the multipliers associated to the requested term from the given dualSolution. + * + * @param [in] ocp : The optimal control problem. + * @param [in] dualSolution: The dual solution + */ + void extractTermMultipliers(const OptimalControlProblem& ocp, const DualSolution& dualSolution); + + /** + * Variables + */ + const Type type_ = Type::Final; + const std::string termName_ = ""; + constraint_callback_t constraintCallback_; + lagrangian_callback_t lagrangianCallback_; + multiplier_callback_t multiplierCallback_; + + /** SolverBase needs to call extractXXX private methods. */ + friend class SolverBase; +}; + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +std::unique_ptr SolverObserver::ConstraintTermObserver(Type type, const std::string& termName, + ConstraintCallbackType&& constraintCallback) { + return std::unique_ptr(new SolverObserver(type, termName, std::forward(constraintCallback), + lagrangian_callback_t(), multiplier_callback_t())); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +std::unique_ptr SolverObserver::LagrangianTermObserver(Type type, const std::string& termName, + LagrangianCallbackType&& lagrangianCallback, + MultiplierCallbackType&& multiplierCallback) { + return std::unique_ptr(new SolverObserver(type, termName, constraint_callback_t(), + std::forward(lagrangianCallback), + std::forward(multiplierCallback))); +} + +} // namespace ocs2 diff --git a/ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h b/ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h index 0bc83f14c..9a1c25d79 100644 --- a/ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h +++ b/ocs2_oc/include/ocs2_oc/trajectory_adjustment/TrajectorySpreading.h @@ -123,8 +123,13 @@ class TrajectorySpreading final { */ size_array_t findPostEventIndices(const scalar_array_t& eventTimes, const scalar_array_t& timeTrajectory) const { size_array_t postEventIndices(eventTimes.size()); - std::transform(eventTimes.cbegin(), eventTimes.cend(), postEventIndices.begin(), - [this, &timeTrajectory](scalar_t time) -> int { return upperBoundIndex(timeTrajectory, time); }); + for (std::size_t i = 0; i < eventTimes.size(); i++) { + if (i == eventTimes.size() - 1 && eventTimes[i] == timeTrajectory.back()) { + postEventIndices[i] = timeTrajectory.size() - 1; + } else { + postEventIndices[i] = upperBoundIndex(timeTrajectory, eventTimes[i]); + } + } return postEventIndices; } diff --git a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp index 149f503b3..81f8e9794 100644 --- a/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp +++ b/ocs2_oc/src/approximate_model/LinearQuadraticApproximator.cpp @@ -47,11 +47,11 @@ void approximateIntermediateLQ(OptimalControlProblem& problem, const scalar_t ti modelData.time = time; modelData.stateDim = state.rows(); modelData.inputDim = input.rows(); - modelData.dynamicsBias.setZero(state.rows()); // Dynamics modelData.dynamicsCovariance = problem.dynamicsPtr->dynamicsCovariance(time, state, input); modelData.dynamics = problem.dynamicsPtr->linearApproximation(time, state, input, preComputation); + modelData.dynamicsBias.setZero(modelData.dynamics.dfdx.rows()); // Cost modelData.cost = ocs2::approximateCost(problem, time, state, input); @@ -95,10 +95,10 @@ void approximatePreJumpLQ(OptimalControlProblem& problem, const scalar_t& time, modelData.time = time; modelData.stateDim = state.rows(); modelData.inputDim = 0; - modelData.dynamicsBias.setZero(state.rows()); // Jump map modelData.dynamics = problem.dynamicsPtr->jumpMapLinearApproximation(time, state, preComputation); + modelData.dynamicsBias.setZero(modelData.dynamics.dfdx.rows()); // Pre-jump cost modelData.cost = approximateEventCost(problem, time, state); @@ -269,23 +269,53 @@ ScalarFunctionQuadraticApproximation approximateFinalCost(const OptimalControlPr /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MetricsCollection computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const vector_t& input, const MultiplierCollection& multipliers) { +Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, + vector_t&& dynamicsViolation) { auto& preComputation = *problem.preComputationPtr; - MetricsCollection metrics; + Metrics metrics; // Cost metrics.cost = computeCost(problem, time, state, input); + // Dynamics violation + metrics.dynamicsViolation = std::move(dynamicsViolation); + // Equality constraints - metrics.stateEqConstraint = problem.stateEqualityConstraintPtr->getValue(time, state, preComputation); - metrics.stateInputEqConstraint = problem.equalityConstraintPtr->getValue(time, state, input, preComputation); + if (!problem.stateEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = problem.stateEqualityConstraintPtr->getValue(time, state, preComputation); + } + if (!problem.equalityConstraintPtr->empty()) { + metrics.stateInputEqConstraint = problem.equalityConstraintPtr->getValue(time, state, input, preComputation); + } - // Lagrangians + // Inequality constraints + if (!problem.stateInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = problem.stateInequalityConstraintPtr->getValue(time, state, preComputation); + } + if (!problem.inequalityConstraintPtr->empty()) { + metrics.stateInputIneqConstraint = problem.inequalityConstraintPtr->getValue(time, state, input, preComputation); + } + + return metrics; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +Metrics computeIntermediateMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, const vector_t& input, + const MultiplierCollection& multipliers, vector_t&& dynamicsViolation) { + auto& preComputation = *problem.preComputationPtr; + + // cost, dynamics violation, equlaity constraints, inequlaity constraints + auto metrics = computeIntermediateMetrics(problem, time, state, input, std::move(dynamicsViolation)); + + // Equality Lagrangians metrics.stateEqLagrangian = problem.stateEqualityLagrangianPtr->getValue(time, state, multipliers.stateEq, preComputation); - metrics.stateIneqLagrangian = problem.stateInequalityLagrangianPtr->getValue(time, state, multipliers.stateIneq, preComputation); metrics.stateInputEqLagrangian = problem.equalityLagrangianPtr->getValue(time, state, input, multipliers.stateInputEq, preComputation); + + // Inequality Lagrangians + metrics.stateIneqLagrangian = problem.stateInequalityLagrangianPtr->getValue(time, state, multipliers.stateIneq, preComputation); metrics.stateInputIneqLagrangian = problem.inequalityLagrangianPtr->getValue(time, state, input, multipliers.stateInputIneq, preComputation); @@ -295,20 +325,44 @@ MetricsCollection computeIntermediateMetrics(OptimalControlProblem& problem, con /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MetricsCollection computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers) { +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, vector_t&& dynamicsViolation) { auto& preComputation = *problem.preComputationPtr; - MetricsCollection metrics; + Metrics metrics; // Cost metrics.cost = computeEventCost(problem, time, state); + // Dynamics violation + metrics.dynamicsViolation = std::move(dynamicsViolation); + // Equality constraint - metrics.stateEqConstraint = problem.preJumpEqualityConstraintPtr->getValue(time, state, preComputation); + if (!problem.preJumpEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = problem.preJumpEqualityConstraintPtr->getValue(time, state, preComputation); + } - // Lagrangians + // Inequality constraint + if (!problem.preJumpInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = problem.preJumpInequalityConstraintPtr->getValue(time, state, preComputation); + } + + return metrics; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +Metrics computePreJumpMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers, vector_t&& dynamicsViolation) { + auto& preComputation = *problem.preComputationPtr; + + // cost, dynamics violation, equlaity constraints, inequlaity constraints + auto metrics = computePreJumpMetrics(problem, time, state, std::move(dynamicsViolation)); + + // Equality Lagrangians metrics.stateEqLagrangian = problem.preJumpEqualityLagrangianPtr->getValue(time, state, multipliers.stateEq, preComputation); + + // Inequality Lagrangians metrics.stateIneqLagrangian = problem.preJumpInequalityLagrangianPtr->getValue(time, state, multipliers.stateIneq, preComputation); return metrics; @@ -317,20 +371,44 @@ MetricsCollection computePreJumpMetrics(OptimalControlProblem& problem, const sc /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -MetricsCollection computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, - const MultiplierCollection& multipliers) { +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state) { auto& preComputation = *problem.preComputationPtr; - MetricsCollection metrics; + Metrics metrics; // Cost metrics.cost = computeFinalCost(problem, time, state); + // Dynamics violation + // metrics.dynamicsViolation = vector_t(); + // Equality constraint - metrics.stateEqConstraint = problem.finalEqualityConstraintPtr->getValue(time, state, preComputation); + if (!problem.finalEqualityConstraintPtr->empty()) { + metrics.stateEqConstraint = problem.finalEqualityConstraintPtr->getValue(time, state, preComputation); + } - // Lagrangians + // Inequality constraint + if (!problem.finalInequalityConstraintPtr->empty()) { + metrics.stateIneqConstraint = problem.finalInequalityConstraintPtr->getValue(time, state, preComputation); + } + + return metrics; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +Metrics computeFinalMetrics(OptimalControlProblem& problem, const scalar_t time, const vector_t& state, + const MultiplierCollection& multipliers) { + auto& preComputation = *problem.preComputationPtr; + + // cost, equlaity constraints, inequlaity constraints + auto metrics = computeFinalMetrics(problem, time, state); + + // Equality Lagrangians metrics.stateEqLagrangian = problem.finalEqualityLagrangianPtr->getValue(time, state, multipliers.stateEq, preComputation); + + // Inequality Lagrangians metrics.stateIneqLagrangian = problem.finalInequalityLagrangianPtr->getValue(time, state, multipliers.stateIneq, preComputation); return metrics; diff --git a/ocs2_oc/src/lintTarget.cpp b/ocs2_oc/src/lintTarget.cpp index 1f5887c14..ce296fdcc 100644 --- a/ocs2_oc/src/lintTarget.cpp +++ b/ocs2_oc/src/lintTarget.cpp @@ -1,26 +1,72 @@ -// Approximate model +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +// approximate model +#include #include +// multiple_shooting +#include +#include +#include +#include +#include +#include +#include + // oc_data #include #include +#include #include +#include // oc_problem #include +#include +#include #include #include // oc_solver -#include #include +// precondition +#include + // synchronized_module #include #include #include #include #include +#include #include // rollout @@ -30,8 +76,12 @@ #include #include +// search_strategy +#include + // trajectory_adjustment #include +#include // dummy target for clang toolchain int main() { diff --git a/ocs2_oc/src/multiple_shooting/Helpers.cpp b/ocs2_oc/src/multiple_shooting/Helpers.cpp new file mode 100644 index 000000000..5df5f400d --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/Helpers.cpp @@ -0,0 +1,147 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/multiple_shooting/Helpers.h" + +#include +#include + +namespace ocs2 { +namespace multiple_shooting { + +void remapProjectedInput(const std::vector& constraintsProjection, const vector_array_t& deltaXSol, + vector_array_t& deltaUSol) { + vector_t tmp; // 1 temporary for re-use. + for (int i = 0; i < deltaUSol.size(); ++i) { + if (constraintsProjection[i].f.size() > 0) { + tmp.noalias() = constraintsProjection[i].dfdu * deltaUSol[i]; + deltaUSol[i] = tmp + constraintsProjection[i].f; + deltaUSol[i].noalias() += constraintsProjection[i].dfdx * deltaXSol[i]; + } + } +} + +void remapProjectedGain(const std::vector& constraintsProjection, matrix_array_t& KMatrices) { + matrix_t tmp; // 1 temporary for re-use. + for (int i = 0; i < KMatrices.size(); ++i) { + if (constraintsProjection[i].f.size() > 0) { + tmp.noalias() = constraintsProjection[i].dfdu * KMatrices[i]; + KMatrices[i] = tmp + constraintsProjection[i].dfdx; + } + } +} + +PrimalSolution toPrimalSolution(const std::vector& time, ModeSchedule&& modeSchedule, vector_array_t&& x, + vector_array_t&& u) { + // Correct for missing inputs at PreEvents and the terminal time + for (int i = 0; i < u.size(); ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { + u[i] = u[i - 1]; + } + } + // Repeat last input to make equal length vectors + u.push_back(u.back()); + + // Construct nominal time, state and input trajectories + PrimalSolution primalSolution; + primalSolution.timeTrajectory_ = toTime(time); + primalSolution.postEventIndices_ = toPostEventIndices(time); + primalSolution.stateTrajectory_ = std::move(x); + primalSolution.inputTrajectory_ = std::move(u); + primalSolution.modeSchedule_ = std::move(modeSchedule); + primalSolution.controllerPtr_.reset(new FeedforwardController(primalSolution.timeTrajectory_, primalSolution.inputTrajectory_)); + return primalSolution; +} + +PrimalSolution toPrimalSolution(const std::vector& time, ModeSchedule&& modeSchedule, vector_array_t&& x, vector_array_t&& u, + matrix_array_t&& KMatrices) { + // Compute feedback, before x and u are moved to primal solution + // see doc/LQR_full.pdf for detailed derivation for feedback terms + vector_array_t uff = u; // Copy and adapt in loop + for (int i = 0; i < KMatrices.size(); ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { + uff[i] = uff[i - 1]; + KMatrices[i] = KMatrices[i - 1]; + } else { + // Linear controller has convention u = uff + K * x; + // We computed u = u'(t) + K (x - x'(t)); + // >> uff = u'(t) - K x'(t) + uff[i].noalias() -= KMatrices[i] * x[i]; + } + } + // Copy last one to get correct length + uff.push_back(uff.back()); + KMatrices.push_back(KMatrices.back()); + + // Correct for missing inputs at PreEvents and the terminal time + for (int i = 0; i < u.size(); ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { + u[i] = u[i - 1]; + } + } + // Repeat last input to make equal length vectors + u.push_back(u.back()); + + // Construct nominal time, state and input trajectories + PrimalSolution primalSolution; + primalSolution.timeTrajectory_ = toTime(time); + primalSolution.postEventIndices_ = toPostEventIndices(time); + primalSolution.stateTrajectory_ = std::move(x); + primalSolution.inputTrajectory_ = std::move(u); + primalSolution.modeSchedule_ = std::move(modeSchedule); + primalSolution.controllerPtr_.reset(new LinearController(primalSolution.timeTrajectory_, std::move(uff), std::move(KMatrices))); + return primalSolution; +} + +ProblemMetrics toProblemMetrics(const std::vector& time, std::vector&& metrics) { + assert(time.size() > 1); + assert(metrics.size() == time.size()); + + // Problem horizon + const int N = static_cast(time.size()) - 1; + + // resize + ProblemMetrics problemMetrics; + problemMetrics.intermediates.reserve(N); + problemMetrics.preJumps.reserve(N / 10); // the size is just a guess + problemMetrics.final = std::move(metrics.back()); + + for (int i = 0; i < N; ++i) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + problemMetrics.preJumps.push_back(std::move(metrics[i])); + } else { + problemMetrics.intermediates.push_back(std::move(metrics[i])); + } + } + + return problemMetrics; +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/Initialization.cpp b/ocs2_oc/src/multiple_shooting/Initialization.cpp new file mode 100644 index 000000000..436fe2fd0 --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/Initialization.cpp @@ -0,0 +1,82 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/multiple_shooting/Initialization.h" + +namespace ocs2 { +namespace multiple_shooting { + +void initializeStateInputTrajectories(const vector_t& initState, const std::vector& timeDiscretization, + const PrimalSolution& primalSolution, Initializer& initializer, vector_array_t& stateTrajectory, + vector_array_t& inputTrajectory) { + const int N = static_cast(timeDiscretization.size()) - 1; // // size of the input trajectory + stateTrajectory.clear(); + stateTrajectory.reserve(N + 1); + inputTrajectory.clear(); + inputTrajectory.reserve(N); + + // Determine till when to use the previous solution + scalar_t interpolateStateTill = timeDiscretization.front().time; + scalar_t interpolateInputTill = timeDiscretization.front().time; + if (primalSolution.timeTrajectory_.size() >= 2) { + interpolateStateTill = primalSolution.timeTrajectory_.back(); + interpolateInputTill = primalSolution.timeTrajectory_[primalSolution.timeTrajectory_.size() - 2]; + } + + // Initial state + const scalar_t initTime = getIntervalStart(timeDiscretization[0]); + if (initTime < interpolateStateTill) { + stateTrajectory.push_back(LinearInterpolation::interpolate(initTime, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_)); + } else { + stateTrajectory.push_back(initState); + } + + for (int i = 0; i < N; i++) { + if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { + // Event Node + inputTrajectory.push_back(vector_t()); // no input at event node + stateTrajectory.push_back(initializeEventNode(timeDiscretization[i].time, stateTrajectory.back())); + } else { + // Intermediate node + const scalar_t time = getIntervalStart(timeDiscretization[i]); + const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); + vector_t input, nextState; + if (time > interpolateInputTill || nextTime > interpolateStateTill) { // Using initializer + std::tie(input, nextState) = initializeIntermediateNode(initializer, time, nextTime, stateTrajectory.back()); + } else { // interpolate previous solution + std::tie(input, nextState) = initializeIntermediateNode(primalSolution, time, nextTime); + } + inputTrajectory.push_back(std::move(input)); + stateTrajectory.push_back(std::move(nextState)); + } + } +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp b/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp new file mode 100644 index 000000000..7cff7663f --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/LagrangianEvaluation.cpp @@ -0,0 +1,66 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/multiple_shooting/LagrangianEvaluation.h" + +namespace ocs2 { +namespace multiple_shooting { + +ScalarFunctionQuadraticApproximation evaluateLagrangianIntermediateNode(const vector_t& lmd, const vector_t& lmd_next, const vector_t& nu, + ScalarFunctionQuadraticApproximation&& cost, + const VectorFunctionLinearApproximation& dynamics, + const VectorFunctionLinearApproximation& stateInputEqConstraints) { + ScalarFunctionQuadraticApproximation lagrangian = std::move(cost); + lagrangian.dfdx.noalias() += dynamics.dfdx.transpose() * lmd_next; + lagrangian.dfdx.noalias() -= lmd; + lagrangian.dfdu.noalias() += dynamics.dfdu.transpose() * lmd_next; + if (stateInputEqConstraints.f.size() > 0) { + lagrangian.dfdx.noalias() += stateInputEqConstraints.dfdx.transpose() * nu; + lagrangian.dfdu.noalias() += stateInputEqConstraints.dfdu.transpose() * nu; + } + return lagrangian; +} + +ScalarFunctionQuadraticApproximation evaluateLagrangianTerminalNode(const vector_t& lmd, ScalarFunctionQuadraticApproximation&& cost) { + ScalarFunctionQuadraticApproximation lagrangian = std::move(cost); + lagrangian.dfdx.noalias() -= lmd; + return lagrangian; +} + +ScalarFunctionQuadraticApproximation evaluateLagrangianEventNode(const vector_t& lmd, const vector_t& lmd_next, + ScalarFunctionQuadraticApproximation&& cost, + const VectorFunctionLinearApproximation& dynamics) { + ScalarFunctionQuadraticApproximation lagrangian = std::move(cost); + lagrangian.dfdx.noalias() += dynamics.dfdx.transpose() * lmd_next; + lagrangian.dfdx.noalias() -= lmd; + return lagrangian; +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp b/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp new file mode 100644 index 000000000..5cb0ee8f8 --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/MetricsComputation.cpp @@ -0,0 +1,134 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/multiple_shooting/MetricsComputation.h" + +#include "ocs2_oc/approximate_model/LinearQuadraticApproximator.h" + +namespace ocs2 { +namespace multiple_shooting { + +Metrics computeMetrics(const Transcription& transcription) { + const auto& constraintsSize = transcription.constraintsSize; + + Metrics metrics; + + // Cost + metrics.cost = transcription.cost.f; + + // Dynamics + metrics.dynamicsViolation = transcription.dynamics.f; + + // Equality constraints + metrics.stateEqConstraint = toConstraintArray(constraintsSize.stateEq, transcription.stateEqConstraints.f); + metrics.stateInputEqConstraint = toConstraintArray(constraintsSize.stateInputEq, transcription.stateInputEqConstraints.f); + + // Inequality constraints. + metrics.stateIneqConstraint = toConstraintArray(constraintsSize.stateIneq, transcription.stateIneqConstraints.f); + metrics.stateInputIneqConstraint = toConstraintArray(constraintsSize.stateInputIneq, transcription.stateInputIneqConstraints.f); + + return metrics; +} + +Metrics computeMetrics(const EventTranscription& transcription) { + const auto& constraintsSize = transcription.constraintsSize; + + Metrics metrics; + + // Cost + metrics.cost = transcription.cost.f; + + // Dynamics + metrics.dynamicsViolation = transcription.dynamics.f; + + // Equality constraints + metrics.stateEqConstraint = toConstraintArray(constraintsSize.stateEq, transcription.eqConstraints.f); + + // Inequality constraints. + metrics.stateIneqConstraint = toConstraintArray(constraintsSize.stateIneq, transcription.ineqConstraints.f); + + return metrics; +} + +Metrics computeMetrics(const TerminalTranscription& transcription) { + const auto& constraintsSize = transcription.constraintsSize; + + Metrics metrics; + + // Cost + metrics.cost = transcription.cost.f; + + // Equality constraints + metrics.stateEqConstraint = toConstraintArray(constraintsSize.stateEq, transcription.eqConstraints.f); + + // Inequality constraints. + metrics.stateIneqConstraint = toConstraintArray(constraintsSize.stateIneq, transcription.ineqConstraints.f); + + return metrics; +} + +Metrics computeIntermediateMetrics(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, scalar_t dt, + const vector_t& x, const vector_t& x_next, const vector_t& u) { + // Dynamics + auto dynamicsViolation = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); + dynamicsViolation -= x_next; + + // Precomputation + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; + optimalControlProblem.preComputationPtr->request(request, t, x, u); + + // Compute metrics + auto metrics = computeIntermediateMetrics(optimalControlProblem, t, x, u, std::move(dynamicsViolation)); + metrics.cost *= dt; // consider dt + + return metrics; +} + +Metrics computeTerminalMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { + // Precomputation + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; + optimalControlProblem.preComputationPtr->requestFinal(request, t, x); + + return computeFinalMetrics(optimalControlProblem, t, x); +} + +Metrics computeEventMetrics(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { + // Precomputation + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint + Request::Dynamics; + optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); + + // Dynamics + auto dynamicsViolation = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x); + dynamicsViolation -= x_next; + + return computePreJumpMetrics(optimalControlProblem, t, x, std::move(dynamicsViolation)); +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp new file mode 100644 index 000000000..278442fa4 --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/PerformanceIndexComputation.cpp @@ -0,0 +1,109 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/multiple_shooting/PerformanceIndexComputation.h" + +#include + +#include "ocs2_oc/approximate_model/LinearQuadraticApproximator.h" +#include "ocs2_oc/multiple_shooting/MetricsComputation.h" + +namespace ocs2 { +namespace multiple_shooting { + +PerformanceIndex computePerformanceIndex(const Transcription& transcription, scalar_t dt) { + PerformanceIndex performance; + + // Dynamics + performance.dynamicsViolationSSE = dt * transcription.dynamics.f.squaredNorm(); + + // Costs + performance.cost = transcription.cost.f; + + // State-input equality constraints + performance.equalityConstraintsSSE = + dt * (getEqConstraintsSSE(transcription.stateEqConstraints.f) + getEqConstraintsSSE(transcription.stateInputEqConstraints.f)); + + // Inequality constraints. + performance.inequalityConstraintsSSE = + dt * (getIneqConstraintsSSE(transcription.stateIneqConstraints.f) + getIneqConstraintsSSE(transcription.stateInputIneqConstraints.f)); + + return performance; +} + +PerformanceIndex computePerformanceIndex(const EventTranscription& transcription) { + PerformanceIndex performance; + + // Dynamics + performance.dynamicsViolationSSE = transcription.dynamics.f.squaredNorm(); + + performance.cost = transcription.cost.f; + + // Equality constraints + performance.equalityConstraintsSSE = getEqConstraintsSSE(transcription.eqConstraints.f); + + // State inequality constraints. + performance.inequalityConstraintsSSE = getIneqConstraintsSSE(transcription.ineqConstraints.f); + + return performance; +} + +PerformanceIndex computePerformanceIndex(const TerminalTranscription& transcription) { + PerformanceIndex performance; + + performance.cost = transcription.cost.f; + + // Equality constraints + performance.equalityConstraintsSSE = getEqConstraintsSSE(transcription.eqConstraints.f); + + // State inequality constraints. + performance.inequalityConstraintsSSE = getIneqConstraintsSSE(transcription.ineqConstraints.f); + + return performance; +} + +PerformanceIndex computeIntermediatePerformance(OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, scalar_t t, + scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { + const auto metrics = computeIntermediateMetrics(optimalControlProblem, discretizer, t, dt, x, x_next, u); + return toPerformanceIndex(metrics, dt); +} + +PerformanceIndex computeEventPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, + const vector_t& x_next) { + const auto metrics = computeEventMetrics(optimalControlProblem, t, x, x_next); + return toPerformanceIndex(metrics); +} + +PerformanceIndex computeTerminalPerformance(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { + const auto metrics = computeTerminalMetrics(optimalControlProblem, t, x); + return toPerformanceIndex(metrics); +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp b/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp new file mode 100644 index 000000000..d158a4ea2 --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/ProjectionMultiplierCoefficients.cpp @@ -0,0 +1,54 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/multiple_shooting/ProjectionMultiplierCoefficients.h" + +namespace ocs2 { +namespace multiple_shooting { + +void ProjectionMultiplierCoefficients::compute(const ScalarFunctionQuadraticApproximation& cost, + const VectorFunctionLinearApproximation& dynamics, + const VectorFunctionLinearApproximation& constraintProjection, + const matrix_t& pseudoInverse) { + vector_t semiprojectedCost_dfdu = cost.dfdu; + semiprojectedCost_dfdu.noalias() += cost.dfduu * constraintProjection.f; + + matrix_t semiprojectedCost_dfdux = cost.dfdux; + semiprojectedCost_dfdux.noalias() += cost.dfduu * constraintProjection.dfdx; + + const matrix_t semiprojectedCost_dfduu = cost.dfduu * constraintProjection.dfdu; + + this->dfdx.noalias() = -pseudoInverse * semiprojectedCost_dfdux; + this->dfdu.noalias() = -pseudoInverse * semiprojectedCost_dfduu; + this->dfdcostate.noalias() = -pseudoInverse * dynamics.dfdu.transpose(); + this->f.noalias() = -pseudoInverse * semiprojectedCost_dfdu; +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/multiple_shooting/Transcription.cpp b/ocs2_oc/src/multiple_shooting/Transcription.cpp new file mode 100644 index 000000000..ae7757682 --- /dev/null +++ b/ocs2_oc/src/multiple_shooting/Transcription.cpp @@ -0,0 +1,195 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/multiple_shooting/Transcription.h" + +#include + +#include "ocs2_oc/approximate_model/ChangeOfInputVariables.h" +#include "ocs2_oc/approximate_model/LinearQuadraticApproximator.h" + +namespace ocs2 { +namespace multiple_shooting { + +Transcription setupIntermediateNode(OptimalControlProblem& optimalControlProblem, DynamicsSensitivityDiscretizer& sensitivityDiscretizer, + scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { + // Results and short-hand notation + Transcription transcription; + auto& cost = transcription.cost; + auto& dynamics = transcription.dynamics; + auto& constraintsSize = transcription.constraintsSize; + auto& stateEqConstraints = transcription.stateEqConstraints; + auto& stateInputEqConstraints = transcription.stateInputEqConstraints; + auto& stateIneqConstraints = transcription.stateIneqConstraints; + auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; + + // Dynamics + // Discretization returns x_{k+1} = A_{k} * dx_{k} + B_{k} * du_{k} + b_{k} + dynamics = sensitivityDiscretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); + dynamics.f -= x_next; // make it dx_{k+1} = ... + + // Precomputation for other terms + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint + Request::Approximation; + optimalControlProblem.preComputationPtr->request(request, t, x, u); + + // Costs: Approximate the integral with forward euler + cost = approximateCost(optimalControlProblem, t, x, u); + cost *= dt; + + // State equality constraints + if (!optimalControlProblem.stateEqualityConstraintPtr->empty()) { + constraintsSize.stateEq = optimalControlProblem.stateEqualityConstraintPtr->getTermsSize(t); + stateEqConstraints = + optimalControlProblem.stateEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + + // State-input equality constraints + if (!optimalControlProblem.equalityConstraintPtr->empty()) { + constraintsSize.stateInputEq = optimalControlProblem.equalityConstraintPtr->getTermsSize(t); + stateInputEqConstraints = + optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + } + + // State inequality constraints. + if (!optimalControlProblem.stateInequalityConstraintPtr->empty()) { + constraintsSize.stateIneq = optimalControlProblem.stateInequalityConstraintPtr->getTermsSize(t); + stateIneqConstraints = + optimalControlProblem.stateInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + + // State-input inequality constraints. + if (!optimalControlProblem.inequalityConstraintPtr->empty()) { + constraintsSize.stateInputIneq = optimalControlProblem.inequalityConstraintPtr->getTermsSize(t); + stateInputIneqConstraints = + optimalControlProblem.inequalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); + } + + return transcription; +} + +void projectTranscription(Transcription& transcription, bool extractProjectionMultiplier) { + auto& cost = transcription.cost; + auto& dynamics = transcription.dynamics; + auto& stateInputEqConstraints = transcription.stateInputEqConstraints; + auto& stateInputIneqConstraints = transcription.stateInputIneqConstraints; + auto& projection = transcription.constraintsProjection; + auto& projectionMultiplierCoefficients = transcription.projectionMultiplierCoefficients; + + if (stateInputEqConstraints.f.size() > 0) { + // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. + if (extractProjectionMultiplier) { + matrix_t constraintPseudoInverse; + std::tie(projection, constraintPseudoInverse) = LinearAlgebra::qrConstraintProjection(stateInputEqConstraints); + projectionMultiplierCoefficients.compute(cost, dynamics, projection, constraintPseudoInverse); + } else { + projection = LinearAlgebra::luConstraintProjection(stateInputEqConstraints).first; + projectionMultiplierCoefficients = ProjectionMultiplierCoefficients(); + } + stateInputEqConstraints = VectorFunctionLinearApproximation(); + + // Adapt dynamics, cost, and state-input inequality constraints + changeOfInputVariables(dynamics, projection.dfdu, projection.dfdx, projection.f); + changeOfInputVariables(cost, projection.dfdu, projection.dfdx, projection.f); + if (stateInputIneqConstraints.f.size() > 0) { + changeOfInputVariables(stateInputIneqConstraints, projection.dfdu, projection.dfdx, projection.f); + } + } +} + +TerminalTranscription setupTerminalNode(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { + // Results and short-hand notation + TerminalTranscription transcription; + auto& cost = transcription.cost; + auto& constraintsSize = transcription.constraintsSize; + auto& eqConstraints = transcription.eqConstraints; + auto& ineqConstraints = transcription.ineqConstraints; + + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; + optimalControlProblem.preComputationPtr->requestFinal(request, t, x); + + // Costs + cost = approximateFinalCost(optimalControlProblem, t, x); + + // State equality constraints. + if (!optimalControlProblem.finalEqualityConstraintPtr->empty()) { + constraintsSize.stateEq = optimalControlProblem.finalEqualityConstraintPtr->getTermsSize(t); + eqConstraints = + optimalControlProblem.finalEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + + // State inequality constraints. + if (!optimalControlProblem.finalInequalityConstraintPtr->empty()) { + constraintsSize.stateIneq = optimalControlProblem.finalInequalityConstraintPtr->getTermsSize(t); + ineqConstraints = + optimalControlProblem.finalInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + + return transcription; +} + +EventTranscription setupEventNode(OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, const vector_t& x_next) { + // Results and short-hand notation + EventTranscription transcription; + auto& cost = transcription.cost; + auto& dynamics = transcription.dynamics; + auto& constraintsSize = transcription.constraintsSize; + auto& eqConstraints = transcription.eqConstraints; + auto& ineqConstraints = transcription.ineqConstraints; + + constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; + optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); + + // Dynamics + // jump map returns // x_{k+1} = A_{k} * dx_{k} + b_{k} + dynamics = optimalControlProblem.dynamicsPtr->jumpMapLinearApproximation(t, x); + dynamics.f -= x_next; // make it dx_{k+1} = ... + dynamics.dfdu.setZero(x.size(), 0); // Overwrite derivative that shouldn't exist. + + // Costs + cost = approximateEventCost(optimalControlProblem, t, x); + + // State equality constraints. + if (!optimalControlProblem.preJumpEqualityConstraintPtr->empty()) { + constraintsSize.stateEq = optimalControlProblem.preJumpEqualityConstraintPtr->getTermsSize(t); + eqConstraints = + optimalControlProblem.preJumpEqualityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + + // State inequality constraints. + if (!optimalControlProblem.preJumpInequalityConstraintPtr->empty()) { + constraintsSize.stateIneq = optimalControlProblem.preJumpInequalityConstraintPtr->getTermsSize(t); + ineqConstraints = + optimalControlProblem.preJumpInequalityConstraintPtr->getLinearApproximation(t, x, *optimalControlProblem.preComputationPtr); + } + + return transcription; +} + +} // namespace multiple_shooting +} // namespace ocs2 diff --git a/ocs2_oc/src/oc_data/PerformanceIndex.cpp b/ocs2_oc/src/oc_data/PerformanceIndex.cpp new file mode 100644 index 000000000..5c66ecb82 --- /dev/null +++ b/ocs2_oc/src/oc_data/PerformanceIndex.cpp @@ -0,0 +1,131 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/oc_data/PerformanceIndex.h" + +#include + +#include + +namespace ocs2 { + +PerformanceIndex& PerformanceIndex::operator+=(const PerformanceIndex& rhs) { + this->merit += rhs.merit; + this->cost += rhs.cost; + this->dualFeasibilitiesSSE += rhs.dualFeasibilitiesSSE; + this->dynamicsViolationSSE += rhs.dynamicsViolationSSE; + this->equalityConstraintsSSE += rhs.equalityConstraintsSSE; + this->inequalityConstraintsSSE += rhs.inequalityConstraintsSSE; + this->equalityLagrangian += rhs.equalityLagrangian; + this->inequalityLagrangian += rhs.inequalityLagrangian; + return *this; +} + +PerformanceIndex& PerformanceIndex::operator*=(const scalar_t c) { + this->merit *= c; + this->cost *= c; + this->dualFeasibilitiesSSE *= c; + this->dynamicsViolationSSE *= c; + this->equalityConstraintsSSE *= c; + this->inequalityConstraintsSSE *= c; + this->equalityLagrangian *= c; + this->inequalityLagrangian *= c; + return *this; +} + +bool PerformanceIndex::isApprox(const PerformanceIndex& other, const scalar_t prec) const { + return numerics::almost_eq(this->merit, other.merit, prec) && numerics::almost_eq(this->cost, other.cost, prec) && + numerics::almost_eq(this->dualFeasibilitiesSSE, other.dualFeasibilitiesSSE, prec) && + numerics::almost_eq(this->dynamicsViolationSSE, other.dynamicsViolationSSE, prec) && + numerics::almost_eq(this->equalityConstraintsSSE, other.equalityConstraintsSSE, prec) && + numerics::almost_eq(this->inequalityConstraintsSSE, other.inequalityConstraintsSSE, prec) && + numerics::almost_eq(this->equalityLagrangian, other.equalityLagrangian, prec) && + numerics::almost_eq(this->inequalityLagrangian, other.inequalityLagrangian, prec); +} + +void swap(PerformanceIndex& lhs, PerformanceIndex& rhs) { + std::swap(lhs.merit, rhs.merit); + std::swap(lhs.cost, rhs.cost); + std::swap(lhs.dualFeasibilitiesSSE, rhs.dualFeasibilitiesSSE); + std::swap(lhs.dynamicsViolationSSE, rhs.dynamicsViolationSSE); + std::swap(lhs.equalityConstraintsSSE, rhs.equalityConstraintsSSE); + std::swap(lhs.inequalityConstraintsSSE, rhs.inequalityConstraintsSSE); + std::swap(lhs.equalityLagrangian, rhs.equalityLagrangian); + std::swap(lhs.inequalityLagrangian, rhs.inequalityLagrangian); +} + +PerformanceIndex toPerformanceIndex(const Metrics& m) { + PerformanceIndex performanceIndex; + performanceIndex.merit = 0.0; // left for the solver to fill + performanceIndex.cost = m.cost; + performanceIndex.dualFeasibilitiesSSE = 0.0; // left for the solver to fill + performanceIndex.dynamicsViolationSSE = getEqConstraintsSSE(m.dynamicsViolation); + performanceIndex.equalityConstraintsSSE = getEqConstraintsSSE(m.stateEqConstraint) + getEqConstraintsSSE(m.stateInputEqConstraint); + performanceIndex.inequalityConstraintsSSE = + getIneqConstraintsSSE(m.stateIneqConstraint) + getIneqConstraintsSSE(m.stateInputIneqConstraint); + performanceIndex.equalityLagrangian = sumPenalties(m.stateEqLagrangian) + sumPenalties(m.stateInputEqLagrangian); + performanceIndex.inequalityLagrangian = sumPenalties(m.stateIneqLagrangian) + sumPenalties(m.stateInputIneqLagrangian); + return performanceIndex; +} + +PerformanceIndex toPerformanceIndex(const Metrics& m, const scalar_t dt) { + auto performanceIndex = toPerformanceIndex(m); + // performanceIndex.cost *= dt no need since it is already considered in multiple_shooting::computeIntermediateMetrics() + performanceIndex.dualFeasibilitiesSSE *= dt; + performanceIndex.dynamicsViolationSSE *= dt; + performanceIndex.equalityConstraintsSSE *= dt; + performanceIndex.inequalityConstraintsSSE *= dt; + return performanceIndex; +} + +std::ostream& operator<<(std::ostream& stream, const PerformanceIndex& performanceIndex) { + const size_t tabSpace = 12; + const auto indentation = stream.width(); + stream << std::left; // fill from left + + stream << std::setw(indentation) << ""; + stream << "Rollout Merit: " << std::setw(tabSpace) << performanceIndex.merit; + stream << "Rollout Cost: " << std::setw(tabSpace) << performanceIndex.cost << '\n'; + + stream << std::setw(indentation) << ""; + stream << "Dual feasibilities SSE: " << std::setw(tabSpace) << performanceIndex.dualFeasibilitiesSSE; + stream << "Dynamics violation SSE: " << std::setw(tabSpace) << performanceIndex.dynamicsViolationSSE << '\n'; + + stream << std::setw(indentation) << ""; + stream << "Equality constraints SSE: " << std::setw(tabSpace) << performanceIndex.equalityConstraintsSSE; + stream << "Inequality constraints SSE: " << std::setw(tabSpace) << performanceIndex.inequalityConstraintsSSE << '\n'; + + stream << std::setw(indentation) << ""; + stream << "Equality Lagrangian: " << std::setw(tabSpace) << performanceIndex.equalityLagrangian; + stream << "Inequality Lagrangian: " << std::setw(tabSpace) << performanceIndex.inequalityLagrangian; + + return stream; +} + +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp b/ocs2_oc/src/oc_data/TimeDiscretization.cpp similarity index 77% rename from ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp rename to ocs2_oc/src/oc_data/TimeDiscretization.cpp index 7f2605ab2..5b9f86ab3 100644 --- a/ocs2_sqp/ocs2_sqp/src/TimeDiscretization.cpp +++ b/ocs2_oc/src/oc_data/TimeDiscretization.cpp @@ -27,7 +27,7 @@ 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 "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_oc/oc_data/TimeDiscretization.h" #include @@ -113,4 +113,41 @@ std::vector timeDiscretizationWithEvents(scalar_t initTime, scala return timeDiscretizationWithDoubleEvents; } +scalar_array_t toTime(const std::vector& annotatedTime) { + scalar_array_t timeTrajectory; + timeTrajectory.reserve(annotatedTime.size()); + for (size_t i = 0; i < annotatedTime.size(); i++) { + timeTrajectory.push_back(annotatedTime[i].time); + } + return timeTrajectory; +} + +scalar_array_t toInterpolationTime(const std::vector& annotatedTime) { + if (annotatedTime.empty()) { + return scalar_array_t(); + } + scalar_array_t timeTrajectory; + timeTrajectory.reserve(annotatedTime.size()); + timeTrajectory.push_back(annotatedTime.back().time - numeric_traits::limitEpsilon()); + for (int i = 1; i < annotatedTime.size() - 1; i++) { + if (annotatedTime[i].event == AnnotatedTime::Event::PostEvent) { + timeTrajectory.push_back(getInterpolationTime(annotatedTime[i])); + } else { + timeTrajectory.push_back(annotatedTime[i].time); + } + } + timeTrajectory.push_back(annotatedTime.back().time - numeric_traits::limitEpsilon()); + return timeTrajectory; +} + +size_array_t toPostEventIndices(const std::vector& annotatedTime) { + size_array_t postEventIndices; + for (size_t i = 0; i < annotatedTime.size(); i++) { + if (annotatedTime[i].event == AnnotatedTime::Event::PreEvent) { + postEventIndices.push_back(i + 1); + } + } + return postEventIndices; +} + } // namespace ocs2 diff --git a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp index d21711fa1..6eb0cc2c9 100644 --- a/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/LoopshapingOptimalControlProblem.cpp @@ -47,6 +47,15 @@ OptimalControlProblem create(const OptimalControlProblem& problem, std::shared_p LoopshapingConstraint::create(*problem.preJumpEqualityConstraintPtr, loopshapingDefinition); augmentedProblem.finalEqualityConstraintPtr = LoopshapingConstraint::create(*problem.finalEqualityConstraintPtr, loopshapingDefinition); + // Inequality constraints + augmentedProblem.inequalityConstraintPtr = LoopshapingConstraint::create(*problem.inequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.stateInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.stateInequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.preJumpInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.preJumpInequalityConstraintPtr, loopshapingDefinition); + augmentedProblem.finalInequalityConstraintPtr = + LoopshapingConstraint::create(*problem.finalInequalityConstraintPtr, loopshapingDefinition); + // Lagrangians augmentedProblem.equalityLagrangianPtr = LoopshapingAugmentedLagrangian::create(*problem.equalityLagrangianPtr, loopshapingDefinition); augmentedProblem.stateEqualityLagrangianPtr = diff --git a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp b/ocs2_oc/src/oc_problem/OcpSize.cpp similarity index 97% rename from ocs2_sqp/hpipm_catkin/src/OcpSize.cpp rename to ocs2_oc/src/oc_problem/OcpSize.cpp index 53d5263fb..cd207aa19 100644 --- a/ocs2_sqp/hpipm_catkin/src/OcpSize.cpp +++ b/ocs2_oc/src/oc_problem/OcpSize.cpp @@ -27,11 +27,9 @@ 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 "hpipm_catkin/OcpSize.h" +#include "ocs2_oc/oc_problem/OcpSize.h" namespace ocs2 { -namespace hpipm_interface { - bool operator==(const OcpSize& lhs, const OcpSize& rhs) noexcept { // use && instead of &= to enable short-circuit evaluation bool same = lhs.numStages == rhs.numStages; @@ -71,5 +69,4 @@ OcpSize extractSizesFromProblem(const std::vector + +namespace ocs2 { +// Helper functions +namespace { +int getNumDecisionVariables(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), + std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0)); +} + +int getNumDynamicsConstraints(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0); +} + +int getNumGeneralEqualityConstraints(const OcpSize& ocpSize) { + return std::accumulate(ocpSize.numIneqConstraints.begin(), ocpSize.numIneqConstraints.end(), (int)0); +} +} // namespace + +void getConstraintMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector& dynamics, + const std::vector* constraintsPtr, const vector_array_t* scalingVectorsPtr, + VectorFunctionLinearApproximation& res) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[getConstraintMatrix] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[getConstraintMatrix] The size of scalingVectors doesn't match the number of stage."); + } + + // Preallocate full constraint matrix + auto& g = res.f; + auto& G = res.dfdx; + if (constraintsPtr == nullptr) { + g.setZero(getNumDynamicsConstraints(ocpSize)); + } else { + g.setZero(getNumDynamicsConstraints(ocpSize) + getNumGeneralEqualityConstraints(ocpSize)); + } + G.setZero(g.rows(), getNumDecisionVariables(ocpSize)); + + // Initial state constraint + const int nu_0 = ocpSize.numInputs.front(); + const int nx_1 = ocpSize.numStates[1]; + if (scalingVectorsPtr == nullptr) { + G.topLeftCorner(nx_1, nu_0 + nx_1) << -dynamics.front().dfdu, matrix_t::Identity(nx_1, nx_1); + } else { + G.topLeftCorner(nx_1, nu_0 + nx_1) << -dynamics.front().dfdu, scalingVectorsPtr->front().asDiagonal().toDenseMatrix(); + } + + // k = 0. Absorb initial state into dynamics + // The initial state is removed from the decision variables + // The first dynamics becomes: + // x[1] = A[0]*x[0] + B[0]*u[0] + b[0] + // = B[0]*u[0] + (b[0] + A[0]*x[0]) + // = B[0]*u[0] + \tilde{b}[0] + // numState[0] = 0 --> No need to specify A[0] here + g.head(nx_1) = dynamics.front().f; + g.head(nx_1).noalias() += dynamics.front().dfdx * x0; + + int currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const auto& dynamics_k = dynamics[k]; + // const auto& constraints_k = constraints; + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + const int nx_next = ocpSize.numStates[k + 1]; + + // Add [-A, -B, I(C)] + if (scalingVectorsPtr == nullptr) { + G.block(currRow, currCol, nx_next, nx_k + nu_k + nx_next) << -dynamics_k.dfdx, -dynamics_k.dfdu, matrix_t::Identity(nx_next, nx_next); + } else { + G.block(currRow, currCol, nx_next, nx_k + nu_k + nx_next) << -dynamics_k.dfdx, -dynamics_k.dfdu, + (*scalingVectorsPtr)[k].asDiagonal().toDenseMatrix(); + } + + // Add [b] + g.segment(currRow, nx_next) = dynamics_k.f; + + currRow += nx_next; + currCol += nx_k + nu_k; + } // end of i loop + + if (constraintsPtr != nullptr) { + currCol = nu_0; + // === Constraints === + // for ocs2 --> C*dx + D*du + e = 0 + // for pipg --> C*dx + D*du = -e + // Initial general constraints + const int nc_0 = ocpSize.numIneqConstraints.front(); + if (nc_0 > 0) { + const auto& constraint_0 = (*constraintsPtr).front(); + G.block(currRow, 0, nc_0, nu_0) = constraint_0.dfdu; + g.segment(currRow, nc_0) = -constraint_0.f; + g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; + currRow += nc_0; + } + + for (int k = 1; k < N; ++k) { + const int nc_k = ocpSize.numIneqConstraints[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + if (nc_k > 0) { + const auto& constraints_k = (*constraintsPtr)[k]; + + // Add [C, D, 0] + G.block(currRow, currCol, nc_k, nx_k + nu_k) << constraints_k.dfdx, constraints_k.dfdu; + // Add [-e] + g.segment(currRow, nc_k) = -constraints_k.f; + currRow += nc_k; + } + + currCol += nx_k + nu_k; + } + + // Final general constraint + const int nc_N = ocpSize.numIneqConstraints[N]; + if (nc_N > 0) { + const auto& constraints_N = (*constraintsPtr)[N]; + G.bottomRightCorner(nc_N, constraints_N.dfdx.cols()) = constraints_N.dfdx; + g.tail(nc_N) = -constraints_N.f; + } + } // end of i loop +} + +void getConstraintMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector& dynamics, + const std::vector* constraintsPtr, + const vector_array_t* scalingVectorsPtr, Eigen::SparseMatrix& G, vector_t& g) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[getConstraintMatrixSparse] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[getConstraintMatrixSparse] The size of scalingVectors doesn't match the number of stage."); + } + + const int nu_0 = ocpSize.numInputs[0]; + const int nx_1 = ocpSize.numStates[1]; + const int nx_N = ocpSize.numStates[N]; + + int nnz = nx_1 * (nu_0 + nx_1); + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nx_next = ocpSize.numStates[k + 1]; + nnz += nx_next * (nx_k + nu_k + nx_next); + } + + if (constraintsPtr != nullptr) { + const int nc_0 = ocpSize.numIneqConstraints[0]; + nnz += nc_0 * nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nc_k = ocpSize.numIneqConstraints[k]; + nnz += nc_k * (nx_k + nu_k); + } + const int nc_N = ocpSize.numIneqConstraints[N]; + + nnz += nc_N * nx_N; + } + std::vector> tripletList; + tripletList.reserve(nnz); + + auto emplaceBackMatrix = [&tripletList](const int startRow, const int startCol, const matrix_t& mat) { + for (int j = 0; j < mat.cols(); j++) { + for (int i = 0; i < mat.rows(); i++) { + if (mat(i, j) != 0) { + tripletList.emplace_back(startRow + i, startCol + j, mat(i, j)); + } + } + } + }; + + if (constraintsPtr == nullptr) { + g.setZero(getNumDynamicsConstraints(ocpSize)); + } else { + g.setZero(getNumDynamicsConstraints(ocpSize) + getNumGeneralEqualityConstraints(ocpSize)); + } + G.resize(g.rows(), getNumDecisionVariables(ocpSize)); + + // Initial state constraint + emplaceBackMatrix(0, 0, -dynamics.front().dfdu); + if (scalingVectorsPtr == nullptr) { + emplaceBackMatrix(0, nu_0, matrix_t::Identity(nx_1, nx_1)); + } else { + emplaceBackMatrix(0, nu_0, scalingVectorsPtr->front().asDiagonal()); + } + + // k = 0. Absorb initial state into dynamics + // The initial state is removed from the decision variables + // The first dynamics becomes: + // x[1] = A[0]*x[0] + B[0]*u[0] + b[0] + // = B[0]*u[0] + (b[0] + A[0]*x[0]) + // = B[0]*u[0] + \tilde{b}[0] + // numState[0] = 0 --> No need to specify A[0] here + g.head(nx_1) = dynamics.front().f; + g.head(nx_1).noalias() += dynamics.front().dfdx * x0; + + int currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const auto& dynamics_k = dynamics[k]; + // const auto& constraints_k = constraints; + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nx_next = ocpSize.numStates[k + 1]; + + // Add [-A, -B, I] + emplaceBackMatrix(currRow, currCol, -dynamics_k.dfdx); + emplaceBackMatrix(currRow, currCol + nx_k, -dynamics_k.dfdu); + if (scalingVectorsPtr == nullptr) { + emplaceBackMatrix(currRow, currCol + nx_k + nu_k, matrix_t::Identity(nx_next, nx_next)); + } else { + emplaceBackMatrix(currRow, currCol + nx_k + nu_k, (*scalingVectorsPtr)[k].asDiagonal()); + } + + // Add [b] + g.segment(currRow, nx_next) = dynamics_k.f; + + currRow += nx_next; + currCol += nx_k + nu_k; + } + + if (constraintsPtr != nullptr) { + currCol = nu_0; + // === Constraints === + // for ocs2 --> C*dx + D*du + e = 0 + // for pipg --> C*dx + D*du = -e + // Initial general constraints + const int nc_0 = ocpSize.numIneqConstraints.front(); + if (nc_0 > 0) { + const auto& constraint_0 = (*constraintsPtr).front(); + emplaceBackMatrix(currRow, 0, constraint_0.dfdu); + + g.segment(currRow, nc_0) = -constraint_0.f; + g.segment(currRow, nc_0) -= constraint_0.dfdx * x0; + currRow += nc_0; + } + + for (int k = 1; k < N; ++k) { + const int nc_k = ocpSize.numIneqConstraints[k]; + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + if (nc_k > 0) { + const auto& constraints_k = (*constraintsPtr)[k]; + + // Add [C, D, 0] + emplaceBackMatrix(currRow, currCol, constraints_k.dfdx); + emplaceBackMatrix(currRow, currCol + nx_k, constraints_k.dfdu); + // Add [-e] + g.segment(currRow, nc_k) = -constraints_k.f; + currRow += nc_k; + } + + currCol += nx_k + nu_k; + } + + // Final general constraint + const int nc_N = ocpSize.numIneqConstraints[N]; + if (nc_N > 0) { + const auto& constraints_N = (*constraintsPtr)[N]; + emplaceBackMatrix(currRow, currCol, constraints_N.dfdx); + g.segment(currRow, nc_N) = -constraints_N.f; + } + } + + G.setFromTriplets(tripletList.begin(), tripletList.end()); + assert(G.nonZeros() <= nnz); +} + +void getCostMatrix(const OcpSize& ocpSize, const vector_t& x0, const std::vector& cost, + ScalarFunctionQuadraticApproximation& res) { + const int N = ocpSize.numStages; + + // Preallocate full Cost matrices + auto& H = res.dfdxx; + auto& h = res.dfdx; + auto& c = res.f; + H.setZero(getNumDecisionVariables(ocpSize), getNumDecisionVariables(ocpSize)); + h.setZero(H.cols()); + c = 0.0; + + // k = 0. Elimination of initial state requires cost adaptation + vector_t r_0 = cost[0].dfdu; + r_0 += cost[0].dfdux * x0; + + const int nu_0 = ocpSize.numInputs[0]; + H.topLeftCorner(nu_0, nu_0) = cost[0].dfduu; + h.head(nu_0) = r_0; + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + + // Add [ Q, P' + // P, Q ] + H.block(currRow, currRow, nx_k + nu_k, nx_k + nu_k) << cost[k].dfdxx, cost[k].dfdux.transpose(), cost[k].dfdux, cost[k].dfduu; + + // Add [ q, r] + h.segment(currRow, nx_k + nu_k) << cost[k].dfdx, cost[k].dfdu; + + // Add nominal cost + c += cost[k].f; + + currRow += nx_k + nu_k; + } + + const int nx_N = ocpSize.numStates[N]; + H.bottomRightCorner(nx_N, nx_N) = cost[N].dfdxx; + h.tail(nx_N) = cost[N].dfdx; + c += cost[N].f; +} + +void getCostMatrixSparse(const OcpSize& ocpSize, const vector_t& x0, const std::vector& cost, + Eigen::SparseMatrix& H, vector_t& h) { + const int N = ocpSize.numStages; + + const int nu_0 = ocpSize.numInputs[0]; + int nnz = nu_0 * nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + nnz += (nx_k + nu_k) * (nx_k + nu_k); + } + const int nx_N = ocpSize.numStates[N]; + + nnz += nx_N * nx_N; + std::vector> tripletList; + tripletList.reserve(nnz); + + auto emplaceBackMatrix = [&tripletList](const int startRow, const int startCol, const matrix_t& mat) { + for (int j = 0; j < mat.cols(); j++) { + for (int i = 0; i < mat.rows(); i++) { + if (mat(i, j) != 0) { + tripletList.emplace_back(startRow + i, startCol + j, mat(i, j)); + } + } + } + }; + + h.setZero(getNumDecisionVariables(ocpSize)); + + // k = 0. Elimination of initial state requires cost adaptation + vector_t r_0 = cost[0].dfdu; + r_0 += cost[0].dfdux * x0; + + // R0 + emplaceBackMatrix(0, 0, cost[0].dfduu); + h.head(nu_0) = r_0; + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + + // Add [ Q, P' + // P, Q ] + emplaceBackMatrix(currRow, currRow, cost[k].dfdxx); + emplaceBackMatrix(currRow, currRow + nx_k, cost[k].dfdux.transpose()); + emplaceBackMatrix(currRow + nx_k, currRow, cost[k].dfdux); + emplaceBackMatrix(currRow + nx_k, currRow + nx_k, cost[k].dfduu); + + // Add [ q, r] + h.segment(currRow, nx_k + nu_k) << cost[k].dfdx, cost[k].dfdu; + + currRow += nx_k + nu_k; + } + + emplaceBackMatrix(currRow, currRow, cost[N].dfdxx); + h.tail(nx_N) = cost[N].dfdx; + + H.resize(getNumDecisionVariables(ocpSize), getNumDecisionVariables(ocpSize)); + H.setFromTriplets(tripletList.begin(), tripletList.end()); + assert(H.nonZeros() <= nnz); +} + +void toOcpSolution(const OcpSize& ocpSize, const vector_t& stackedSolution, const vector_t x0, vector_array_t& xTrajectory, + vector_array_t& uTrajectory) { + const int N = ocpSize.numStages; + xTrajectory.resize(N + 1); + uTrajectory.resize(N); + + xTrajectory.front() = x0; + + int curRow = 0; + for (int i = 0; i < N; i++) { + const auto nx = ocpSize.numStates[i + 1]; + const auto nu = ocpSize.numInputs[i]; + xTrajectory[i + 1] = stackedSolution.segment(curRow + nu, nx); + uTrajectory[i] = stackedSolution.segment(curRow, nu); + + curRow += nx + nu; + } +} + +void toKktSolution(const vector_array_t& xTrajectory, const vector_array_t& uTrajectory, vector_t& stackedSolution) { + int numDecisionVariables = 0; + for (int i = 1; i < xTrajectory.size(); i++) { + numDecisionVariables += uTrajectory[i - 1].size() + xTrajectory[i].size(); + } + + stackedSolution.resize(numDecisionVariables); + + int curRow = 0; + for (int i = 1; i < xTrajectory.size(); i++) { + const auto nu = uTrajectory[i - 1].size(); + const auto nx = xTrajectory[i].size(); + stackedSolution.segment(curRow, nx + nu) << uTrajectory[i - 1], xTrajectory[i]; + curRow += nx + nu; + } +} + +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp index 8ede3a4c4..53580c7e3 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblem.cpp @@ -50,6 +50,11 @@ OptimalControlProblem::OptimalControlProblem() stateEqualityConstraintPtr(new StateConstraintCollection), preJumpEqualityConstraintPtr(new StateConstraintCollection), finalEqualityConstraintPtr(new StateConstraintCollection), + /* Inequality constraints */ + inequalityConstraintPtr(new StateInputConstraintCollection), + stateInequalityConstraintPtr(new StateConstraintCollection), + preJumpInequalityConstraintPtr(new StateConstraintCollection), + finalInequalityConstraintPtr(new StateConstraintCollection), /* Lagrangians */ equalityLagrangianPtr(new StateInputAugmentedLagrangianCollection), stateEqualityLagrangianPtr(new StateAugmentedLagrangianCollection), @@ -82,6 +87,11 @@ OptimalControlProblem::OptimalControlProblem(const OptimalControlProblem& other) stateEqualityConstraintPtr(other.stateEqualityConstraintPtr->clone()), preJumpEqualityConstraintPtr(other.preJumpEqualityConstraintPtr->clone()), finalEqualityConstraintPtr(other.finalEqualityConstraintPtr->clone()), + /* Inequality constraints */ + inequalityConstraintPtr(other.inequalityConstraintPtr->clone()), + stateInequalityConstraintPtr(other.stateInequalityConstraintPtr->clone()), + preJumpInequalityConstraintPtr(other.preJumpInequalityConstraintPtr->clone()), + finalInequalityConstraintPtr(other.finalInequalityConstraintPtr->clone()), /* Lagrangians */ equalityLagrangianPtr(other.equalityLagrangianPtr->clone()), stateEqualityLagrangianPtr(other.stateEqualityLagrangianPtr->clone()), @@ -130,6 +140,12 @@ void OptimalControlProblem::swap(OptimalControlProblem& other) noexcept { preJumpEqualityConstraintPtr.swap(other.preJumpEqualityConstraintPtr); finalEqualityConstraintPtr.swap(other.finalEqualityConstraintPtr); + /* Inequality constraints */ + inequalityConstraintPtr.swap(other.inequalityConstraintPtr); + stateInequalityConstraintPtr.swap(other.stateInequalityConstraintPtr); + preJumpInequalityConstraintPtr.swap(other.preJumpInequalityConstraintPtr); + finalInequalityConstraintPtr.swap(other.finalInequalityConstraintPtr); + /* Lagrangians */ equalityLagrangianPtr.swap(other.equalityLagrangianPtr); stateEqualityLagrangianPtr.swap(other.stateEqualityLagrangianPtr); diff --git a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp index bc88c40ff..75e44d649 100644 --- a/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp +++ b/ocs2_oc/src/oc_problem/OptimalControlProblemHelperFunction.cpp @@ -64,11 +64,11 @@ void initializeDualSolution(const OptimalControlProblem& ocp, const PrimalSoluti for (size_t i = 0; i < primalSolution.postEventIndices_.size(); i++) { const auto cachedTimeIndex = cacheEventIndexBias + i; const auto& time = primalSolution.timeTrajectory_[i]; - auto& multiplierCollection = dualSolution.preJumps[i]; + auto& multipliers = dualSolution.preJumps[i]; if (cachedTimeIndex < cachedDualSolution.preJumps.size()) { - multiplierCollection = cachedDualSolution.preJumps[cachedTimeIndex]; + multipliers = cachedDualSolution.preJumps[cachedTimeIndex]; } else { - initializePreJumpMultiplierCollection(ocp, time, multiplierCollection); + initializePreJumpMultiplierCollection(ocp, time, multipliers); } } } @@ -77,11 +77,11 @@ void initializeDualSolution(const OptimalControlProblem& ocp, const PrimalSoluti dualSolution.intermediates.resize(primalSolution.timeTrajectory_.size()); for (size_t i = 0; i < primalSolution.timeTrajectory_.size(); i++) { const auto& time = primalSolution.timeTrajectory_[i]; - auto& multiplierCollection = dualSolution.intermediates[i]; + auto& multipliers = dualSolution.intermediates[i]; if (interpolatableTimePeriod.first <= time && time <= interpolatableTimePeriod.second) { - multiplierCollection = getIntermediateDualSolutionAtTime(cachedDualSolution, time); + multipliers = getIntermediateDualSolutionAtTime(cachedDualSolution, time); } else { - initializeIntermediateMultiplierCollection(ocp, time, multiplierCollection); + initializeIntermediateMultiplierCollection(ocp, time, multipliers); } } } @@ -89,28 +89,27 @@ void initializeDualSolution(const OptimalControlProblem& ocp, const PrimalSoluti /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void initializeFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multiplierCollection) { - ocp.finalEqualityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateEq); - ocp.finalInequalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateIneq); +void initializeFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multipliers) { + ocp.finalEqualityLagrangianPtr->initializeLagrangian(time, multipliers.stateEq); + ocp.finalInequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void initializePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multiplierCollection) { - ocp.preJumpEqualityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateEq); - ocp.preJumpInequalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateIneq); +void initializePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multipliers) { + ocp.preJumpEqualityLagrangianPtr->initializeLagrangian(time, multipliers.stateEq); + ocp.preJumpInequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void initializeIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, - MultiplierCollection& multiplierCollection) { - ocp.stateEqualityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateEq); - ocp.stateInequalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateIneq); - ocp.equalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateInputEq); - ocp.inequalityLagrangianPtr->initializeLagrangian(time, multiplierCollection.stateInputIneq); +void initializeIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, MultiplierCollection& multipliers) { + ocp.stateEqualityLagrangianPtr->initializeLagrangian(time, multipliers.stateEq); + ocp.stateInequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateIneq); + ocp.equalityLagrangianPtr->initializeLagrangian(time, multipliers.stateInputEq); + ocp.inequalityLagrangianPtr->initializeLagrangian(time, multipliers.stateInputIneq); } /******************************************************************************************************/ @@ -122,9 +121,9 @@ void updateDualSolution(const OptimalControlProblem& ocp, const PrimalSolution& if (!primalSolution.timeTrajectory_.empty()) { const auto& time = primalSolution.timeTrajectory_.back(); const auto& state = primalSolution.stateTrajectory_.back(); - auto& metricsCollection = problemMetrics.final; - auto& multiplierCollection = dualSolution.final; - updateFinalMultiplierCollection(ocp, time, state, metricsCollection, multiplierCollection); + auto& metrics = problemMetrics.final; + auto& multipliers = dualSolution.final; + updateFinalMultiplierCollection(ocp, time, state, metrics, multipliers); } // preJump @@ -134,9 +133,9 @@ void updateDualSolution(const OptimalControlProblem& ocp, const PrimalSolution& const auto timeIndex = primalSolution.postEventIndices_[i] - 1; const auto& time = primalSolution.timeTrajectory_[timeIndex]; const auto& state = primalSolution.stateTrajectory_[timeIndex]; - auto& metricsCollection = problemMetrics.preJumps[i]; - auto& multiplierCollection = dualSolution.preJumps[i]; - updatePreJumpMultiplierCollection(ocp, time, state, metricsCollection, multiplierCollection); + auto& metrics = problemMetrics.preJumps[i]; + auto& multipliers = dualSolution.preJumps[i]; + updatePreJumpMultiplierCollection(ocp, time, state, metrics, multipliers); } // intermediates @@ -146,54 +145,68 @@ void updateDualSolution(const OptimalControlProblem& ocp, const PrimalSolution& const auto& time = primalSolution.timeTrajectory_[i]; const auto& state = primalSolution.stateTrajectory_[i]; const auto& input = primalSolution.inputTrajectory_[i]; - auto& metricsCollection = problemMetrics.intermediates[i]; - auto& multiplierCollection = dualSolution.intermediates[i]; - updateIntermediateMultiplierCollection(ocp, time, state, input, metricsCollection, multiplierCollection); + auto& metrics = problemMetrics.intermediates[i]; + auto& multipliers = dualSolution.intermediates[i]; + updateIntermediateMultiplierCollection(ocp, time, state, input, metrics, multipliers); } } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void updateFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection) { - ocp.finalEqualityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateEqLagrangian, multiplierCollection.stateEq); - ocp.finalInequalityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateIneqLagrangian, multiplierCollection.stateIneq); +void updateFinalMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, Metrics& metrics, + MultiplierCollection& multipliers) { + ocp.finalEqualityLagrangianPtr->updateLagrangian(time, state, metrics.stateEqLagrangian, multipliers.stateEq); + ocp.finalInequalityLagrangianPtr->updateLagrangian(time, state, metrics.stateIneqLagrangian, multipliers.stateIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -void updatePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection) { - ocp.preJumpEqualityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateEqLagrangian, multiplierCollection.stateEq); - ocp.preJumpInequalityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateIneqLagrangian, multiplierCollection.stateIneq); +void updatePreJumpMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, Metrics& metrics, + MultiplierCollection& multipliers) { + ocp.preJumpEqualityLagrangianPtr->updateLagrangian(time, state, metrics.stateEqLagrangian, multipliers.stateEq); + ocp.preJumpInequalityLagrangianPtr->updateLagrangian(time, state, metrics.stateIneqLagrangian, multipliers.stateIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ void updateIntermediateMultiplierCollection(const OptimalControlProblem& ocp, scalar_t time, const vector_t& state, const vector_t& input, - MetricsCollection& metricsCollection, MultiplierCollection& multiplierCollection) { - ocp.stateEqualityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateEqLagrangian, multiplierCollection.stateEq); - ocp.stateInequalityLagrangianPtr->updateLagrangian(time, state, metricsCollection.stateIneqLagrangian, multiplierCollection.stateIneq); - ocp.equalityLagrangianPtr->updateLagrangian(time, state, input, metricsCollection.stateInputEqLagrangian, - multiplierCollection.stateInputEq); - ocp.inequalityLagrangianPtr->updateLagrangian(time, state, input, metricsCollection.stateInputIneqLagrangian, - multiplierCollection.stateInputIneq); + Metrics& metrics, MultiplierCollection& multipliers) { + ocp.stateEqualityLagrangianPtr->updateLagrangian(time, state, metrics.stateEqLagrangian, multipliers.stateEq); + ocp.stateInequalityLagrangianPtr->updateLagrangian(time, state, metrics.stateIneqLagrangian, multipliers.stateIneq); + ocp.equalityLagrangianPtr->updateLagrangian(time, state, input, metrics.stateInputEqLagrangian, multipliers.stateInputEq); + ocp.inequalityLagrangianPtr->updateLagrangian(time, state, input, metrics.stateInputIneqLagrangian, multipliers.stateInputIneq); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -const LagrangianMetrics* extractFinalTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const MetricsCollection& metricsColl) { +const vector_t* extractFinalTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const Metrics& metrics) { + size_t index; + if (ocp.finalEqualityConstraintPtr->getTermIndex(name, index)) { + return &metrics.stateEqConstraint[index]; + + } else if (ocp.finalInequalityConstraintPtr->getTermIndex(name, index)) { + return &metrics.stateIneqConstraint[index]; + + } else { + return nullptr; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +const LagrangianMetrics* extractFinalTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const Metrics& metrics) { size_t index; if (ocp.finalEqualityLagrangianPtr->getTermIndex(name, index)) { - return &metricsColl.stateEqLagrangian[index]; + return &metrics.stateEqLagrangian[index]; } else if (ocp.finalInequalityLagrangianPtr->getTermIndex(name, index)) { - return &metricsColl.stateIneqLagrangian[index]; + return &metrics.stateIneqLagrangian[index]; } else { return nullptr; @@ -203,23 +216,91 @@ const LagrangianMetrics* extractFinalTermMetrics(const OptimalControlProblem& oc /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool extractPreJumpTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector& metricsCollArray, - std::vector& metricsArray) { - metricsArray.clear(); +bool extractPreJumpTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const std::vector& metricsArray, + std::vector>& constraintArray) { + constraintArray.clear(); + + size_t index; + if (ocp.preJumpEqualityConstraintPtr->getTermIndex(name, index)) { + constraintArray.reserve(metricsArray.size()); + for (const auto& m : metricsArray) { + constraintArray.emplace_back(m.stateEqConstraint[index]); + } + return true; + + } else if (ocp.preJumpInequalityConstraintPtr->getTermIndex(name, index)) { + constraintArray.reserve(metricsArray.size()); + for (const auto& m : metricsArray) { + constraintArray.emplace_back(m.stateIneqConstraint[index]); + } + return true; + + } else { + return false; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool extractPreJumpTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const std::vector& metricsArray, + std::vector& lagrangianMetricsArray) { + lagrangianMetricsArray.clear(); size_t index; if (ocp.preJumpEqualityLagrangianPtr->getTermIndex(name, index)) { - metricsArray.reserve(metricsCollArray.size()); - for (const auto& metricsColl : metricsCollArray) { - metricsArray.push_back(metricsColl.stateEqLagrangian[index]); + lagrangianMetricsArray.reserve(metricsArray.size()); + for (const auto& m : metricsArray) { + lagrangianMetricsArray.push_back(m.stateEqLagrangian[index]); } return true; } else if (ocp.preJumpInequalityLagrangianPtr->getTermIndex(name, index)) { - metricsArray.reserve(metricsCollArray.size()); - for (const auto& metricsColl : metricsCollArray) { - metricsArray.push_back(metricsColl.stateIneqLagrangian[index]); + lagrangianMetricsArray.reserve(metricsArray.size()); + for (const auto& m : metricsArray) { + lagrangianMetricsArray.push_back(m.stateIneqLagrangian[index]); + } + return true; + + } else { + return false; + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +bool extractIntermediateTermConstraint(const OptimalControlProblem& ocp, const std::string& name, const std::vector& metricsTraj, + std::vector>& constraintTraj) { + constraintTraj.clear(); + + size_t index; + if (ocp.equalityConstraintPtr->getTermIndex(name, index)) { + constraintTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + constraintTraj.emplace_back(m.stateInputEqConstraint[index]); + } + return true; + + } else if (ocp.stateEqualityConstraintPtr->getTermIndex(name, index)) { + constraintTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + constraintTraj.emplace_back(m.stateEqConstraint[index]); + } + return true; + + } else if (ocp.inequalityConstraintPtr->getTermIndex(name, index)) { + constraintTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + constraintTraj.emplace_back(m.stateInputIneqConstraint[index]); + } + return true; + + } else if (ocp.stateInequalityConstraintPtr->getTermIndex(name, index)) { + constraintTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + constraintTraj.emplace_back(m.stateIneqConstraint[index]); } return true; @@ -231,37 +312,37 @@ bool extractPreJumpTermMetrics(const OptimalControlProblem& ocp, const std::stri /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -bool extractIntermediateTermMetrics(const OptimalControlProblem& ocp, const std::string& name, - const std::vector& metricsCollTraj, - std::vector& metricsTrajectory) { - metricsTrajectory.clear(); +bool extractIntermediateTermLagrangianMetrics(const OptimalControlProblem& ocp, const std::string& name, + const std::vector& metricsTraj, + std::vector& lagrangianMetricsTraj) { + lagrangianMetricsTraj.clear(); size_t index; if (ocp.equalityLagrangianPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.push_back(metricsColl.stateInputEqLagrangian[index]); + lagrangianMetricsTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + lagrangianMetricsTraj.push_back(m.stateInputEqLagrangian[index]); } return true; } else if (ocp.stateEqualityLagrangianPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.push_back(metricsColl.stateEqLagrangian[index]); + lagrangianMetricsTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + lagrangianMetricsTraj.push_back(m.stateEqLagrangian[index]); } return true; } else if (ocp.inequalityLagrangianPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.push_back(metricsColl.stateInputIneqLagrangian[index]); + lagrangianMetricsTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + lagrangianMetricsTraj.push_back(m.stateInputIneqLagrangian[index]); } return true; } else if (ocp.stateInequalityLagrangianPtr->getTermIndex(name, index)) { - metricsTrajectory.reserve(metricsCollTraj.size()); - for (const auto& metricsColl : metricsCollTraj) { - metricsTrajectory.push_back(metricsColl.stateIneqLagrangian[index]); + lagrangianMetricsTraj.reserve(metricsTraj.size()); + for (const auto& m : metricsTraj) { + lagrangianMetricsTraj.push_back(m.stateIneqLagrangian[index]); } return true; diff --git a/ocs2_oc/src/oc_solver/SolverBase.cpp b/ocs2_oc/src/oc_solver/SolverBase.cpp index 3d22ddcf6..127038062 100644 --- a/ocs2_oc/src/oc_solver/SolverBase.cpp +++ b/ocs2_oc/src/oc_solver/SolverBase.cpp @@ -102,14 +102,17 @@ void SolverBase::preRun(scalar_t initTime, const vector_t& initState, scalar_t f /******************************************************************************************************/ /******************************************************************************************************/ void SolverBase::postRun() { - if (!synchronizedModules_.empty() || !augmentedLagrangianObservers_.empty()) { + if (!synchronizedModules_.empty() || !solverObservers_.empty()) { const auto solution = primalSolution(getFinalTime()); for (auto& module : synchronizedModules_) { module->postSolverRun(solution); } - for (auto& observer : augmentedLagrangianObservers_) { - observer->extractTermMetrics(getOptimalControlProblem(), solution, getSolutionMetrics()); - observer->extractTermMultipliers(getOptimalControlProblem(), getDualSolution()); + for (auto& observer : solverObservers_) { + observer->extractTermConstraint(getOptimalControlProblem(), solution, getSolutionMetrics()); + observer->extractTermLagrangianMetrics(getOptimalControlProblem(), solution, getSolutionMetrics()); + if (getDualSolution() != nullptr) { + observer->extractTermMultipliers(getOptimalControlProblem(), *getDualSolution()); + } } } } diff --git a/ocs2_oc/src/precondition/Ruzi.cpp b/ocs2_oc/src/precondition/Ruzi.cpp new file mode 100644 index 000000000..3f7cb6e59 --- /dev/null +++ b/ocs2_oc/src/precondition/Ruzi.cpp @@ -0,0 +1,461 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/precondition/Ruzi.h" + +#include +#include +#include + +namespace ocs2 { +namespace precondition { + +// Internal helper functions +namespace { + +scalar_t limitScaling(const scalar_t& v) { + if (v < 1e-4) { + return 1.0; + } else if (v > 1e+4) { + return 1e+4; + } else { + return v; + } +} + +template +vector_t matrixInfNormRows(const Eigen::MatrixBase& mat) { + if (mat.rows() == 0 || mat.cols() == 0) { + return vector_t(0); + } else { + return mat.rowwise().template lpNorm(); + } +} + +template +vector_t matrixInfNormRows(const Eigen::MatrixBase& mat, const Eigen::MatrixBase&... rest) { + vector_t temp = matrixInfNormRows(rest...); + if (mat.rows() == 0 || mat.cols() == 0) { + return temp; + } else if (temp.rows() != 0) { + return mat.rowwise().template lpNorm().cwiseMax(temp); + } else { + return mat.rowwise().template lpNorm(); + } +} + +template +vector_t matrixInfNormCols(const Eigen::MatrixBase& mat) { + if (mat.rows() == 0 || mat.cols() == 0) { + return vector_t(0); + } else { + return mat.colwise().template lpNorm().transpose(); + } +} + +template +vector_t matrixInfNormCols(const Eigen::MatrixBase& mat, const Eigen::MatrixBase&... rest) { + vector_t temp = matrixInfNormCols(rest...); + if (mat.rows() == 0 || mat.cols() == 0) { + return temp; + } else if (temp.rows() != 0) { + return mat.colwise().template lpNorm().transpose().cwiseMax(temp); + } else { + return mat.colwise().template lpNorm().transpose(); + } +} + +template +void scaleMatrixInPlace(const vector_t* rowScale, const vector_t* colScale, Eigen::MatrixBase& mat) { + if (rowScale != nullptr) { + mat.array().colwise() *= rowScale->array(); + } + if (colScale != nullptr) { + mat *= colScale->asDiagonal(); + } +} + +void invSqrtInfNormInParallel(ThreadPool& threadPool, const std::vector& dynamics, + const std::vector& cost, const vector_array_t& scalingVectors, + vector_array_t& D, vector_array_t& E) { + // Helper function + auto invSqrt = [](const vector_t& v) -> vector_t { return v.unaryExpr(std::ref(limitScaling)).array().sqrt().inverse(); }; + + // resize + const int N = static_cast(cost.size()) - 1; + E.resize(N); + D.resize(2 * N); + + D[0] = invSqrt(matrixInfNormCols(cost[0].dfduu, dynamics[0].dfdu)); + E[0] = invSqrt(matrixInfNormRows(dynamics[0].dfdu, scalingVectors[0])); + + std::atomic_int timeStamp{1}; + auto task = [&](int workerId) { + int k; + while ((k = timeStamp++) < N) { + D[2 * k - 1] = invSqrt(matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux, scalingVectors[k - 1].transpose().eval(), dynamics[k].dfdx)); + D[2 * k] = invSqrt(matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu, dynamics[k].dfdu)); + E[k] = invSqrt(matrixInfNormRows(dynamics[k].dfdx, dynamics[k].dfdu, scalingVectors[k])); + } + }; + threadPool.runParallel(std::move(task), threadPool.numThreads() + 1U); + + D[2 * N - 1] = invSqrt(matrixInfNormCols(cost[N].dfdxx, scalingVectors[N - 1].transpose().eval())); +} + +void scaleDataOneStepInPlaceInParallel(ThreadPool& threadPool, const vector_array_t& D, const vector_array_t& E, + std::vector& dynamics, + std::vector& cost, std::vector& scalingVectors) { + // cost at 0 + scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdu); + scaleMatrixInPlace(&(D[0]), &(D[0]), cost.front().dfduu); + scaleMatrixInPlace(&(D[0]), nullptr, cost.front().dfdux); + // constraints at 0 + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].f); + scaleMatrixInPlace(&(E[0]), nullptr, dynamics[0].dfdx); + scalingVectors[0].array() *= E[0].array() * D[1].array(); + if (dynamics[0].dfdu.size() > 0) { + scaleMatrixInPlace(&(E[0]), &(D[0]), dynamics[0].dfdu); + } + + std::atomic_int timeStamp1{1}, timeStamp2{1}; + auto scaleCostConstraints = [&](int workerId) { + int k; + const int N = static_cast(cost.size()) - 1; + // cost + while ((k = timeStamp1++) <= N) { + scaleMatrixInPlace(&(D[2 * k - 1]), nullptr, cost[k].dfdx); + scaleMatrixInPlace(&(D[2 * k - 1]), &(D[2 * k - 1]), cost[k].dfdxx); + if (cost[k].dfdu.size() > 0) { + scaleMatrixInPlace(&(D[2 * k]), nullptr, cost[k].dfdu); + scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k]), cost[k].dfduu); + scaleMatrixInPlace(&(D[2 * k]), &(D[2 * k - 1]), cost[k].dfdux); + } + } + // constraints + while ((k = timeStamp2++) < N) { + scaleMatrixInPlace(&(E[k]), nullptr, dynamics[k].f); + scaleMatrixInPlace(&(E[k]), &(D[2 * k - 1]), dynamics[k].dfdx); + scalingVectors[k].array() *= E[k].array() * D[2 * k + 1].array(); + if (dynamics[k].dfdu.size() > 0) { + scaleMatrixInPlace(&(E[k]), &(D[2 * k]), dynamics[k].dfdu); + } + } + }; + threadPool.runParallel(std::move(scaleCostConstraints), threadPool.numThreads() + 1U); +} + +vector_t matrixInfNormRows(const Eigen::SparseMatrix& mat) { + vector_t infNorm; + infNorm.setZero(mat.rows()); + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix::InnerIterator it(mat, j); it; ++it) { + int i = it.row(); + infNorm(i) = std::max(infNorm(i), std::abs(it.value())); + } + } + return infNorm; +} + +vector_t matrixInfNormCols(const Eigen::SparseMatrix& mat) { + vector_t infNorm; + infNorm.setZero(mat.cols()); + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix::InnerIterator it(mat, j); it; ++it) { + infNorm(j) = std::max(infNorm(j), std::abs(it.value())); + } + } + return infNorm; +} + +void scaleMatrixInPlace(const vector_t& rowScale, const vector_t& colScale, Eigen::SparseMatrix& mat) { + for (int j = 0; j < mat.outerSize(); ++j) { + for (Eigen::SparseMatrix::InnerIterator it(mat, j); it; ++it) { + if (it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1) { + throw std::runtime_error("[scaleMatrixInPlace] it.row() > rowScale.size() - 1 || it.col() > colScale.size() - 1"); + } + it.valueRef() *= rowScale(it.row()) * colScale(j); + } + } +} + +} // anonymous namespace + +void ocpDataInPlaceInParallel(ThreadPool& threadPool, const vector_t& x0, const OcpSize& ocpSize, const int iteration, + std::vector& dynamics, + std::vector& cost, vector_array_t& DOut, vector_array_t& EOut, + vector_array_t& scalingVectors, scalar_t& cOut) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[precondition::ocpDataInPlaceInParallel] The number of stages cannot be less than 1."); + } + + // Init output + cOut = 1.0; + DOut.resize(2 * N); + EOut.resize(N); + scalingVectors.resize(N); + for (int i = 0; i < N; i++) { + DOut[2 * i].setOnes(ocpSize.numInputs[i]); + DOut[2 * i + 1].setOnes(ocpSize.numStates[i + 1]); + EOut[i].setOnes(ocpSize.numStates[i + 1]); + scalingVectors[i].setOnes(ocpSize.numStates[i + 1]); + } + + const auto numDecisionVariables = std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), 0) + + std::accumulate(std::next(ocpSize.numStates.begin()), ocpSize.numStates.end(), 0); + + vector_array_t D(2 * N), E(N); + std::atomic_int timeIndex{0}; + const size_t numWorkers = threadPool.numThreads() + 1U; + for (int i = 0; i < iteration; i++) { + invSqrtInfNormInParallel(threadPool, dynamics, cost, scalingVectors, D, E); + scaleDataOneStepInPlaceInParallel(threadPool, D, E, dynamics, cost, scalingVectors); + + timeIndex = 0; + scalar_array_t infNormOfhArray(numWorkers, 0.0); + scalar_array_t sumOfInfNormOfHArray(numWorkers, 0.0); + auto infNormOfh_sumOfInfNormOfH = [&](int workerId) { + scalar_t workerInfNormOfh = 0.0; + scalar_t workerSumOfInfNormOfH = 0.0; + + int k = timeIndex++; + if (k == 0) { // Only one worker will execute this + workerInfNormOfh = (cost[0].dfdu + cost[0].dfdux * x0).lpNorm(); + workerSumOfInfNormOfH = matrixInfNormCols(cost[0].dfduu).derived().sum(); + k = timeIndex++; + } + + while (k <= N) { + workerInfNormOfh = std::max(workerInfNormOfh, cost[k].dfdx.lpNorm()); + workerInfNormOfh = std::max(workerInfNormOfh, cost[k].dfdu.lpNorm()); + workerSumOfInfNormOfH += matrixInfNormCols(cost[k].dfdxx, cost[k].dfdux).derived().sum(); + workerSumOfInfNormOfH += matrixInfNormCols(cost[k].dfdux.transpose().eval(), cost[k].dfduu).derived().sum(); + k = timeIndex++; + } + + infNormOfhArray[workerId] = std::max(infNormOfhArray[workerId], workerInfNormOfh); + sumOfInfNormOfHArray[workerId] += workerSumOfInfNormOfH; + }; + threadPool.runParallel(std::move(infNormOfh_sumOfInfNormOfH), numWorkers); + + const auto infNormOfh = *std::max_element(infNormOfhArray.cbegin(), infNormOfhArray.cend()); + const auto sumOfInfNormOfH = std::accumulate(sumOfInfNormOfHArray.cbegin(), sumOfInfNormOfHArray.cend(), 0.0); + const auto averageOfInfNormOfH = sumOfInfNormOfH / static_cast(numDecisionVariables); + const auto gamma = 1.0 / limitScaling(std::max(averageOfInfNormOfH, infNormOfh)); + + // compute EOut, DOut, and scale cost + timeIndex = 0; + auto computeDOutEOutScaleCost = [&](int workerId) { + int k = timeIndex++; + while (k < N) { + EOut[k].array() *= E[k].array(); + DOut[2 * k].array() *= D[2 * k].array(); + DOut[2 * k + 1].array() *= D[2 * k + 1].array(); + // cost + cost[k].dfdxx *= gamma; + cost[k].dfduu *= gamma; + cost[k].dfdux *= gamma; + cost[k].dfdx *= gamma; + cost[k].dfdu *= gamma; + + k = timeIndex++; + } + if (k == N) { // Only one worker will execute this + cost[N].dfdxx *= gamma; + cost[N].dfduu *= gamma; + cost[N].dfdux *= gamma; + cost[N].dfdx *= gamma; + cost[N].dfdu *= gamma; + } + }; + threadPool.runParallel(std::move(computeDOutEOutScaleCost), numWorkers); + + // compute cOut + cOut *= gamma; + } +} + +void kktMatrixInPlace(int iteration, Eigen::SparseMatrix& H, vector_t& h, Eigen::SparseMatrix& G, vector_t& g, + vector_t& DOut, vector_t& EOut, scalar_t& cOut) { + const int nz = H.rows(); + const int nc = G.rows(); + + // Init output + cOut = 1.0; + DOut.setOnes(nz); + EOut.setOnes(nc); + + for (int i = 0; i < iteration; i++) { + vector_t D, E; + const vector_t infNormOfHCols = matrixInfNormCols(H); + const vector_t infNormOfGCols = matrixInfNormCols(G); + + D = infNormOfHCols.array().max(infNormOfGCols.array()); + E = matrixInfNormRows(G); + + for (int i = 0; i < D.size(); i++) { + if (D(i) > 1e+4) D(i) = 1e+4; + if (D(i) < 1e-4) D(i) = 1.0; + } + + for (int i = 0; i < E.size(); i++) { + if (E(i) > 1e+4) E(i) = 1e+4; + if (E(i) < 1e-4) E(i) = 1.0; + } + + D = D.array().sqrt().inverse(); + E = E.array().sqrt().inverse(); + + scaleMatrixInPlace(D, D, H); + scaleMatrixInPlace(E, D, G); + h.array() *= D.array(); + g.array() *= E.array(); + + DOut = DOut.cwiseProduct(D); + EOut = EOut.cwiseProduct(E); + + const scalar_t infNormOfh = h.lpNorm(); + const vector_t infNormOfHColsUpdated = matrixInfNormCols(H); + const scalar_t gamma = 1.0 / limitScaling(std::max(infNormOfHColsUpdated.mean(), infNormOfh)); + + H *= gamma; + h *= gamma; + + cOut *= gamma; + } +} + +void scaleOcpData(const OcpSize& ocpSize, const vector_t& D, const vector_t& E, const scalar_t c, + std::vector& dynamics, std::vector& cost, + std::vector& scalingVectors) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[precondition::scaleOcpData] The number of stages cannot be less than 1."); + } + + scalingVectors.resize(N); + + // Scale H & h + const int nu_0 = ocpSize.numInputs.front(); + // Scale row - Pre multiply D + cost.front().dfduu.array().colwise() *= c * D.head(nu_0).array(); + // Scale col - Post multiply D + cost.front().dfduu *= D.head(nu_0).asDiagonal(); + + cost.front().dfdu.array() *= c * D.head(nu_0).array(); + + cost.front().dfdux.array().colwise() *= c * D.head(nu_0).array(); + + int currRow = nu_0; + for (int k = 1; k < N; ++k) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + auto& Q = cost[k].dfdxx; + auto& R = cost[k].dfduu; + auto& P = cost[k].dfdux; + auto& q = cost[k].dfdx; + auto& r = cost[k].dfdu; + + Q.array().colwise() *= c * D.segment(currRow, nx_k).array(); + Q *= D.segment(currRow, nx_k).asDiagonal(); + q.array() *= c * D.segment(currRow, nx_k).array(); + + P *= D.segment(currRow, nx_k).asDiagonal(); + currRow += nx_k; + + R.array().colwise() *= c * D.segment(currRow, nu_k).array(); + R *= D.segment(currRow, nu_k).asDiagonal(); + r.array() *= c * D.segment(currRow, nu_k).array(); + + P.array().colwise() *= c * D.segment(currRow, nu_k).array(); + + currRow += nu_k; + } + + const int nx_N = ocpSize.numStates[N]; + cost[N].dfdxx.array().colwise() *= c * D.tail(nx_N).array(); + cost[N].dfdxx *= D.tail(nx_N).asDiagonal(); + cost[N].dfdx.array() *= c * D.tail(nx_N).array(); + + // Scale G & g + auto& B_0 = dynamics[0].dfdu; + auto& C_0 = scalingVectors[0]; + const int nx_1 = ocpSize.numStates[1]; + + // \tilde{B} = E * B * D, + // scaling = E * D, + // \tilde{g} = E * g + B_0.array().colwise() *= E.head(nx_1).array(); + B_0 *= D.head(nu_0).asDiagonal(); + + C_0 = E.head(nx_1).array() * D.segment(nu_0, nx_1).array(); + + dynamics[0].dfdx.array().colwise() *= E.head(nx_1).array(); + dynamics[0].f.array() *= E.head(nx_1).array(); + + currRow = nx_1; + int currCol = nu_0; + for (int k = 1; k < N; ++k) { + const int nu_k = ocpSize.numInputs[k]; + const int nx_k = ocpSize.numStates[k]; + const int nx_next = ocpSize.numStates[k + 1]; + auto& A_k = dynamics[k].dfdx; + auto& B_k = dynamics[k].dfdu; + auto& b_k = dynamics[k].f; + auto& C_k = scalingVectors[k]; + + A_k.array().colwise() *= E.segment(currRow, nx_next).array(); + A_k *= D.segment(currCol, nx_k).asDiagonal(); + + B_k.array().colwise() *= E.segment(currRow, nx_next).array(); + B_k *= D.segment(currCol + nx_k, nu_k).asDiagonal(); + + C_k = E.segment(currRow, nx_next).array() * D.segment(currCol + nx_k + nu_k, nx_next).array(); + + b_k.array() *= E.segment(currRow, nx_next).array(); + + currRow += nx_next; + currCol += nx_k + nu_k; + } +} + +void descaleSolution(const vector_array_t& D, vector_array_t& xTrajectory, vector_array_t& uTrajectory) { + if (D.size() != xTrajectory.size() + uTrajectory.size() - 1) { + throw std::runtime_error("[precondition::descaleSolution] - Size doesn't match."); + } + + for (int k = 0; k < uTrajectory.size(); k++) { + uTrajectory[k].array() *= D[2 * k].array(); + xTrajectory[k + 1].array() *= D[2 * k + 1].array(); + } +} + +} // namespace precondition +} // namespace ocs2 diff --git a/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp b/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp index 6d3c0e8b9..f2442d8d7 100644 --- a/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp +++ b/ocs2_oc/src/rollout/PerformanceIndicesRollout.cpp @@ -49,7 +49,7 @@ scalar_t rolloutCost(cost_wraper_t costWraper, const scalar_array_t& timeTraject costTrajectory.push_back(costWraper(timeTrajectory[i], stateTrajectory[i], inputTrajectory[i])); } - return trapezoidalIntegration(timeTrajectory, costTrajectory); + return trapezoidalIntegration(timeTrajectory, costTrajectory, 0.0); } /******************************************************************************************************/ @@ -68,7 +68,7 @@ scalar_t rolloutConstraint(constraints_wraper_t constraintWraper, const scalar_a constraintTrajectoryISE.push_back(constraint.squaredNorm()); } - return trapezoidalIntegration(timeTrajectory, constraintTrajectoryISE); + return trapezoidalIntegration(timeTrajectory, constraintTrajectoryISE, 0.0); } } // namespace PerformanceIndicesRollout diff --git a/ocs2_oc/src/rollout/RolloutBase.cpp b/ocs2_oc/src/rollout/RolloutBase.cpp index ad1b10350..e7cb1d3e2 100644 --- a/ocs2_oc/src/rollout/RolloutBase.cpp +++ b/ocs2_oc/src/rollout/RolloutBase.cpp @@ -57,10 +57,11 @@ std::vector> RolloutBase::findActiveModesTimeInter for (int i = 0; i < numSubsystems; i++) { const auto& beginTime = switchingTimes[i]; const auto& endTime = switchingTimes[i + 1]; + timeIntervalArray[i] = std::make_pair(beginTime, endTime); // adjusting the start time to correct for subsystem recognition constexpr auto eps = numeric_traits::weakEpsilon(); - timeIntervalArray[i] = std::make_pair(std::min(beginTime + eps, endTime), endTime); + timeIntervalArray[i].first = std::min(beginTime + eps, endTime); } // end of for loop return timeIntervalArray; diff --git a/ocs2_oc/src/search_strategy/FilterLinesearch.cpp b/ocs2_oc/src/search_strategy/FilterLinesearch.cpp new file mode 100644 index 000000000..80cfa47c4 --- /dev/null +++ b/ocs2_oc/src/search_strategy/FilterLinesearch.cpp @@ -0,0 +1,91 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/search_strategy/FilterLinesearch.h" + +namespace ocs2 { + +std::pair FilterLinesearch::acceptStep(const PerformanceIndex& baselinePerformance, + const PerformanceIndex& stepPerformance, + scalar_t armijoDescentMetric) const { + const scalar_t baselineConstraintViolation = totalConstraintViolation(baselinePerformance); + const scalar_t stepConstraintViolation = totalConstraintViolation(stepPerformance); + + // Step acceptance and record step type + if (stepConstraintViolation > g_max) { + // High constraint violation. Only accept decrease in constraints. + const bool accepted = stepConstraintViolation < ((1.0 - gamma_c) * baselineConstraintViolation); + return std::make_pair(accepted, StepType::CONSTRAINT); + + } else if (stepConstraintViolation < g_min && baselineConstraintViolation < g_min && armijoDescentMetric < 0.0) { + // With low violation and having a descent direction, require the armijo condition. + const bool accepted = stepPerformance.merit < (baselinePerformance.merit + armijoFactor * armijoDescentMetric); + return std::make_pair(accepted, StepType::COST); + + } else { + // Medium violation: either merit or constraints decrease (with small gamma_c mixing of old constraints) + const bool accepted = stepPerformance.merit < (baselinePerformance.merit - gamma_c * baselineConstraintViolation) || + stepConstraintViolation < ((1.0 - gamma_c) * baselineConstraintViolation); + return std::make_pair(accepted, StepType::DUAL); + } +} + +std::string toString(const FilterLinesearch::StepType& stepType) { + using StepType = FilterLinesearch::StepType; + switch (stepType) { + case StepType::COST: + return "Cost"; + case StepType::CONSTRAINT: + return "Constraint"; + case StepType::DUAL: + return "Dual"; + case StepType::ZERO: + return "Zero"; + case StepType::UNKNOWN: + default: + return "Unknown"; + } +} + +scalar_t armijoDescentMetric(const std::vector& cost, const vector_array_t& deltaXSol, + const vector_array_t& deltaUSol) { + // To determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + scalar_t metric = 0.0; + for (int i = 0; i < cost.size(); i++) { + if (cost[i].dfdx.size() > 0) { + metric += cost[i].dfdx.dot(deltaXSol[i]); + } + if (cost[i].dfdu.size() > 0) { + metric += cost[i].dfdu.dot(deltaUSol[i]); + } + } + return metric; +} + +} // namespace ocs2 diff --git a/ocs2_oc/src/synchronized_module/AugmentedLagrangianObserver.cpp b/ocs2_oc/src/synchronized_module/AugmentedLagrangianObserver.cpp deleted file mode 100644 index bb7c00404..000000000 --- a/ocs2_oc/src/synchronized_module/AugmentedLagrangianObserver.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -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. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -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 "ocs2_oc/synchronized_module/AugmentedLagrangianObserver.h" - -#include "ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h" - -namespace ocs2 { - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void AugmentedLagrangianObserver::extractTermMetrics(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, - const ProblemMetrics& problemMetrics) { - // search intermediate - if (extractIntermediateTermMetrics(ocp, termName_, problemMetrics.intermediates, termMetricsArray_)) { - if (!primalSolution.timeTrajectory_.empty() && metricsCallback_ != nullptr) { - metricsCallback_(primalSolution.timeTrajectory_, termMetricsArray_); - } - } - - // search pre_jump - else if (extractPreJumpTermMetrics(ocp, termName_, problemMetrics.preJumps, termMetricsArray_)) { - if (!primalSolution.postEventIndices_.empty() && metricsCallback_ != nullptr) { - scalar_array_t timeArray(primalSolution.postEventIndices_.size()); - std::transform(primalSolution.postEventIndices_.cbegin(), primalSolution.postEventIndices_.cend(), timeArray.begin(), - [&](size_t postInd) -> scalar_t { return primalSolution.timeTrajectory_[postInd - 1]; }); - metricsCallback_(timeArray, termMetricsArray_); - } - } - - // search final - else { - const LagrangianMetrics* metricsPtr = extractFinalTermMetrics(ocp, termName_, problemMetrics.final); - if (metricsPtr != nullptr) { - if (!primalSolution.timeTrajectory_.empty() && metricsCallback_ != nullptr) { - const scalar_array_t timeArray{primalSolution.timeTrajectory_.back()}; - termMetricsArray_.push_back(*metricsPtr); - metricsCallback_(timeArray, termMetricsArray_); - } - - } else { - throw std::runtime_error("[AugmentedLagrangianObserver::extractTermsMetrics] Term (" + termName_ + - ") does not exist in the Lagrangian collections!"); - } - } -} - -/******************************************************************************************************/ -/******************************************************************************************************/ -/******************************************************************************************************/ -void AugmentedLagrangianObserver::extractTermMultipliers(const OptimalControlProblem& ocp, const DualSolution& dualSolution) { - // search intermediate - if (extractIntermediateTermMultiplier(ocp, termName_, dualSolution.intermediates, termMultiplierArray_)) { - if (!dualSolution.timeTrajectory.empty() && multiplierCallback_ != nullptr) { - multiplierCallback_(dualSolution.timeTrajectory, termMultiplierArray_); - } - } - - // search pre_jump - else if (extractPreJumpTermMultiplier(ocp, termName_, dualSolution.preJumps, termMultiplierArray_)) { - if (!dualSolution.postEventIndices.empty() && multiplierCallback_ != nullptr) { - scalar_array_t timeArray(dualSolution.postEventIndices.size()); - std::transform(dualSolution.postEventIndices.cbegin(), dualSolution.postEventIndices.cend(), timeArray.begin(), - [&](size_t postInd) -> scalar_t { return dualSolution.timeTrajectory[postInd - 1]; }); - multiplierCallback_(timeArray, termMultiplierArray_); - } - } - - // search final - else { - const Multiplier* multiplierPtr = extractFinalTermMultiplier(ocp, termName_, dualSolution.final); - if (multiplierPtr != nullptr) { - if (!dualSolution.timeTrajectory.empty() && multiplierCallback_ != nullptr) { - const scalar_array_t timeArray{dualSolution.timeTrajectory.back()}; - termMultiplierArray_.push_back(*multiplierPtr); - multiplierCallback_(timeArray, termMultiplierArray_); - } - - } else { - throw std::runtime_error("[AugmentedLagrangianObserver::extractTermMultipliers] Term (" + termName_ + - ") does not exist in the Lagrangian collections!"); - } - } -} - -} // namespace ocs2 diff --git a/ocs2_oc/src/synchronized_module/SolverObserver.cpp b/ocs2_oc/src/synchronized_module/SolverObserver.cpp new file mode 100644 index 000000000..bbb11dce6 --- /dev/null +++ b/ocs2_oc/src/synchronized_module/SolverObserver.cpp @@ -0,0 +1,200 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_oc/synchronized_module/SolverObserver.h" + +#include "ocs2_oc/oc_problem/OptimalControlProblemHelperFunction.h" + +namespace ocs2 { + +namespace { +std::string toString(SolverObserver::Type type) { + switch (type) { + case SolverObserver::Type::Final: + return "Final"; + case SolverObserver::Type::PreJump: + return "PreJump"; + case SolverObserver::Type::Intermediate: + return "Intermediate"; + default: + throw std::runtime_error("[SolverObserver::toString] undefined type!"); + } +} +} // unnamed namespace + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void SolverObserver::extractTermConstraint(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, + const ProblemMetrics& problemMetrics) { + if (!constraintCallback_ || primalSolution.timeTrajectory_.empty()) { + return; + } + + bool termIsFound = true; + switch (type_) { + case Type::Final: { + const auto* termConstraintPtr = extractFinalTermConstraint(ocp, termName_, problemMetrics.final); + termIsFound = termConstraintPtr != nullptr; + if (termIsFound) { + const scalar_array_t timeArray{primalSolution.timeTrajectory_.back()}; + const std::vector> termConstraintArray{*termConstraintPtr}; + constraintCallback_(timeArray, termConstraintArray); + } + break; + } + case Type::PreJump: { + std::vector> termConstraintArray; + termIsFound = extractPreJumpTermConstraint(ocp, termName_, problemMetrics.preJumps, termConstraintArray); + if (termIsFound) { + scalar_array_t timeArray(primalSolution.postEventIndices_.size()); + std::transform(primalSolution.postEventIndices_.cbegin(), primalSolution.postEventIndices_.cend(), timeArray.begin(), + [&](size_t postInd) -> scalar_t { return primalSolution.timeTrajectory_[postInd - 1]; }); + constraintCallback_(timeArray, termConstraintArray); + } + break; + } + case Type::Intermediate: { + std::vector> termConstraintArray; + termIsFound = extractIntermediateTermConstraint(ocp, termName_, problemMetrics.intermediates, termConstraintArray); + if (termIsFound) { + constraintCallback_(primalSolution.timeTrajectory_, termConstraintArray); + } + break; + } + default: + throw std::runtime_error("[SolverObserver::extractTermConstraint] undefined type!"); + } + + if (!termIsFound) { + throw std::runtime_error("[SolverObserver::extractTermConstraint] term (" + termName_ + ") does not exist in " + toString(type_) + + "-time constraint collections!"); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void SolverObserver::extractTermLagrangianMetrics(const OptimalControlProblem& ocp, const PrimalSolution& primalSolution, + const ProblemMetrics& problemMetrics) { + if (!lagrangianCallback_ || primalSolution.timeTrajectory_.empty()) { + return; + } + + bool termIsFound = true; + switch (type_) { + case Type::Final: { + const auto* lagrangianMetricsPtr = extractFinalTermLagrangianMetrics(ocp, termName_, problemMetrics.final); + termIsFound = lagrangianMetricsPtr != nullptr; + if (termIsFound) { + const scalar_array_t timeArray{primalSolution.timeTrajectory_.back()}; + const std::vector termLagrangianMetricsArray{*lagrangianMetricsPtr}; + lagrangianCallback_(timeArray, termLagrangianMetricsArray); + } + break; + } + case Type::PreJump: { + std::vector termLagrangianMetricsArray; + termIsFound = extractPreJumpTermLagrangianMetrics(ocp, termName_, problemMetrics.preJumps, termLagrangianMetricsArray); + if (termIsFound) { + scalar_array_t timeArray(primalSolution.postEventIndices_.size()); + std::transform(primalSolution.postEventIndices_.cbegin(), primalSolution.postEventIndices_.cend(), timeArray.begin(), + [&](size_t postInd) -> scalar_t { return primalSolution.timeTrajectory_[postInd - 1]; }); + lagrangianCallback_(timeArray, termLagrangianMetricsArray); + } + break; + } + case Type::Intermediate: { + std::vector termLagrangianMetricsArray; + termIsFound = extractIntermediateTermLagrangianMetrics(ocp, termName_, problemMetrics.intermediates, termLagrangianMetricsArray); + if (termIsFound) { + lagrangianCallback_(primalSolution.timeTrajectory_, termLagrangianMetricsArray); + } + break; + } + default: + throw std::runtime_error("[SolverObserver::extractTermLagrangianMetrics] undefined type!"); + } + + if (!termIsFound) { + throw std::runtime_error("[SolverObserver::extractTermLagrangianMetrics] term (" + termName_ + ") does not exist in " + + toString(type_) + "-time Lagrangian collections!"); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void SolverObserver::extractTermMultipliers(const OptimalControlProblem& ocp, const DualSolution& dualSolution) { + if (!multiplierCallback_ || dualSolution.timeTrajectory.empty()) { + return; + } + + bool termIsFound = true; + switch (type_) { + case Type::Final: { + const auto* multiplierPtr = extractFinalTermMultiplier(ocp, termName_, dualSolution.final); + termIsFound = multiplierPtr != nullptr; + if (termIsFound) { + const scalar_array_t timeArray{dualSolution.timeTrajectory.back()}; + const std::vector termMultiplierArray{*multiplierPtr}; + multiplierCallback_(timeArray, termMultiplierArray); + } + break; + } + case Type::PreJump: { + std::vector termMultiplierArray; + termIsFound = extractPreJumpTermMultiplier(ocp, termName_, dualSolution.preJumps, termMultiplierArray); + if (termIsFound) { + scalar_array_t timeArray(dualSolution.postEventIndices.size()); + std::transform(dualSolution.postEventIndices.cbegin(), dualSolution.postEventIndices.cend(), timeArray.begin(), + [&](size_t postInd) -> scalar_t { return dualSolution.timeTrajectory[postInd - 1]; }); + multiplierCallback_(timeArray, termMultiplierArray); + } + break; + } + case Type::Intermediate: { + std::vector termMultiplierArray; + termIsFound = extractIntermediateTermMultiplier(ocp, termName_, dualSolution.intermediates, termMultiplierArray); + if (termIsFound) { + multiplierCallback_(dualSolution.timeTrajectory, termMultiplierArray); + } + break; + } + default: + throw std::runtime_error("[SolverObserver::extractTermMultipliers] undefined type!"); + } + + if (!termIsFound) { + throw std::runtime_error("[SolverObserver::extractTermMultipliers] term (" + termName_ + ") does not exist in " + toString(type_) + + "-time Lagrangian collections!"); + } +} + +} // namespace ocs2 diff --git a/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp b/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp index e64f62925..a89511a97 100644 --- a/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp +++ b/ocs2_oc/src/trajectory_adjustment/TrajectorySpreading.cpp @@ -44,23 +44,6 @@ std::ostream& operator<<(std::ostream& os, const std::pair& ind) { os << "(" << ind.first << ", " << ind.second << ")"; return os; } - -/** - * Returns the first mismatching pair of elements from two ranges: one defined by [first1, last1) and - * another defined by [first2,last2). - * - * TODO deprecate this function once switched to c++14 and use: std::mismatch(first1, last1, first2, last2) - */ -template -std::pair mismatch(InputIt1 first1, InputIt1 last1, InputIt2 first2, InputIt2 last2) { - auto mismatchedIndex = std::mismatch(first1, last1, first2); - while (std::distance(last2, mismatchedIndex.second) > 0) { - --mismatchedIndex.first; - --mismatchedIndex.second; - } - return mismatchedIndex; -} - } // anonymous namespace /******************************************************************************************************/ @@ -90,12 +73,11 @@ auto TrajectorySpreading::set(const ModeSchedule& oldModeSchedule, const ModeSch // this means: mode[firstMatchingModeIndex + w] != updatedMode[updatedFirstMatchingModeIndex + w] size_t w = 0; while (oldStartIndexOfMatchedSequence < oldModeSchedule.modeSequence.size()) { - // TODO: change to std::mismatch(first1, last1, first2, last2). It is supported since c++14 // +1 to include the last active mode - const auto mismatchedIndex = mismatch(oldModeSchedule.modeSequence.cbegin() + oldStartIndexOfMatchedSequence, - oldModeSchedule.modeSequence.cbegin() + oldLastActiveModeIndex + 1, - newModeSchedule.modeSequence.cbegin() + newStartIndexOfMatchedSequence, - newModeSchedule.modeSequence.cbegin() + newLastActiveModeIndex + 1); + const auto mismatchedIndex = std::mismatch(oldModeSchedule.modeSequence.cbegin() + oldStartIndexOfMatchedSequence, + oldModeSchedule.modeSequence.cbegin() + oldLastActiveModeIndex + 1, + newModeSchedule.modeSequence.cbegin() + newStartIndexOfMatchedSequence, + newModeSchedule.modeSequence.cbegin() + newLastActiveModeIndex + 1); w = std::distance(oldModeSchedule.modeSequence.begin() + oldStartIndexOfMatchedSequence, mismatchedIndex.first); @@ -170,7 +152,7 @@ auto TrajectorySpreading::set(const ModeSchedule& oldModeSchedule, const ModeSch // if last mode of the new mode sequence is NOT matched else if (!isLastActiveModeOfNewModeSequenceMatched) { const auto mismatchEventTime = newModeSchedule.eventTimes[newStartIndexOfMatchedSequence + w - 1]; - eraseFromIndex_ = upperBoundIndex(oldTimeTrajectory, mismatchEventTime); + eraseFromIndex_ = lowerBoundIndex(oldTimeTrajectory, mismatchEventTime); } // computes the index of the spreading values and intervals diff --git a/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h b/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h new file mode 100644 index 000000000..1101b8471 --- /dev/null +++ b/ocs2_oc/test/include/ocs2_oc/test/DoubleIntegratorReachingTask.h @@ -0,0 +1,174 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ocs2 { + +class DoubleIntegratorReachingTask { + public: + static constexpr size_t STATE_DIM = 2; + static constexpr size_t INPUT_DIM = 1; + static constexpr scalar_t timeStep = 1e-2; + static constexpr scalar_t minRelCost = 1e-12; // to avoid early termination + static constexpr scalar_t constraintTolerance = 1e-3; + + enum class PenaltyType { + QuadraticPenalty, + SmoothAbsolutePenalty, + }; + + DoubleIntegratorReachingTask() = default; + virtual ~DoubleIntegratorReachingTask() = default; + + protected: + const scalar_t tGoal = 1.0; + const vector_t xInit = vector_t::Zero(STATE_DIM); + const vector_t xGoal = (vector_t(STATE_DIM) << 2.0, 0.0).finished(); + + std::unique_ptr getInitializer() const { return std::make_unique(INPUT_DIM); } + + std::shared_ptr getReferenceManagerPtr() const { + const ModeSchedule modeSchedule({tGoal}, {0, 1}); + const TargetTrajectories targetTrajectories({tGoal}, {xGoal}, {vector_t::Zero(INPUT_DIM)}); + return std::make_shared(targetTrajectories, modeSchedule); + } + + std::unique_ptr getCostPtr() const { + matrix_t Q = matrix_t::Zero(STATE_DIM, STATE_DIM); + matrix_t R = 0.1 * matrix_t::Identity(INPUT_DIM, INPUT_DIM); + return std::make_unique(std::move(Q), std::move(R)); + } + + std::unique_ptr getDynamicsPtr() const { + matrix_t A = (matrix_t(STATE_DIM, STATE_DIM) << 0.0, 1.0, 0.0, 0.0).finished(); + matrix_t B = (matrix_t(STATE_DIM, INPUT_DIM) << 0.0, 1.0).finished(); + return std::make_unique(std::move(A), std::move(B)); + } + + std::unique_ptr getGoalReachingAugmentedLagrangian(const vector_t& xGoal, PenaltyType penaltyType) { + constexpr scalar_t scale = 10.0; + constexpr scalar_t stepSize = 1.0; + + auto goalReachingConstraintPtr = std::make_unique(-xGoal, matrix_t::Identity(xGoal.size(), xGoal.size())); + + switch (penaltyType) { + case PenaltyType::QuadraticPenalty: { + using penalty_type = augmented::QuadraticPenalty; + penalty_type::Config boundsConfig{scale, stepSize}; + return create(std::move(goalReachingConstraintPtr), penalty_type::create(boundsConfig)); + } + case PenaltyType::SmoothAbsolutePenalty: { + using penalty_type = augmented::SmoothAbsolutePenalty; + penalty_type::Config boundsConfig{scale, 0.1, stepSize}; + return create(std::move(goalReachingConstraintPtr), penalty_type::create(boundsConfig)); + } + default: + throw std::runtime_error("[TestCartpole::getPenalty] undefined penaltyType"); + } + } + + /** This class enforces zero force at the second mode (mode = 1)*/ + class ZeroInputConstraint final : public StateInputConstraint { + public: + ZeroInputConstraint(const ReferenceManager& referenceManager) + : StateInputConstraint(ConstraintOrder::Linear), referenceManagerPtr_(&referenceManager) {} + + ~ZeroInputConstraint() override = default; + ZeroInputConstraint* clone() const override { return new ZeroInputConstraint(*this); } + + size_t getNumConstraints(scalar_t time) const override { return 1; } + + /** Only active after the fist mode */ + bool isActive(scalar_t time) const override { return referenceManagerPtr_->getModeSchedule().modeAtTime(time) > 0 ? true : false; } + + vector_t getValue(scalar_t t, const vector_t& x, const vector_t& u, const PreComputation&) const override { return u; } + + VectorFunctionLinearApproximation getLinearApproximation(scalar_t t, const vector_t& x, const vector_t& u, + const PreComputation&) const override { + VectorFunctionLinearApproximation approx; + approx.f = u; + approx.dfdx.setZero(getNumConstraints(t), x.size()); + approx.dfdu.setIdentity(getNumConstraints(t), u.size()); + return approx; + } + + private: + ZeroInputConstraint(const ZeroInputConstraint&) = default; + const ReferenceManager* referenceManagerPtr_; + }; + + /* + * printout trajectory. Use the following commands for plotting in MATLAB: + * subplot(2, 1, 1); plot(timeTrajectory, stateTrajectory); + * xlabel("time [sec]"); legend("pos", "vel"); + * subplot(2, 1, 2); plot(timeTrajectory, inputTrajectory); + * xlabel("time [sec]"); legend("force"); + */ + void printSolution(const PrimalSolution& primalSolution, bool display) const { + if (display) { + std::cerr << "\n"; + // time + std::cerr << "timeTrajectory = ["; + for (const auto& t : primalSolution.timeTrajectory_) { + std::cerr << t << "; "; + } + std::cerr << "];\n"; + // state + std::cerr << "stateTrajectory = ["; + for (const auto& x : primalSolution.stateTrajectory_) { + std::cerr << x.transpose() << "; "; + } + std::cerr << "];\n"; + // input + std::cerr << "inputTrajectory = ["; + for (const auto& u : primalSolution.inputTrajectory_) { + std::cerr << u.transpose() << "; "; + } + std::cerr << "];\n"; + } + } +}; + +constexpr size_t DoubleIntegratorReachingTask::STATE_DIM; +constexpr size_t DoubleIntegratorReachingTask::INPUT_DIM; +constexpr scalar_t DoubleIntegratorReachingTask::timeStep; +constexpr scalar_t DoubleIntegratorReachingTask::minRelCost; +constexpr scalar_t DoubleIntegratorReachingTask::constraintTolerance; + +} // namespace ocs2 diff --git a/ocs2_oc/test/include/ocs2_oc/test/EXP0.h b/ocs2_oc/test/include/ocs2_oc/test/EXP0.h index 1e2cfb6a6..cdf3d1473 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/EXP0.h +++ b/ocs2_oc/test/include/ocs2_oc/test/EXP0.h @@ -165,8 +165,8 @@ inline OptimalControlProblem createExp0Problem(const std::shared_ptradd("cost", std::unique_ptr(new ocs2::EXP0_Cost())); - problem.finalCostPtr->add("finalCost", std::unique_ptr(new ocs2::EXP0_FinalCost())); + problem.costPtr->add("cost", std::make_unique()); + problem.finalCostPtr->add("finalCost", std::make_unique()); return problem; } diff --git a/ocs2_oc/test/include/ocs2_oc/test/EXP1.h b/ocs2_oc/test/include/ocs2_oc/test/EXP1.h index 86a8e837d..63904fa09 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/EXP1.h +++ b/ocs2_oc/test/include/ocs2_oc/test/EXP1.h @@ -222,8 +222,8 @@ inline OptimalControlProblem createExp1Problem(const std::shared_ptradd("cost", std::unique_ptr(new ocs2::EXP1_Cost())); - problem.finalCostPtr->add("finalCost", std::unique_ptr(new ocs2::EXP1_FinalCost())); + problem.costPtr->add("cost", std::make_unique()); + problem.finalCostPtr->add("finalCost", std::make_unique()); return problem; } diff --git a/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h b/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h index e5f7e159b..01fd36f5e 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h +++ b/ocs2_oc/test/include/ocs2_oc/test/circular_kinematics.h @@ -126,12 +126,10 @@ inline OptimalControlProblem createCircularKinematicsProblem(const std::string& problem.dynamicsPtr.reset(new CircularKinematicsSystem()); // cost function - std::unique_ptr cost(new CircularKinematicsCost(libraryFolder)); - problem.costPtr->add("cost", std::move(cost)); + problem.costPtr->add("cost", std::make_unique(libraryFolder)); // constraint - std::unique_ptr constraint(new CircularKinematicsConstraints()); - problem.equalityConstraintPtr->add("constraint", std::move(constraint)); + problem.equalityConstraintPtr->add("constraint", std::make_unique()); return problem; } diff --git a/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h b/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h index d9e69116f..ccd8dc890 100644 --- a/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h +++ b/ocs2_oc/test/include/ocs2_oc/test/testProblemsGeneration.h @@ -48,7 +48,7 @@ inline ScalarFunctionQuadraticApproximation getRandomCost(int n, int m) { ScalarFunctionQuadraticApproximation cost; cost.dfdxx = QPPR.topLeftCorner(n, n); cost.dfdx = vector_t::Random(n); - if (m > 0) { + if (m >= 0) { cost.dfdux = QPPR.bottomLeftCorner(m, n); cost.dfduu = QPPR.bottomRightCorner(m, m); cost.dfdu = vector_t::Random(m); @@ -58,18 +58,18 @@ inline ScalarFunctionQuadraticApproximation getRandomCost(int n, int m) { } inline std::unique_ptr getOcs2Cost(const ScalarFunctionQuadraticApproximation& cost) { - return std::unique_ptr(new ocs2::QuadraticStateInputCost(cost.dfdxx, cost.dfduu, cost.dfdux)); + return std::make_unique(cost.dfdxx, cost.dfduu, cost.dfdux); } inline std::unique_ptr getOcs2StateCost(const ScalarFunctionQuadraticApproximation& costFinal) { - return std::unique_ptr(new ocs2::QuadraticStateCost(costFinal.dfdxx)); + return std::make_unique(costFinal.dfdxx); } /** Get random linear dynamics of n states and m inputs */ inline VectorFunctionLinearApproximation getRandomDynamics(int n, int m) { VectorFunctionLinearApproximation dynamics; dynamics.dfdx = matrix_t::Random(n, n); - if (m > 0) { + if (m >= 0) { dynamics.dfdu = matrix_t::Random(n, m); } dynamics.f = vector_t::Random(n); @@ -77,14 +77,14 @@ inline VectorFunctionLinearApproximation getRandomDynamics(int n, int m) { } inline std::unique_ptr getOcs2Dynamics(const VectorFunctionLinearApproximation& dynamics) { - return std::unique_ptr(new ocs2::LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu)); + return std::make_unique(dynamics.dfdx, dynamics.dfdu); } /** Get random nc linear constraints of n states, and m inputs */ inline VectorFunctionLinearApproximation getRandomConstraints(int n, int m, int nc) { VectorFunctionLinearApproximation constraints; constraints.dfdx = matrix_t::Random(nc, n); - if (m > 0) { + if (m >= 0) { constraints.dfdu = matrix_t::Random(nc, m); } constraints.f = vector_t::Random(nc); @@ -92,12 +92,12 @@ inline VectorFunctionLinearApproximation getRandomConstraints(int n, int m, int } inline std::unique_ptr getOcs2Constraints(const VectorFunctionLinearApproximation& stateInputConstraints) { - return std::unique_ptr( - new ocs2::LinearStateInputConstraint(stateInputConstraints.f, stateInputConstraints.dfdx, stateInputConstraints.dfdu)); + return std::make_unique(stateInputConstraints.f, stateInputConstraints.dfdx, + stateInputConstraints.dfdu); } inline std::unique_ptr getOcs2StateOnlyConstraints(const VectorFunctionLinearApproximation& stateOnlyConstraints) { - return std::unique_ptr(new ocs2::LinearStateConstraint(stateOnlyConstraints.f, stateOnlyConstraints.dfdx)); + return std::make_unique(stateOnlyConstraints.f, stateOnlyConstraints.dfdx); } } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp b/ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp similarity index 52% rename from ocs2_sqp/ocs2_sqp/test/testProjection.cpp rename to ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp index f948839bc..f7f573caa 100644 --- a/ocs2_sqp/ocs2_sqp/test/testProjection.cpp +++ b/ocs2_oc/test/multiple_shooting/testProjectionMultiplierCoefficients.cpp @@ -29,36 +29,36 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "ocs2_sqp/ConstraintProjection.h" +#include +#include -#include +#include "ocs2_oc/test/testProblemsGeneration.h" -TEST(test_projection, testProjectionQR) { - const auto constraint = ocs2::getRandomConstraints(30, 20, 10); +using namespace ocs2; - const auto projection = ocs2::qrConstraintProjection(constraint); +TEST(testMultipleShootingHelpers, testProjectionMultiplierCoefficients) { + constexpr size_t stateDim = 30; + constexpr size_t inputDim = 20; + constexpr size_t constraintDim = 10; - // range of Pu is in null-space of D - ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); + const auto cost = getRandomCost(stateDim, inputDim); + const auto dynamics = getRandomDynamics(stateDim, inputDim); + const auto constraint = getRandomConstraints(stateDim, inputDim, constraintDim); - // D * Px cancels the C term - ASSERT_TRUE((constraint.dfdx + constraint.dfdu * projection.dfdx).isZero()); + auto result = LinearAlgebra::qrConstraintProjection(constraint); + const auto projection = std::move(result.first); + const auto pseudoInverse = std::move(result.second); - // D * Pe cancels the e term - ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); -} - -TEST(test_projection, testProjectionLU) { - const auto constraint = ocs2::getRandomConstraints(30, 20, 10); - - const auto projection = ocs2::luConstraintProjection(constraint); + multiple_shooting::ProjectionMultiplierCoefficients projectionMultiplierCoefficients; + projectionMultiplierCoefficients.compute(cost, dynamics, projection, pseudoInverse); - // range of Pu is in null-space of D - ASSERT_TRUE((constraint.dfdu * projection.dfdu).isZero()); + const matrix_t dfdx = -pseudoInverse * (cost.dfdux + cost.dfduu * projection.dfdx); + const matrix_t dfdu = -pseudoInverse * (cost.dfduu * projection.dfdu); + const matrix_t dfdcostate = -pseudoInverse * dynamics.dfdu.transpose(); + const vector_t f = -pseudoInverse * (cost.dfdu + cost.dfduu * projection.f); - // D * Px cancels the C term - ASSERT_TRUE((constraint.dfdx + constraint.dfdu * projection.dfdx).isZero()); - - // D * Pe cancels the e term - ASSERT_TRUE((constraint.f + constraint.dfdu * projection.f).isZero()); -} \ No newline at end of file + ASSERT_TRUE(projectionMultiplierCoefficients.dfdx.isApprox(dfdx)); + ASSERT_TRUE(projectionMultiplierCoefficients.dfdu.isApprox(dfdu)); + ASSERT_TRUE(projectionMultiplierCoefficients.dfdcostate.isApprox(dfdcostate)); + ASSERT_TRUE(projectionMultiplierCoefficients.f.isApprox(f)); +} diff --git a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp b/ocs2_oc/test/multiple_shooting/testTranscriptionMetrics.cpp similarity index 53% rename from ocs2_sqp/ocs2_sqp/test/testTranscription.cpp rename to ocs2_oc/test/multiple_shooting/testTranscriptionMetrics.cpp index f3c6fdce3..147d68cb1 100644 --- a/ocs2_sqp/ocs2_sqp/test/testTranscription.cpp +++ b/ocs2_oc/test/multiple_shooting/testTranscriptionMetrics.cpp @@ -29,83 +29,93 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "ocs2_sqp/MultipleShootingTranscription.h" +#include +#include -#include -#include - -namespace { -/** Helper to compare if two performance indices are identical */ -bool areIdentical(const ocs2::PerformanceIndex& lhs, const ocs2::PerformanceIndex& rhs) { - return lhs.merit == rhs.merit && lhs.cost == rhs.cost && lhs.dynamicsViolationSSE == rhs.dynamicsViolationSSE && - lhs.equalityConstraintsSSE == rhs.equalityConstraintsSSE && lhs.equalityLagrangian == rhs.equalityLagrangian && - lhs.inequalityLagrangian == rhs.inequalityLagrangian; -} -} // namespace +#include "ocs2_oc/test/circular_kinematics.h" +#include "ocs2_oc/test/testProblemsGeneration.h" using namespace ocs2; -using namespace ocs2::multiple_shooting; -TEST(test_transcription, intermediate_performance) { +TEST(test_transcription_metrics, intermediate) { + constexpr int nx = 2; + constexpr int nu = 2; + // optimal control problem OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/sqp_test_generated"); + // equality constraints + problem.equalityConstraintPtr->add("equalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 2))); + problem.stateEqualityConstraintPtr->add("stateEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 1))); + // inequality constraints + problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 3))); + problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + auto discretizer = selectDynamicsDiscretization(SensitivityIntegratorType::RK4); auto sensitivityDiscretizer = selectDynamicsSensitivityDiscretization(SensitivityIntegratorType::RK4); - scalar_t t = 0.5; - scalar_t dt = 0.1; - const vector_t x = (vector_t(2) << 1.0, 0.1).finished(); - const vector_t x_next = (vector_t(2) << 1.1, 0.2).finished(); - const vector_t u = (vector_t(2) << 0.1, 1.3).finished(); - const auto transcription = setupIntermediateNode(problem, sensitivityDiscretizer, true, t, dt, x, x_next, u); - - const auto performance = computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); + const scalar_t t = 0.5; + const scalar_t dt = 0.1; + const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); + const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); + const vector_t u = (vector_t(nu) << 0.1, 1.3).finished(); + const auto transcription = multiple_shooting::setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); + const auto metrics = multiple_shooting::computeIntermediateMetrics(problem, discretizer, t, dt, x, x_next, u); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(metrics.isApprox(multiple_shooting::computeMetrics(transcription), 1e-12)); } -TEST(test_transcription, terminal_performance) { - int nx = 3; +TEST(test_transcription_metrics, event) { + constexpr int nx = 2; + // optimal control problem OptimalControlProblem problem; + // dynamics + const auto dynamics = getRandomDynamics(nx, 0); + const auto jumpMap = matrix_t::Random(nx, nx); + problem.dynamicsPtr.reset(new LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); + // cost - problem.finalCostPtr->add("finalCost", getOcs2StateCost(getRandomCost(nx, 0))); - problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); + problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); + + // constraints + problem.preJumpEqualityConstraintPtr->add("preJumpEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 3))); + problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; - scalar_t t = 0.5; - const vector_t x = vector_t::Random(nx); - const auto transcription = setupTerminalNode(problem, t, x); - const auto performance = computeTerminalPerformance(problem, t, x); + const scalar_t t = 0.5; + const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); + const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); + const auto transcription = multiple_shooting::setupEventNode(problem, t, x, x_next); + const auto metrics = multiple_shooting::computeEventMetrics(problem, t, x, x_next); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(metrics.isApprox(multiple_shooting::computeMetrics(transcription), 1e-12)); } -TEST(test_transcription, event_performance) { - int nx = 2; +TEST(test_transcription_metrics, terminal) { + constexpr int nx = 3; + // optimal control problem OptimalControlProblem problem; - // dynamics - const auto dynamics = getRandomDynamics(nx, 0); - const auto jumpMap = matrix_t::Random(nx, nx); - problem.dynamicsPtr.reset(new LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); - // cost - problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); + problem.finalCostPtr->add("finalCost", getOcs2StateCost(getRandomCost(nx, 0))); + problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); + + // constraints + problem.finalEqualityConstraintPtr->add("finalEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 3))); + problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); problem.targetTrajectoriesPtr = &targetTrajectories; const scalar_t t = 0.5; - const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); - const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); - const auto transcription = setupEventNode(problem, t, x, x_next); - const auto performance = computeEventPerformance(problem, t, x, x_next); + const vector_t x = vector_t::Random(nx); + const auto transcription = multiple_shooting::setupTerminalNode(problem, t, x); + const auto metrics = multiple_shooting::computeTerminalMetrics(problem, t, x); - ASSERT_TRUE(areIdentical(performance, transcription.performance)); + ASSERT_TRUE(metrics.isApprox(multiple_shooting::computeMetrics(transcription), 1e-12)); } diff --git a/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp b/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp new file mode 100644 index 000000000..7fb21e8e5 --- /dev/null +++ b/ocs2_oc/test/multiple_shooting/testTranscriptionPerformanceIndex.cpp @@ -0,0 +1,121 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include +#include + +#include "ocs2_oc/test/circular_kinematics.h" +#include "ocs2_oc/test/testProblemsGeneration.h" + +using namespace ocs2; + +TEST(test_transcription_performance, intermediate) { + constexpr int nx = 2; + constexpr int nu = 2; + + // optimal control problem + OptimalControlProblem problem = createCircularKinematicsProblem("/tmp/sqp_test_generated"); + + // equality constraints + problem.equalityConstraintPtr->add("equalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 2))); + problem.stateEqualityConstraintPtr->add("stateEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 1))); + // inequality constraints + problem.inequalityConstraintPtr->add("inequalityConstraint", getOcs2Constraints(getRandomConstraints(nx, nu, 3))); + problem.stateInequalityConstraintPtr->add("stateInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + + auto discretizer = selectDynamicsDiscretization(SensitivityIntegratorType::RK4); + auto sensitivityDiscretizer = selectDynamicsSensitivityDiscretization(SensitivityIntegratorType::RK4); + + const scalar_t t = 0.5; + const scalar_t dt = 0.1; + const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); + const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); + const vector_t u = (vector_t(nu) << 0.1, 1.3).finished(); + const auto transcription = multiple_shooting::setupIntermediateNode(problem, sensitivityDiscretizer, t, dt, x, x_next, u); + const auto performance = multiple_shooting::computeIntermediatePerformance(problem, discretizer, t, dt, x, x_next, u); + + ASSERT_TRUE(performance.isApprox(multiple_shooting::computePerformanceIndex(transcription, dt), 1e-12)); +} + +TEST(test_transcription_performance, event) { + constexpr int nx = 2; + + // optimal control problem + OptimalControlProblem problem; + + // dynamics + const auto dynamics = getRandomDynamics(nx, 0); + const auto jumpMap = matrix_t::Random(nx, nx); + problem.dynamicsPtr.reset(new LinearSystemDynamics(dynamics.dfdx, dynamics.dfdu, jumpMap)); + + // cost + problem.preJumpCostPtr->add("eventCost", getOcs2StateCost(getRandomCost(nx, 0))); + + // constraints + problem.preJumpEqualityConstraintPtr->add("preJumpEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 3))); + problem.preJumpInequalityConstraintPtr->add("preJumpInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + + const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); + problem.targetTrajectoriesPtr = &targetTrajectories; + + const scalar_t t = 0.5; + const vector_t x = (vector_t(nx) << 1.0, 0.1).finished(); + const vector_t x_next = (vector_t(nx) << 1.1, 0.2).finished(); + const auto transcription = multiple_shooting::setupEventNode(problem, t, x, x_next); + const auto performance = multiple_shooting::computeEventPerformance(problem, t, x, x_next); + + ASSERT_TRUE(performance.isApprox(multiple_shooting::computePerformanceIndex(transcription), 1e-12)); +} + +TEST(test_transcription_performance, terminal) { + constexpr int nx = 3; + + // optimal control problem + OptimalControlProblem problem; + + // cost + problem.finalCostPtr->add("finalCost", getOcs2StateCost(getRandomCost(nx, 0))); + problem.finalSoftConstraintPtr->add("finalSoftCost", getOcs2StateCost(getRandomCost(nx, 0))); + + // constraints + problem.finalEqualityConstraintPtr->add("finalEqualityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 3))); + problem.finalInequalityConstraintPtr->add("finalInequalityConstraint", getOcs2StateOnlyConstraints(getRandomConstraints(nx, 0, 4))); + + const TargetTrajectories targetTrajectories({0.0}, {vector_t::Random(nx)}, {vector_t::Random(0)}); + problem.targetTrajectoriesPtr = &targetTrajectories; + + const scalar_t t = 0.5; + const vector_t x = vector_t::Random(nx); + const auto transcription = multiple_shooting::setupTerminalNode(problem, t, x); + const auto performance = multiple_shooting::computeTerminalPerformance(problem, t, x); + + ASSERT_TRUE(performance.isApprox(multiple_shooting::computePerformanceIndex(transcription), 1e-12)); +} diff --git a/ocs2_sqp/ocs2_sqp/test/testDiscretization.cpp b/ocs2_oc/test/oc_data/testTimeDiscretization.cpp similarity index 95% rename from ocs2_sqp/ocs2_sqp/test/testDiscretization.cpp rename to ocs2_oc/test/oc_data/testTimeDiscretization.cpp index 0ecdfda3d..b1b420ac1 100644 --- a/ocs2_sqp/ocs2_sqp/test/testDiscretization.cpp +++ b/ocs2_oc/test/oc_data/testTimeDiscretization.cpp @@ -29,11 +29,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_oc/oc_data/TimeDiscretization.h" using namespace ocs2; -TEST(test_discretization, noEvents_plusEps) { +TEST(test_time_discretization, noEvents_plusEps) { scalar_t initTime = 0.1; scalar_t finalTime = 0.3 + std::numeric_limits::epsilon(); scalar_t dt = 0.1; @@ -51,7 +51,7 @@ TEST(test_discretization, noEvents_plusEps) { ASSERT_EQ(time.size(), 3); } -TEST(test_discretization, noEvents_minEps) { +TEST(test_time_discretization, noEvents_minEps) { scalar_t initTime = 0.1; scalar_t finalTime = 0.3 - std::numeric_limits::epsilon(); scalar_t dt = 0.1; @@ -69,7 +69,7 @@ TEST(test_discretization, noEvents_minEps) { ASSERT_EQ(time.size(), 3); } -TEST(test_discretization, eventAtBeginning) { +TEST(test_time_discretization, eventAtBeginning) { scalar_t initTime = 0.1; scalar_t finalTime = 0.3; scalar_t dt = 0.1; @@ -87,7 +87,7 @@ TEST(test_discretization, eventAtBeginning) { ASSERT_EQ(time.size(), 3); } -TEST(test_discretization, withEvents) { +TEST(test_time_discretization, withEvents) { scalar_t initTime = 3.0; scalar_t finalTime = 4.0; scalar_t dt = 0.1; diff --git a/ocs2_oc/test/oc_problem/testOcpToKkt.cpp b/ocs2_oc/test/oc_problem/testOcpToKkt.cpp new file mode 100644 index 000000000..10e490ec8 --- /dev/null +++ b/ocs2_oc/test/oc_problem/testOcpToKkt.cpp @@ -0,0 +1,94 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include "ocs2_oc/oc_problem/OcpSize.h" +#include "ocs2_oc/oc_problem/OcpToKkt.h" + +#include "ocs2_oc/test/testProblemsGeneration.h" + +class OcpToKktTest : public testing::Test { + protected: + // x_0, x_1, ... x_{N - 1}, X_{N} + static constexpr size_t N_ = 10; // numStages + static constexpr size_t nx_ = 4; + static constexpr size_t nu_ = 3; + static constexpr size_t nc_ = 5; + static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); + static constexpr size_t numConstraints = N_ * (nx_ + nc_); + + OcpToKktTest() { + srand(0); + + x0 = ocs2::vector_t::Random(nx_); + for (int i = 0; i < N_; i++) { + dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + } + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + + ocpSize_ = ocs2::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray); + } + + ocs2::OcpSize ocpSize_; + ocs2::vector_t x0; + std::vector dynamicsArray; + std::vector costArray; + std::vector constraintsArray; +}; + +TEST_F(OcpToKktTest, sparseConstraintsApproximation) { + Eigen::SparseMatrix G; + ocs2::vector_t g; + ocs2::vector_array_t scalingVectors(N_); + for (auto& v : scalingVectors) { + v = ocs2::vector_t::Random(nx_); + } + ocs2::VectorFunctionLinearApproximation constraintsApproximation; + ocs2::getConstraintMatrix(ocpSize_, x0, dynamicsArray, &constraintsArray, &scalingVectors, constraintsApproximation); + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, &constraintsArray, &scalingVectors, G, g); + + EXPECT_TRUE(constraintsApproximation.dfdx.isApprox(G.toDense())); + EXPECT_TRUE(constraintsApproximation.f.isApprox(g)); +} + +TEST_F(OcpToKktTest, sparseCostApproximation) { + Eigen::SparseMatrix H; + ocs2::vector_t h; + + ocs2::ScalarFunctionQuadraticApproximation costApproximation; + ocs2::getCostMatrix(ocpSize_, x0, costArray, costApproximation); + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H, h); + + EXPECT_TRUE(costApproximation.dfdxx.isApprox(H.toDense())); + EXPECT_TRUE(costApproximation.dfdx.isApprox(h)); +} diff --git a/ocs2_oc/test/precondition/testPrecondition.cpp b/ocs2_oc/test/precondition/testPrecondition.cpp new file mode 100644 index 000000000..a0243f710 --- /dev/null +++ b/ocs2_oc/test/precondition/testPrecondition.cpp @@ -0,0 +1,217 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include + +#include "ocs2_oc/oc_problem/OcpToKkt.h" +#include "ocs2_oc/precondition/Ruzi.h" +#include "ocs2_oc/test/testProblemsGeneration.h" + +class PreconditionTest : public testing::Test { + protected: + // x_0, x_1, ... x_{N - 1}, X_{N} + static constexpr size_t N_ = 10; // numStages + static constexpr size_t nx_ = 4; + static constexpr size_t nu_ = 3; + static constexpr size_t numDecisionVariables_ = N_ * (nx_ + nu_); + static constexpr size_t numConstraints_ = N_ * nx_; + + PreconditionTest() { + srand(0); + + x0 = ocs2::vector_t::Random(nx_); + for (int i = 0; i < N_; i++) { + dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + } + costArray.push_back(ocs2::getRandomCost(nx_, 0)); + + ocpSize_ = ocs2::extractSizesFromProblem(dynamicsArray, costArray, nullptr); + } + + ocs2::OcpSize ocpSize_; + ocs2::vector_t x0; + std::vector dynamicsArray; + std::vector costArray; +}; + +constexpr size_t PreconditionTest::N_; +constexpr size_t PreconditionTest::nx_; +constexpr size_t PreconditionTest::nu_; +constexpr size_t PreconditionTest::numDecisionVariables_; +constexpr size_t PreconditionTest::numConstraints_; + +TEST_F(PreconditionTest, kktMatrixInPlace) { + ocs2::vector_t D, E; + ocs2::scalar_t c; + + Eigen::SparseMatrix H; + ocs2::vector_t h; + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H, h); + // Copy of the original matrix/vector + const Eigen::SparseMatrix H_src = H; + const ocs2::vector_t h_src = h; + + Eigen::SparseMatrix G; + ocs2::vector_t g; + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, nullptr, G, g); + // Copy of the original matrix/vector + const Eigen::SparseMatrix G_src = G; + const ocs2::vector_t g_src = g; + + // Test 1: Construct the stacked cost and constraints matrices first and scale next. + ocs2::precondition::kktMatrixInPlace(5, H, h, G, g, D, E, c); + + // After pre-conditioning, H, h, G, and g will be scaled in place.. + const Eigen::SparseMatrix H_ref = c * D.asDiagonal() * H_src * D.asDiagonal(); + const ocs2::vector_t h_ref = c * D.asDiagonal() * h_src; + const Eigen::SparseMatrix G_ref = E.asDiagonal() * G_src * D.asDiagonal(); + const ocs2::vector_t g_ref = E.asDiagonal() * g_src; + + // The scaled matrix given by calculatePreConditioningFactors function and the one calculated with D, E and c factors should be the same. + ASSERT_TRUE(H_ref.isApprox(H)); // H + ASSERT_TRUE(h_ref.isApprox(h)); // h + ASSERT_TRUE(G_ref.isApprox(G)) << "G_ref:\n" << G_ref << "\nG:\n" << G.toDense(); // G + ASSERT_TRUE(g_ref.isApprox(g)); // g + + // Normalized the inf-Norm of both rows and cols of KKT matrix should be closer to 1 + const ocs2::matrix_t KKT = (ocs2::matrix_t(H_src.rows() + G_src.rows(), H_src.rows() + G_src.rows()) << H_src.toDense(), + G_src.transpose().toDense(), G_src.toDense(), ocs2::matrix_t::Zero(G_src.rows(), G_src.rows())) + .finished(); + + const ocs2::matrix_t KKTScaled = (ocs2::matrix_t(H.rows() + G.rows(), H.rows() + G.rows()) << H.toDense(), G.transpose().toDense(), + G.toDense(), ocs2::matrix_t::Zero(G.rows(), G.rows())) + .finished(); + + // Inf-norm of row vectors + ocs2::vector_t infNormRows = KKT.cwiseAbs().rowwise().maxCoeff(); + ocs2::vector_t infNormRowsScaled = KKTScaled.cwiseAbs().rowwise().maxCoeff(); + EXPECT_LT((infNormRowsScaled.array() - 1).abs().maxCoeff(), (infNormRows.array() - 1).abs().maxCoeff()); + // Inf-norm of column vectors + ocs2::vector_t infNormCols = KKT.cwiseAbs().colwise().maxCoeff(); + ocs2::vector_t infNormColsScaled = KKTScaled.cwiseAbs().colwise().maxCoeff(); + EXPECT_LT((infNormColsScaled.array() - 1).abs().maxCoeff(), (infNormCols.array() - 1).abs().maxCoeff()); + + // Test 2: Scale const and dynamics data of each time step first and then construct the stacked matrices from the scaled data. + std::vector scalingVectors; + ocs2::precondition::scaleOcpData(ocpSize_, D, E, c, dynamicsArray, costArray, scalingVectors); + + Eigen::SparseMatrix H_scaledData; + ocs2::vector_t h_scaledData; + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H_scaledData, h_scaledData); + Eigen::SparseMatrix G_scaledData; + ocs2::vector_t g_scaledData; + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, &scalingVectors, G_scaledData, g_scaledData); + EXPECT_TRUE(H_ref.isApprox(H_scaledData)); // H + EXPECT_TRUE(h_ref.isApprox(h_scaledData)); // h + EXPECT_TRUE(G_ref.isApprox(G_scaledData)); // G + EXPECT_TRUE(g_ref.isApprox(g_scaledData)); // g +} + +TEST_F(PreconditionTest, ocpDataInPlaceInParallel) { + ocs2::ThreadPool threadPool(5, 99); + + ocs2::vector_t D_ref, E_ref; + ocs2::scalar_t c_ref; + + Eigen::SparseMatrix H_ref; + ocs2::vector_t h_ref; + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H_ref, h_ref); + + // Generate reference + ocs2::vector_t g_ref; + Eigen::SparseMatrix G_ref; + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, nullptr, G_ref, g_ref); + ocs2::precondition::kktMatrixInPlace(5, H_ref, h_ref, G_ref, g_ref, D_ref, E_ref, c_ref); + + // Test start + ocs2::vector_array_t D_array, E_array; + ocs2::scalar_t c; + ocs2::vector_array_t scalingVectors(N_); + ocs2::precondition::ocpDataInPlaceInParallel(threadPool, x0, ocpSize_, 5, dynamicsArray, costArray, D_array, E_array, scalingVectors, c); + + ocs2::vector_t D_stacked(D_ref.rows()), E_stacked(E_ref.rows()); + int curRow = 0; + for (auto& v : D_array) { + D_stacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + curRow = 0; + for (auto& v : E_array) { + E_stacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + + EXPECT_TRUE(D_stacked.isApprox(D_ref)); + EXPECT_TRUE(E_stacked.isApprox(E_ref)); + EXPECT_DOUBLE_EQ(c, c_ref); + + Eigen::SparseMatrix H_scaledData; + ocs2::vector_t h_scaledData; + ocs2::getCostMatrixSparse(ocpSize_, x0, costArray, H_scaledData, h_scaledData); + Eigen::SparseMatrix G_scaledData; + ocs2::vector_t g_scaledData; + ocs2::getConstraintMatrixSparse(ocpSize_, x0, dynamicsArray, nullptr, &scalingVectors, G_scaledData, g_scaledData); + + EXPECT_TRUE(H_ref.isApprox(H_scaledData)); // H + EXPECT_TRUE(h_ref.isApprox(h_scaledData)); // h + EXPECT_TRUE(G_ref.isApprox(G_scaledData)); // G + EXPECT_TRUE(g_ref.isApprox(g_scaledData)); // g +} + +TEST_F(PreconditionTest, descaleSolution) { + ocs2::vector_array_t D(2 * N_); + ocs2::vector_t DStacked(numDecisionVariables_); + ocs2::vector_array_t x(N_ + 1), u(N_); + x[0].setRandom(nx_); + for (int i = 0; i < N_; i++) { + D[2 * i].setRandom(nu_); + D[2 * i + 1].setRandom(nx_); + u[i].setRandom(nu_); + x[i + 1].setRandom(nx_); + } + int curRow = 0; + for (auto& v : D) { + DStacked.segment(curRow, v.size()) = v; + curRow += v.size(); + } + ocs2::vector_t packedSolution; + ocs2::toKktSolution(x, u, packedSolution); + + packedSolution.array() *= DStacked.array(); + + ocs2::vector_t packedSolutionNew; + ocs2::precondition::descaleSolution(D, x, u); + ocs2::toKktSolution(x, u, packedSolutionNew); + EXPECT_TRUE(packedSolutionNew.isApprox(packedSolution)) << std::setprecision(6) << "DescaledSolution: \n" + << packedSolutionNew.transpose() << "\nIt should be \n" + << packedSolution.transpose(); +} \ No newline at end of file diff --git a/ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp b/ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp index 1d5dbbbee..344caf273 100644 --- a/ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp +++ b/ocs2_oc/test/rollout/testTimeTriggeredRollout.cpp @@ -72,15 +72,13 @@ TEST(time_rollout_test, time_rollout_test) { }(); // rollout class - std::unique_ptr rolloutBasePtr(new TimeTriggeredRollout(systemDynamics, rolloutSettings)); + auto rolloutPtr = std::make_unique(systemDynamics, rolloutSettings); scalar_array_t timeTrajectory; - size_array_t eventsPastTheEndIndeces; + size_array_t postEventIndices; vector_array_t stateTrajectory; vector_array_t inputTrajectory; - - rolloutBasePtr->run(initTime, initState, finalTime, &controller, modeSchedule, timeTrajectory, eventsPastTheEndIndeces, stateTrajectory, - inputTrajectory); + rolloutPtr->run(initTime, initState, finalTime, &controller, modeSchedule, timeTrajectory, postEventIndices, stateTrajectory, inputTrajectory); // check sizes const auto totalSize = timeTrajectory.size(); diff --git a/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp b/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp index c2c0cc755..49202d81a 100644 --- a/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp +++ b/ocs2_oc/test/trajectory_adjustment/TrajectorySpreadingTest.cpp @@ -52,6 +52,16 @@ struct Result { ocs2::size_array_t preEventModeTrajectory; }; +std::size_t modeAtTime(const ocs2::ModeSchedule& modeSchedule, ocs2::scalar_t time, bool isFinalTime) { + if (isFinalTime) { + auto timeItr = std::upper_bound(modeSchedule.eventTimes.begin(), modeSchedule.eventTimes.end(), time); + int modeIndex = std::distance(modeSchedule.eventTimes.begin(), timeItr); + return modeSchedule.modeSequence[modeIndex]; + } else { + return modeSchedule.modeAtTime(time); + } +} + class TrajectorySpreadingTest : public testing::Test { protected: static constexpr size_t STATE_DIM = 2; @@ -99,8 +109,9 @@ class TrajectorySpreadingTest : public testing::Test { [&out](size_t eventIndex) { return out.stateTrajectory[eventIndex - 1]; }); out.modeTrajectory.resize(out.timeTrajectory.size()); - std::transform(out.timeTrajectory.begin(), out.timeTrajectory.end(), out.modeTrajectory.begin(), - [&modeSchedule](ocs2::scalar_t time) { return modeSchedule.modeAtTime(time); }); + for (int i = 0; i < out.modeTrajectory.size(); i++) { + out.modeTrajectory[i] = modeAtTime(modeSchedule, out.timeTrajectory[i], i == out.modeTrajectory.size() - 1 ? true : false); + } out.preEventModeTrajectory.resize(out.postEventsIndeces.size()); std::transform(out.postEventsIndeces.begin(), out.postEventsIndeces.end(), out.preEventModeTrajectory.begin(), @@ -194,17 +205,22 @@ class TrajectorySpreadingTest : public testing::Test { const ocs2::scalar_t finalTime = spreadResult.timeTrajectory.back(); const auto startEventItr = std::upper_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), initTime); - // If a time point is aligned with(the same as) an event time, it belongs to the pre-mode. - const auto endEventItr = std::lower_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), finalTime); - - ocs2::size_array_t postEventIndice(endEventItr - startEventItr); - std::transform(startEventItr, endEventItr, postEventIndice.begin(), [&spreadResult](ocs2::scalar_t time) { - auto timeItr = std::upper_bound(spreadResult.timeTrajectory.begin(), spreadResult.timeTrajectory.end(), time); - return std::distance(spreadResult.timeTrajectory.begin(), timeItr); - }); + // If the final time is aligned with(the same as) an event time, it belongs to the post-mode. + const auto endEventItr = std::upper_bound(updatedModeSchedule.eventTimes.begin(), updatedModeSchedule.eventTimes.end(), finalTime); + + ocs2::size_array_t postEventIndices(endEventItr - startEventItr); + for (auto itr = startEventItr; itr != endEventItr; itr++) { + int index = std::distance(startEventItr, itr); + if (itr == endEventItr - 1 && *itr == spreadResult.timeTrajectory.back()) { + postEventIndices[index] = spreadResult.timeTrajectory.size() - 1; + } else { + auto timeItr = std::upper_bound(spreadResult.timeTrajectory.begin(), spreadResult.timeTrajectory.end(), *itr); + postEventIndices[index] = std::distance(spreadResult.timeTrajectory.begin(), timeItr); + } + } - EXPECT_TRUE(spreadResult.postEventsIndeces.size() == postEventIndice.size()); - EXPECT_TRUE(std::equal(postEventIndice.begin(), postEventIndice.end(), spreadResult.postEventsIndeces.begin())); + EXPECT_TRUE(spreadResult.postEventsIndeces.size() == postEventIndices.size()); + EXPECT_TRUE(std::equal(postEventIndices.begin(), postEventIndices.end(), spreadResult.postEventsIndeces.begin())); } else { EXPECT_EQ(spreadResult.postEventsIndeces.size(), 0); @@ -220,9 +236,10 @@ class TrajectorySpreadingTest : public testing::Test { auto eventIndexActualItr = spreadResult.postEventsIndeces.begin(); auto eventTimeReferenceInd = ocs2::lookup::findIndexInTimeArray(updatedModeSchedule.eventTimes, period.first); for (size_t k = 0; k < spreadResult.timeTrajectory.size(); k++) { - // time should be monotonic sequence - if (k > 0) { - EXPECT_TRUE(spreadResult.timeTrajectory[k - 1] < spreadResult.timeTrajectory[k]); + // Time should be monotonic sequence except the final time. It is possible that the last two time points have the same time, but one + // stands for pre-mode and the other stands for post-mode. + if (k > 0 && k < spreadResult.timeTrajectory.size() - 1) { + EXPECT_TRUE(spreadResult.timeTrajectory[k - 1] < spreadResult.timeTrajectory[k]) << "TimeIndex: " << k; } // Pre-event time should be equal to the event time @@ -238,7 +255,9 @@ class TrajectorySpreadingTest : public testing::Test { eventIndexActualItr++; } // mode should match the given modeSchedule - EXPECT_TRUE(updatedModeSchedule.modeAtTime(spreadResult.timeTrajectory[k]) == spreadResult.modeTrajectory[k]); + auto updatedMode = + modeAtTime(updatedModeSchedule, spreadResult.timeTrajectory[k], k == spreadResult.timeTrajectory.size() - 1 ? true : false); + EXPECT_TRUE(updatedMode == spreadResult.modeTrajectory[k]); } // end of k loop // test postEventsIndeces @@ -319,6 +338,48 @@ TEST_F(TrajectorySpreadingTest, partially_matching_modes) { EXPECT_TRUE(status.willPerformTrajectorySpreading); } +TEST_F(TrajectorySpreadingTest, final_time_is_the_same_as_event_time_1) { + const ocs2::scalar_array_t eventTimes{1.1, 1.3}; + const ocs2::size_array_t modeSequence{0, 1, 2}; + + const ocs2::scalar_array_t updatedEventTimes{1.1, 2.1}; + const ocs2::size_array_t updatedModeSequence{0, 1, 2}; + + const std::pair period{0.2, 2.1}; + const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period); + + EXPECT_FALSE(status.willTruncate); + EXPECT_TRUE(status.willPerformTrajectorySpreading); +} + +TEST_F(TrajectorySpreadingTest, final_time_is_the_same_as_event_time_2) { + const ocs2::scalar_array_t eventTimes{1.1, 2.1}; + const ocs2::size_array_t modeSequence{0, 1, 2}; + + const ocs2::scalar_array_t updatedEventTimes{1.1, 1.3}; + const ocs2::size_array_t updatedModeSequence{0, 1, 2}; + + const std::pair period{0.2, 2.1}; + const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period); + + EXPECT_FALSE(status.willTruncate); + EXPECT_TRUE(status.willPerformTrajectorySpreading); +} + +TEST_F(TrajectorySpreadingTest, erase_trajectory) { + const ocs2::scalar_array_t eventTimes{1.1, 1.3}; + const ocs2::size_array_t modeSequence{0, 1, 2}; + + const ocs2::scalar_array_t updatedEventTimes{1.1, 1.3}; + const ocs2::size_array_t updatedModeSequence{0, 1, 3}; + + const std::pair period{0.2, 2.1}; + const auto status = checkResults({eventTimes, modeSequence}, {updatedEventTimes, updatedModeSequence}, period); + + EXPECT_TRUE(status.willTruncate); + EXPECT_FALSE(status.willPerformTrajectorySpreading); +} + TEST_F(TrajectorySpreadingTest, fully_matched_modes) { const ocs2::scalar_array_t eventTimes{1.1, 1.3}; const ocs2::size_array_t modeSequence{0, 1, 2}; diff --git a/ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h b/ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h index 01878f38c..978a22170 100644 --- a/ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h +++ b/ocs2_ocs2/include/ocs2_ocs2/GSLQPSolver.h @@ -56,8 +56,6 @@ class GSLQSolver { typedef typename DIMENSIONS::MP_Options MP_Options_t; typedef typename DIMENSIONS::scalar_t scalar_t; typedef typename DIMENSIONS::scalar_array_t scalar_array_t; - typedef typename DIMENSIONS::eigen_scalar_t eigen_scalar_t; - typedef typename DIMENSIONS::eigen_scalar_array_t eigen_scalar_array_t; typedef typename DIMENSIONS::state_vector_t state_vector_t; typedef typename DIMENSIONS::state_vector_array_t state_vector_array_t; typedef typename DIMENSIONS::input_vector_t input_vector_t; diff --git a/ocs2_perceptive/CMakeLists.txt b/ocs2_perceptive/CMakeLists.txt new file mode 100644 index 000000000..509406a35 --- /dev/null +++ b/ocs2_perceptive/CMakeLists.txt @@ -0,0 +1,116 @@ +cmake_minimum_required(VERSION 3.0) +project(ocs2_perceptive) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_core + ocs2_robotic_tools +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem +) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +# Generate compile_commands.json for clang tools +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIRS} + LIBRARIES + ${PROJECT_NAME} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + DEPENDS + Boost +) + +############# +## Build ## +############# +include_directories( + include + ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/end_effector/EndEffectorDistanceConstraint.cpp + src/end_effector/EndEffectorDistanceConstraintCppAd.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) + +add_executable(${PROJECT_NAME}_lintTarget + src/lintTarget.cpp +) + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + add_clang_tooling( + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/test + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR + ) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# + +install( + DIRECTORY + include/${PROJECT_NAME}/ + DESTINATION + ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +############# +## Testing ## +############# +## Info ============================== +## to run tests, cd package folder and run +## $ catkin build -DCMAKE_BUILD_TYPE=RelWithDebInfo --this +## $ catkin run_tests --no-deps --this +## to see the summary of unit test results run +## $ catkin_test_results ../../../build/ocs2_perceptive + +catkin_add_gtest(test_bilinear_interpolation + test/interpolation/testBilinearInterpolation.cpp +) +target_link_libraries(test_bilinear_interpolation + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + gtest_main +) + +catkin_add_gtest(test_trilinear_interpolation + test/interpolation/testTrilinearInterpolation.cpp +) +target_link_libraries(test_trilinear_interpolation + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} + gtest_main +) diff --git a/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h new file mode 100644 index 000000000..4c419d822 --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/ComputeDistanceTransform.h @@ -0,0 +1,84 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +namespace ocs2 { + +/** + * Computes one-dimensional distance transform for a sampled function based on lower envelope of parabolas method introduced by: + * P. F. Felzenszwalb and D. P. Huttenlocher. "Distance transforms of sampled functions." (2012). + * + * @param numSamples: The size of the sampled function. + * @param getValue: Lambda function to get value from with signature: ScalarType(size_t index). + * @param setValue: Lambda function to set value to with signature: void(size_t index, ScalarType value). + * @param start: The start index for iteration over. + * @param end: The index of the element following the last element for iteration over. + * @param vBuffer: A buffered memory of unsigned integers with size "numSamples". This memory holds the information + * about the locations of parabolas in lower envelope. + * @param zBuffer: A buffered memory of Scalar type with size "numSamples + 1". This memory holds the information + * about the locations of boundaries between parabolas. + * + * @tparam GetValFunc: Template typename for inferring lambda expression with signature Scalar(size_t). + * @tparam SetValFunc: Template typename for inferring lambda expression with signature void(size_t, Scalar). + * @tparam Scalar: The Scalar type + */ +template +void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, size_t start, size_t end, + std::vector& vBuffer, std::vector& zBuffer); + +/** + * Computes one-dimensional distance transform for a sampled function based on lower envelope of parabolas method introduced by: + * P. F. Felzenszwalb and D. P. Huttenlocher. "Distance transforms of sampled functions." (2012). + * + * @param numSamples: The size of the sampled function. + * @param getValue: Lambda function to get value from with signature: ScalarType(size_t index). + * @param setValue: Lambda function to set value to with signature: void(size_t index, ScalarType value). + * @param setImageIndex: Lambda function to set mirror index with signature: void(size_t index, size_t mirrorIndex). + * @param start: The start index for iteration over. + * @param end: The index of the element following the last element for iteration over. + * @param vBuffer: A buffered memory of unsigned integers with size "numSamples". This memory holds the information + * about the locations of parabolas in lower envelope. + * @param zBuffer: A buffered memory of Scalar type with size "numSamples + 1". This memory holds the information + * about the locations of boundaries between parabolas. + * + * @tparam GetValFunc: Template typename for inferring lambda expression with signature Scalar(size_t). + * @tparam SetValFunc: Template typename for inferring lambda expression with signature void(size_t, Scalar). + * @tparam SetImageIndexFunc: Template typename for inferring lambda expression with signature void(size_t, size_t). + * @tparam Scalar: The Scalar type + */ +template +void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, SetImageIndexFunc&& setImageIndex, + size_t start, size_t end, std::vector& vBuffer, std::vector& zBuffer); + +} // namespace ocs2 + +#include "implementation/ComputeDistanceTransform.h" diff --git a/ocs2_perceptive/include/ocs2_perceptive/distance_transform/DistanceTransformInterface.h b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/DistanceTransformInterface.h new file mode 100644 index 000000000..d49e9395d --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/DistanceTransformInterface.h @@ -0,0 +1,72 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include + +namespace ocs2 { + +/** Distance field which defines a 3D point distance to the "nearest" surface and the gradient of the field. */ +class DistanceTransformInterface { + public: + using vector3_t = Eigen::Matrix; + + DistanceTransformInterface() = default; + virtual ~DistanceTransformInterface() = default; + + DistanceTransformInterface(const DistanceTransformInterface&) = delete; + DistanceTransformInterface& operator=(const DistanceTransformInterface&) = delete; + DistanceTransformInterface(DistanceTransformInterface&&) = default; + DistanceTransformInterface& operator=(DistanceTransformInterface&&) = default; + + /** Gets the distance to the given point. */ + virtual scalar_t getValue(const vector3_t& p) const = 0; + + /** Projects the given point to the nearest point on the surface. */ + virtual vector3_t getProjectedPoint(const vector3_t& p) const = 0; + + /** Gets the distance's value and its gradient at the given point. */ + virtual std::pair getLinearApproximation(const vector3_t& p) const = 0; +}; + +/** Identity distance transform with constant zero value and zero gradients. */ +class IdentityDistanceTransform : public DistanceTransformInterface { + public: + IdentityDistanceTransform() = default; + ~IdentityDistanceTransform() override = default; + + scalar_t getValue(const vector3_t&) const override { return 0.0; } + vector3_t getProjectedPoint(const vector3_t& p) const override { return p; } + std::pair getLinearApproximation(const vector3_t&) const override { return {0.0, vector3_t::Zero()}; } +}; + +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/distance_transform/implementation/ComputeDistanceTransform.h b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/implementation/ComputeDistanceTransform.h new file mode 100644 index 000000000..47d1459fa --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/distance_transform/implementation/ComputeDistanceTransform.h @@ -0,0 +1,99 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, size_t start, size_t end, + std::vector& vBuffer, std::vector& zBuffer) { + computeDistanceTransform( + numSamples, std::forward(getValue), std::forward(setValue), [&](size_t x, size_t ind) {}, start, end, vBuffer, + zBuffer); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +void computeDistanceTransform(size_t numSamples, GetValFunc&& getValue, SetValFunc&& setValue, SetImageIndexFunc&& setImageIndex, + size_t start, size_t end, std::vector& vBuffer, std::vector& zBuffer) { + constexpr auto PlusInfinity = std::numeric_limits::max(); + constexpr auto MinusInfinity = std::numeric_limits::lowest(); + + // make sure the buffer memories has the right size + if (vBuffer.size() < numSamples) { + vBuffer.resize(numSamples); // locations of parabolas in lower envelope + } + if (zBuffer.size() < numSamples + 1) { + zBuffer.resize(numSamples + 1); // locations of boundaries between parabolas + } + + // initialization + vBuffer[start] = start; + zBuffer[start] = MinusInfinity; + zBuffer[start + 1] = PlusInfinity; + size_t k = start; // index of rightmost parabola in lower envelope + + // compute lower envelope + for (size_t q = start + 1; q < end; q++) { + k++; // compensates for first line of next do-while block + + Scalar s = 0; + do { + k--; + // compute horizontal position of intersection between the parabola from q & the current lowest parabola + s = ((getValue(q) + q * q) - (getValue(vBuffer[k]) + vBuffer[k] * vBuffer[k])) / (2 * q - 2 * vBuffer[k]); + } while (s <= zBuffer[k]); + + k++; + vBuffer[k] = q; + zBuffer[k] = s; + zBuffer[k + 1] = PlusInfinity; + } // end of for loop + + // fill in values of distance transform + k = start; + for (size_t q = start; q < end; q++) { + while (zBuffer[k + 1] < q) { + k++; + } + const size_t ind = vBuffer[k]; + const Scalar val = (q - ind) * (q - ind) + getValue(ind); + setValue(q, val); + setImageIndex(q, ind); + } // end of for loop +} + +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h b/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h new file mode 100644 index 000000000..ef595264d --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h @@ -0,0 +1,86 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include +#include + +#include "ocs2_perceptive/distance_transform/DistanceTransformInterface.h" + +namespace ocs2 { + +/** + * End-effector distance constraint Function. + */ +class EndEffectorDistanceConstraint final : public ocs2::StateConstraint { + public: + /** Constructor */ + EndEffectorDistanceConstraint(size_t stateDim, scalar_t weight, std::unique_ptr> kinematicsPtr); + + /** Constructor */ + EndEffectorDistanceConstraint(size_t stateDim, scalar_t weight, std::unique_ptr> kinematicsPtr, + scalar_array_t clearances); + + /** Default destructor */ + ~EndEffectorDistanceConstraint() override = default; + + void set(const DistanceTransformInterface& distanceTransform); + void set(scalar_t clearance, const DistanceTransformInterface& distanceTransform); + void set(const scalar_array_t& clearances, const DistanceTransformInterface& distanceTransform); + + EndEffectorDistanceConstraint* clone() const override { return new EndEffectorDistanceConstraint(*this); } + size_t getNumConstraints(scalar_t time) const override { return kinematicsPtr_->getIds().size(); } + const std::vector& getIDs() const { return kinematicsPtr_->getIds(); } + + vector_t getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const override; + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const override; + + size_t getStateDimensions() const { return stateDim_; } + EndEffectorKinematics& getKinematics() { return *kinematicsPtr_; } + + private: + EndEffectorDistanceConstraint(const EndEffectorDistanceConstraint& other); + + const size_t stateDim_; + const scalar_t weight_; + + std::unique_ptr> kinematicsPtr_; + + scalar_array_t clearances_; + const DistanceTransformInterface* distanceTransformPtr_ = nullptr; +}; + +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h b/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h new file mode 100644 index 000000000..c4b4c7e40 --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h @@ -0,0 +1,95 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +#include "ocs2_perceptive/distance_transform/DistanceTransformInterface.h" + +namespace ocs2 { + +/** + * End-effector distance constraint Function. + */ +class EndEffectorDistanceConstraintCppAd final : public ocs2::StateInputConstraint { + public: + struct Config { + Config(scalar_t weightParam = 1.0, bool generateModelParam = true, bool verboseParam = true) + : weight(weightParam), generateModel(generateModelParam), verbose(verboseParam) {} + scalar_t weight; + bool generateModel; + bool verbose; + }; + + /** Constructor */ + EndEffectorDistanceConstraintCppAd(size_t stateDim, size_t inputDim, Config config, + std::unique_ptr> adKinematicsPtr); + + /** Default destructor */ + ~EndEffectorDistanceConstraintCppAd() override = default; + + void set(scalar_t clearance, const DistanceTransformInterface& distanceTransform); + void set(vector_t clearances, const DistanceTransformInterface& distanceTransform); + + EndEffectorDistanceConstraintCppAd* clone() const override { return new EndEffectorDistanceConstraintCppAd(*this); } + size_t getNumConstraints(scalar_t time) const override { return adKinematicsPtr_->getIds().size(); } + const std::vector& getIDs() const { return adKinematicsPtr_->getIds(); } + + vector_t getValue(scalar_t time, const vector_t& state, const vector_t& input, const PreComputation& preComp) const override; + VectorFunctionLinearApproximation getLinearApproximation(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const override; + VectorFunctionQuadraticApproximation getQuadraticApproximation(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const override; + + EndEffectorKinematics& getKinematicsAD() { return *adKinematicsPtr_; } + std::pair getStateInputDimensions() const { return {stateDim_, inputDim_}; } + + private: + EndEffectorDistanceConstraintCppAd(const EndEffectorDistanceConstraintCppAd& other); + + const size_t stateDim_; + const size_t inputDim_; + const Config config_; + + std::unique_ptr> adKinematicsPtr_; + std::unique_ptr kinematicsModelPtr_; + + vector_t clearances_; + const DistanceTransformInterface* distanceTransformPtr_ = nullptr; +}; + +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h new file mode 100644 index 000000000..27afa995b --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/BilinearInterpolation.h @@ -0,0 +1,71 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include + +namespace ocs2 { +namespace bilinear_interpolation { + +/** + * Compute the value of a function at a queried position using bi-linear interpolation on a 2D-grid. + * + * @param resolution The resolution of the grid. + * @param referenceCorner The reference position on the 2-D grid closest to the point. + * @param cornerValues The values around the reference corner, in the order: (0, 0), (1, 0), (0, 1), (1, 1). + * @param position The queried position. + * @tparam Scalar : The Scalar type. + * @return Scalar : The interpolated function's value at the queried position. + */ +template +Scalar getValue(Scalar resolution, const Eigen::Matrix& referenceCorner, const std::array& cornerValues, + const Eigen::Matrix& position); + +/** + * Computes a first-order approximation of the function at a queried position using bi-linear interpolation on a 2D-grid. + * + * @param resolution The resolution of the grid. + * @param referenceCorner The reference position on the 2-D grid closest to the point. + * @param cornerValues The values around the reference corner, in the order: (0, 0), (1, 0), (0, 1), (1, 1). + * @param position The queried position. + * @tparam Scalar : The Scalar type. + * @return std::pair> : A tuple containing the value and jacobian. + */ +template +std::pair> getLinearApproximation(Scalar resolution, const Eigen::Matrix& referenceCorner, + const std::array& cornerValues, + const Eigen::Matrix& position); + +} // namespace bilinear_interpolation +} // namespace ocs2 + +#include "implementation/BilinearInterpolation.h" diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/TrilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/TrilinearInterpolation.h new file mode 100644 index 000000000..0d468fdbb --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/TrilinearInterpolation.h @@ -0,0 +1,73 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include + +namespace ocs2 { +namespace trilinear_interpolation { + +/** + * Compute the value of a function at a queried position using tri-linear interpolation on a 3D-grid. + * + * @param resolution The resolution of the grid. + * @param referenceCorner The reference position on the 3-D grid closest to the point. + * @param cornerValues The values around the reference corner, in the order: + * (0, 0, 0), (1, 0, 0), (0, 1, 0), (1, 1, 0), (0, 0, 1), (1, 0, 1), (0, 1, 1), (1, 1, 1). + * @param position The queried position. + * @tparam Scalar : The Scalar type. + * @return Scalar : The interpolated function's value at the queried position. + */ +template +Scalar getValue(Scalar resolution, const Eigen::Matrix& referenceCorner, const std::array& cornerValues, + const Eigen::Matrix& position); + +/** + * Computes a first-order approximation of the function at a queried position using tri-linear interpolation on a 3D-grid. + * + * @param resolution The resolution of the grid. + * @param referenceCorner The reference position on the 3-D grid closest to the point. + * @param cornerValues The values around the reference corner, in the order: + * (0, 0, 0), (1, 0, 0), (0, 1, 0), (1, 1, 0), (0, 0, 1), (1, 0, 1), (0, 1, 1), (1, 1, 1). + * @param position The queried position. + * @tparam Scalar : The Scalar type. + * @return std::pair> : A tuple containing the value and jacobian. + */ +template +std::pair> getLinearApproximation(Scalar resolution, const Eigen::Matrix& referenceCorner, + const std::array& cornerValues, + const Eigen::Matrix& position); + +} // namespace trilinear_interpolation +} // namespace ocs2 + +#include "implementation/TrilinearInterpolation.h" diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/BilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/BilinearInterpolation.h new file mode 100644 index 000000000..194fb73af --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/BilinearInterpolation.h @@ -0,0 +1,84 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +namespace ocs2 { +namespace bilinear_interpolation { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +Scalar getValue(Scalar resolution, const Eigen::Matrix& referenceCorner, const std::array& cornerValues, + const Eigen::Matrix& position) { + // auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array v; + v[0] = (position.x() - referenceCorner.x()) * r_inv; + v[1] = (position.y() - referenceCorner.y()) * r_inv; + v[2] = 1 - v[0]; + v[3] = 1 - v[1]; + // (1 - x)(1 - y) f_00 + x (1 - y) f_10 + (1 - x) y f_01 + x y f_11 + return cornerValues[0] * v[2] * v[3] + cornerValues[1] * v[0] * v[3] + cornerValues[2] * v[2] * v[1] + cornerValues[3] * v[0] * v[1]; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +std::pair> getLinearApproximation(Scalar resolution, const Eigen::Matrix& referenceCorner, + const std::array& cornerValues, + const Eigen::Matrix& position) { + // auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array v; + v[0] = (position.y() - referenceCorner.y()) * r_inv; + v[1] = (position.x() - referenceCorner.x()) * r_inv; + v[2] = 1.0 - v[0]; + v[3] = 1.0 - v[1]; + v[4] = cornerValues[2] * v[3]; // (1 - x) f_01 + v[5] = cornerValues[3] * v[1]; // x f_11 + v[3] = cornerValues[0] * v[3]; // (1 - x) f_00 + v[1] = cornerValues[1] * v[1]; // x f_10 + + // x ( 1 - y) f_10 + (1 - x) (1 - y) f_00 + (1 - x) y f_01 + x y f_11 + const Scalar value = v[1] * v[2] + v[3] * v[2] + v[4] * v[0] + v[5] * v[0]; + + Eigen::Matrix gradient; + // (1 - y) (f_10 - f_00) + y (f_11 - f_01) + gradient.x() = (v[2] * (cornerValues[1] - cornerValues[0]) + v[0] * (cornerValues[3] - cornerValues[2])) * r_inv; + // (1 - x) (f_01 - f_00) + x (f_11 - f_10) + gradient.y() = (v[4] + v[5] - (v[3] + v[1])) * r_inv; + + return {value, gradient}; +} + +} // namespace bilinear_interpolation +} // namespace ocs2 diff --git a/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/TrilinearInterpolation.h b/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/TrilinearInterpolation.h new file mode 100644 index 000000000..17df07b2d --- /dev/null +++ b/ocs2_perceptive/include/ocs2_perceptive/interpolation/implementation/TrilinearInterpolation.h @@ -0,0 +1,98 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +namespace ocs2 { +namespace trilinear_interpolation { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +Scalar getValue(Scalar resolution, const Eigen::Matrix& referenceCorner, const std::array& cornerValues, + const Eigen::Matrix& position) { + // auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array v; + v[0] = (position.x() - referenceCorner.x()) * r_inv; + v[1] = (position.y() - referenceCorner.y()) * r_inv; + v[2] = (position.z() - referenceCorner.z()) * r_inv; + v[3] = 1 - v[0]; + v[4] = 1 - v[1]; + v[5] = 1 - v[2]; + v[6] = v[3] * cornerValues[0] + v[0] * cornerValues[1]; // f_00 = (1 - x) f_000 + x f_100 + v[7] = v[3] * cornerValues[2] + v[0] * cornerValues[3]; // f_10 = (1 - x) f_010 + x f_110 + v[8] = v[3] * cornerValues[4] + v[0] * cornerValues[5]; // f_01 = (1 - x) f_001 + x f_101 + v[9] = v[3] * cornerValues[6] + v[0] * cornerValues[7]; // f_11 = (1 - x) f_011 + x f_111 + // (1 - z) ((1 - y) f_00 + y f_10) + z ((1 - y) f_01 + y f_11) + return v[5] * (v[4] * v[6] + v[1] * v[7]) + v[2] * (v[4] * v[8] + v[1] * v[9]); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +template +std::pair> getLinearApproximation(Scalar resolution, const Eigen::Matrix& referenceCorner, + const std::array& cornerValues, + const Eigen::Matrix& position) { + /// auxiliary variables + const Scalar r_inv = 1.0 / resolution; + std::array v; + v[0] = (position.x() - referenceCorner.x()) * r_inv; + v[1] = (position.y() - referenceCorner.y()) * r_inv; + v[2] = (position.z() - referenceCorner.z()) * r_inv; + v[3] = 1 - v[0]; + v[4] = 1 - v[1]; + v[5] = 1 - v[2]; + v[6] = v[3] * cornerValues[0] + v[0] * cornerValues[1]; // f_00 = (1 - x) f_000 + x f_100 + v[7] = v[3] * cornerValues[2] + v[0] * cornerValues[3]; // f_10 = (1 - x) f_010 + x f_110 + v[8] = v[3] * cornerValues[4] + v[0] * cornerValues[5]; // f_01 = (1 - x) f_001 + x f_101 + v[9] = v[3] * cornerValues[6] + v[0] * cornerValues[7]; // f_11 = (1 - x) f_011 + x f_111 + v[10] = v[4] * v[6] + v[1] * v[7]; // f_0 = (1 - y) f_00 + y f_10 + v[11] = v[4] * v[8] + v[1] * v[9]; // f_1 = (1 - y) f_01 + y f_11 + + // f = (1 - z) f_0 + z f_1 + const Scalar value = v[5] * v[10] + v[2] * v[11]; + + Eigen::Matrix gradient; + // df_z = f_11 - f_10 + gradient.z() = (v[11] - v[10]) * r_inv; + // df_y = (1 - z) (f_10 - f00) - z (f_11 - f_01) + gradient.y() = (v[5] * (v[7] - v[6]) + v[2] * (v[9] - v[8])) * r_inv; + // df_x = (1 - z) (1 - y) (f_100 - f_000) + (1 - z) y (f_110 - f_010) + z (1 - y) (f_101 - f_001) + z y (f_111 - f_011) + gradient.x() = (v[5] * v[4] * (cornerValues[1] - cornerValues[0]) + v[5] * v[1] * (cornerValues[3] - cornerValues[2]) + + v[2] * v[4] * (cornerValues[5] - cornerValues[4]) + v[2] * v[1] * (cornerValues[7] - cornerValues[6])) * + r_inv; + + return {value, gradient}; +} + +} // namespace trilinear_interpolation +} // namespace ocs2 diff --git a/ocs2_perceptive/package.xml b/ocs2_perceptive/package.xml new file mode 100644 index 000000000..a85af33ff --- /dev/null +++ b/ocs2_perceptive/package.xml @@ -0,0 +1,16 @@ + + + ocs2_perceptive + 0.0.0 + The ocs2_perceptive package + + Farbod Farshidian + + BSD + + catkin + cmake_clang_tools + ocs2_core + ocs2_robotic_tools + + diff --git a/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraint.cpp b/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraint.cpp new file mode 100644 index 000000000..f8a51ddb6 --- /dev/null +++ b/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraint.cpp @@ -0,0 +1,136 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_perceptive/end_effector/EndEffectorDistanceConstraint.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraint::EndEffectorDistanceConstraint(size_t stateDim, scalar_t weight, + std::unique_ptr> kinematicsPtr) + : StateConstraint(ConstraintOrder::Linear), + stateDim_(stateDim), + weight_(weight), + kinematicsPtr_(std::move(kinematicsPtr)), + clearances_(kinematicsPtr_->getIds().size(), 0.0) {} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraint::EndEffectorDistanceConstraint(size_t stateDim, scalar_t weight, + std::unique_ptr> kinematicsPtr, + scalar_array_t clearances) + : StateConstraint(ConstraintOrder::Linear), + stateDim_(stateDim), + weight_(weight), + kinematicsPtr_(std::move(kinematicsPtr)), + clearances_(std::move(clearances)) { + if (clearances_.size() != kinematicsPtr_->getIds().size()) { + throw std::runtime_error("[EndEffectorDistanceConstraint] clearances.size() != kinematicsPtr->getIds().size()"); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraint::EndEffectorDistanceConstraint(const EndEffectorDistanceConstraint& other) + : StateConstraint(other), + stateDim_(other.stateDim_), + weight_(other.weight_), + kinematicsPtr_(other.kinematicsPtr_->clone()), + clearances_(other.clearances_) {} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraint::set(const DistanceTransformInterface& distanceTransform) { + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraint::set(scalar_t clearance, const DistanceTransformInterface& distanceTransform) { + clearances_.assign(kinematicsPtr_->getIds().size(), clearance); + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraint::set(const scalar_array_t& clearances, const DistanceTransformInterface& distanceTransform) { + assert(clearances.size() == kinematicsPtr_->getIds().size()); + clearances_ = clearances; + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t EndEffectorDistanceConstraint::getValue(scalar_t time, const vector_t& state, const PreComputation& preComp) const { + if (distanceTransformPtr_ == nullptr) { + throw std::runtime_error("[EndEffectorDistanceConstraint] First, set the distance-transform by calling set()!"); + } + + const auto numEEs = kinematicsPtr_->getIds().size(); + const auto eePositions = kinematicsPtr_->getPosition(state); + + vector_t g(numEEs); + for (size_t i = 0; i < numEEs; i++) { + g(i) = weight_ * (distanceTransformPtr_->getValue(eePositions[i]) - clearances_[i]); + } // end of i loop + + return g; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +VectorFunctionLinearApproximation EndEffectorDistanceConstraint::getLinearApproximation(scalar_t time, const vector_t& state, + const PreComputation& preComp) const { + if (distanceTransformPtr_ == nullptr) { + throw std::runtime_error("[EndEffectorDistanceConstraint] First, set the distance-transform by calling set()!"); + } + + const auto numEEs = kinematicsPtr_->getIds().size(); + const auto eePosLinApprox = kinematicsPtr_->getPositionLinearApproximation(state); + + VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero(numEEs, stateDim_, 0); + for (size_t i = 0; i < numEEs; i++) { + const auto distanceValueGradient = distanceTransformPtr_->getLinearApproximation(eePosLinApprox[i].f); + approx.f(i) = weight_ * (distanceValueGradient.first - clearances_[i]); + approx.dfdx.row(i).noalias() = weight_ * (distanceValueGradient.second.transpose() * eePosLinApprox[i].dfdx); + } // end of i loop + + return approx; +} + +} // namespace ocs2 diff --git a/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraintCppAd.cpp b/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraintCppAd.cpp new file mode 100644 index 000000000..e1cf49993 --- /dev/null +++ b/ocs2_perceptive/src/end_effector/EndEffectorDistanceConstraintCppAd.cpp @@ -0,0 +1,160 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_perceptive/end_effector/EndEffectorDistanceConstraintCppAd.h" + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraintCppAd::EndEffectorDistanceConstraintCppAd(size_t stateDim, size_t inputDim, Config config, + std::unique_ptr> adKinematicsPtr) + : StateInputConstraint(ConstraintOrder::Linear), + stateDim_(stateDim), + inputDim_(inputDim), + config_(std::move(config)), + adKinematicsPtr_(std::move(adKinematicsPtr)) { + auto surrogateKinematicsAD = [this](const ad_vector_t& x, ad_vector_t& y) { + const size_t numEEs = adKinematicsPtr_->getIds().size(); + const auto eePositions = adKinematicsPtr_->getPosition(x); + y.resize(3 * numEEs); + for (size_t i = 0; i < numEEs; i++) { + y.segment<3>(3 * i) = eePositions[i]; + } // end of i loop + }; + + std::string libName = "ee_kinemtaics"; + for (const auto& id : adKinematicsPtr_->getIds()) { + libName += "_" + id; + } + kinematicsModelPtr_.reset(new CppAdInterface(surrogateKinematicsAD, stateDim_, libName)); + constexpr auto order = CppAdInterface::ApproximationOrder::First; + if (config_.generateModel) { + kinematicsModelPtr_->createModels(order, config_.verbose); + } else { + kinematicsModelPtr_->loadModelsIfAvailable(order, config_.verbose); + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +EndEffectorDistanceConstraintCppAd::EndEffectorDistanceConstraintCppAd(const EndEffectorDistanceConstraintCppAd& other) + : StateInputConstraint(other), + stateDim_(other.stateDim_), + inputDim_(other.inputDim_), + config_(other.config_), + adKinematicsPtr_(other.adKinematicsPtr_->clone()), + kinematicsModelPtr_(new CppAdInterface(*other.kinematicsModelPtr_)) {} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraintCppAd::set(scalar_t clearance, const DistanceTransformInterface& distanceTransform) { + const auto numEEs = adKinematicsPtr_->getIds().size(); + clearances_ = vector_t::Ones(numEEs) * clearance; + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void EndEffectorDistanceConstraintCppAd::set(vector_t clearances, const DistanceTransformInterface& distanceTransform) { + const auto numEEs = adKinematicsPtr_->getIds().size(); + if (clearances.size() != numEEs) { + throw std::runtime_error( + "[EndEffectorDistanceConstraint] The size of the input clearance vector doesn't match the number of end-effectors!"); + } + + clearances_ = clearances; + distanceTransformPtr_ = &distanceTransform; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +vector_t EndEffectorDistanceConstraintCppAd::getValue(scalar_t time, const vector_t& state, const vector_t& input, + const PreComputation& preComp) const { + const auto numEEs = adKinematicsPtr_->getIds().size(); + const auto eePositions = kinematicsModelPtr_->getFunctionValue(state); + assert(eePositions.size() == 3 * numEEs); + + vector_t g(numEEs); + for (size_t i = 0; i < numEEs; i++) { + g(i) = config_.weight * (distanceTransformPtr_->getValue(eePositions.segment<3>(3 * i)) - clearances_[i]); + } // end of i loop + + return g; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +VectorFunctionLinearApproximation EndEffectorDistanceConstraintCppAd::getLinearApproximation(scalar_t time, const vector_t& state, + const vector_t& input, + const PreComputation& preComp) const { + const auto numEEs = adKinematicsPtr_->getIds().size(); + const auto eePositions = kinematicsModelPtr_->getFunctionValue(state); + const auto eeJacobians = kinematicsModelPtr_->getJacobian(state); + assert(eePositions.size() == 3 * numEEs); + assert(eeJacobians.rows() == 3 * numEEs); + assert(eeJacobians.cols() == stateDim_); + + VectorFunctionLinearApproximation approx = VectorFunctionLinearApproximation::Zero(numEEs, stateDim_, inputDim_); + for (size_t i = 0; i < numEEs; i++) { + const auto distanceValueGradient = distanceTransformPtr_->getLinearApproximation(eePositions.segment<3>(3 * i)); + approx.f(i) = config_.weight * (distanceValueGradient.first - clearances_[i]); + approx.dfdx.row(i).noalias() = config_.weight * (distanceValueGradient.second.transpose() * eeJacobians.middleRows<3>(3 * i)); + } // end of i loop + + return approx; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +VectorFunctionQuadraticApproximation EndEffectorDistanceConstraintCppAd::getQuadraticApproximation(scalar_t time, const vector_t& state, + const vector_t& input, + const PreComputation& preComp) const { + const auto numEEs = adKinematicsPtr_->getIds().size(); + auto linearApprox = getLinearApproximation(time, state, input, preComp); + + VectorFunctionQuadraticApproximation quadraticApproximation; + quadraticApproximation.f = std::move(linearApprox.f); + quadraticApproximation.dfdx = std::move(linearApprox.dfdx); + quadraticApproximation.dfdu = std::move(linearApprox.dfdu); + quadraticApproximation.dfdxx.assign(numEEs, matrix_t::Zero(stateDim_, stateDim_)); + quadraticApproximation.dfdux.assign(numEEs, matrix_t::Zero(inputDim_, stateDim_)); + quadraticApproximation.dfduu.assign(numEEs, matrix_t::Zero(inputDim_, inputDim_)); + + return quadraticApproximation; +} + +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h b/ocs2_perceptive/src/lintTarget.cpp similarity index 65% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h rename to ocs2_perceptive/src/lintTarget.cpp index 0e7dfe93f..e78b475d7 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/ConstraintProjection.h +++ b/ocs2_perceptive/src/lintTarget.cpp @@ -27,36 +27,16 @@ 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. ******************************************************************************/ -#pragma once - -#include - -namespace ocs2 { - -/** - * Returns the linear projection - * u = Pu * \tilde{u} + Px * x + Pe - * - * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u} - * - * Implementation based on the QR decomposition - * - * @param constraint : C = dfdx, D = dfdu, e = f; - * @return Px = dfdx, Pu = dfdu, Pe = f; - */ -VectorFunctionLinearApproximation qrConstraintProjection(const VectorFunctionLinearApproximation& constraint); - -/** - * Returns the linear projection - * u = Pu * \tilde{u} + Px * x + Pe - * - * s.t. C*x + D*u + e = 0 is satisfied for any \tilde{u} - * - * Implementation based on the LU decomposition - * - * @param constraint : C = dfdx, D = dfdu, e = f; - * @return Px = dfdx, Pu = dfdu, Pe = f; - */ -VectorFunctionLinearApproximation luConstraintProjection(const VectorFunctionLinearApproximation& constraint); - -} // namespace ocs2 \ No newline at end of file +#include +#include + +#include +#include + +#include +#include + +// dummy target for clang toolchain +int main() { + return 0; +} diff --git a/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp b/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp new file mode 100644 index 000000000..9e48d0f04 --- /dev/null +++ b/ocs2_perceptive/test/interpolation/testBilinearInterpolation.cpp @@ -0,0 +1,158 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include +#include + +#include + +#include +#include + +#include "ocs2_perceptive/interpolation/BilinearInterpolation.h" + +namespace ocs2 { +namespace bilinear_interpolation { + +class TestBilinearInterpolation : public ::testing::Test { + protected: + using array4_t = std::array; + using vector2_t = Eigen::Matrix; + + TestBilinearInterpolation() { + auto adBilinearInterpolation = [](const ad_vector_t& x, const ad_vector_t& p, ad_vector_t& out) { + const ad_scalar_t resolution = p(0); + const ad_vector_t referenceCorner = (ad_vector_t(2) << p(1), p(2)).finished(); + const std::array cornerValues = {p(3), p(4), p(5), p(6)}; + + const ad_vector_t positionRed = (x.head(2) - referenceCorner) / resolution; + const ad_vector_t positionRedFlip = ad_vector_t::Ones(2) - positionRed; + + out.resize(1); + out(0) = cornerValues[0] * positionRedFlip(0) * positionRedFlip(1) + cornerValues[1] * positionRed(0) * positionRedFlip(1) + + cornerValues[2] * positionRedFlip(0) * positionRed(1) + cornerValues[3] * positionRed(0) * positionRed(1); + out(0) *= x(2); + }; + + cppadInterfacePtr.reset(new CppAdInterface(adBilinearInterpolation, variableDim, parameterDim, "TestBilinearInterpolation")); + cppadInterfacePtr->createModels(CppAdInterface::ApproximationOrder::First, verbose); + } + + scalar_t bilinearInterpolation(scalar_t resolution, const vector2_t& referenceCorner, const array4_t& cornerValues, + const vector2_t& position) { + const vector2_t positionRed = (position - referenceCorner) / resolution; + const vector2_t positionRedFlip = vector2_t::Ones() - positionRed; + const scalar_t value = cornerValues[0] * positionRedFlip.x() * positionRedFlip.y() + + cornerValues[1] * positionRed.x() * positionRedFlip.y() + + cornerValues[2] * positionRedFlip.x() * positionRed.y() + cornerValues[3] * positionRed.x() * positionRed.y(); + return value; + } + + /** parpam = (resolution, referenceCornerXY, cornerValues[0:3]) */ + vector_t toParameter(scalar_t resolution, const vector2_t& referenceCorner, const array4_t& cornerValues) { + vector_t p(7); + p(0) = resolution; + p(1) = referenceCorner.x(); + p(2) = referenceCorner.y(); + p(3) = cornerValues[0]; + p(4) = cornerValues[1]; + p(5) = cornerValues[2]; + p(6) = cornerValues[3]; + return p; + } + + static constexpr bool verbose = true; + static constexpr size_t numSamples = 100; + static constexpr size_t variableDim = 3; // (x, y, 1.0) + static constexpr size_t parameterDim = 7; // (resolution, referenceCornerXY, cornerValues[0:3]) + static constexpr scalar_t precision = 1e-6; + static constexpr scalar_t resolution = 0.05; + + std::unique_ptr cppadInterfacePtr; +}; + +constexpr bool TestBilinearInterpolation::verbose; +constexpr size_t TestBilinearInterpolation::numSamples; +constexpr size_t TestBilinearInterpolation::variableDim; +constexpr size_t TestBilinearInterpolation::parameterDim; +constexpr scalar_t TestBilinearInterpolation::precision; +constexpr scalar_t TestBilinearInterpolation::resolution; + +TEST_F(TestBilinearInterpolation, testAdFunction) { + for (size_t i = 0; i < numSamples; i++) { + const vector_t randValues = vector_t::Random(parameterDim - 1 + variableDim - 1); + const vector2_t position(randValues(0), randValues(1)); + const vector2_t referenceCorner(randValues(2), randValues(3)); + const array4_t cornerValues = {randValues(4), randValues(5), randValues(6), randValues(7)}; + + const scalar_t trueValue = bilinearInterpolation(resolution, referenceCorner, cornerValues, position); + + const vector_t input = (vector_t(3) << position, 1.0).finished(); + const vector_t param = toParameter(resolution, referenceCorner, cornerValues); + // true value + const scalar_t value = cppadInterfacePtr->getFunctionValue(input, param)(0); + // true linear approximation + const matrix_t jacAug = cppadInterfacePtr->getJacobian(input, param); + const std::pair linApprox = {jacAug(0, 2), vector2_t(jacAug(0, 0), jacAug(0, 1))}; + + EXPECT_TRUE(std::abs(trueValue - value) < precision) << "the true value is " << trueValue << " while the value is " << value; + EXPECT_TRUE(std::abs(trueValue - linApprox.first) < precision) + << "the true value is " << trueValue << " while the linApprox.first is " << linApprox.first; + } // end of i loop +} + +TEST_F(TestBilinearInterpolation, testBilinearInterpolation) { + for (size_t i = 0; i < numSamples; i++) { + const vector_t randValues = vector_t::Random(parameterDim - 1 + variableDim - 1); + const vector2_t position(randValues(0), randValues(1)); + const vector2_t referenceCorner(randValues(2), randValues(3)); + const array4_t cornerValues = {randValues(4), randValues(5), randValues(6), randValues(7)}; + + const vector_t input = (vector_t(3) << position, 1.0).finished(); + const vector_t param = toParameter(resolution, referenceCorner, cornerValues); + // true value + const scalar_t trueValue = cppadInterfacePtr->getFunctionValue(input, param)(0); + // true linear approximation + const matrix_t jacAug = cppadInterfacePtr->getJacobian(input, param); + const std::pair trueLinApprox = {jacAug(0, 2), vector2_t(jacAug(0, 0), jacAug(0, 1))}; + + // value and linear approximation + const auto value = bilinear_interpolation::getValue(resolution, referenceCorner, cornerValues, position); + const auto linApprox = bilinear_interpolation::getLinearApproximation(resolution, referenceCorner, cornerValues, position); + + EXPECT_TRUE(std::abs(trueValue - value) < precision) << "the true value is " << trueValue << " while the value is " << value; + EXPECT_TRUE(std::abs(trueValue - linApprox.first) < precision) + << "the true value is " << trueValue << " while linApprox.first is " << linApprox.first; + EXPECT_TRUE(trueLinApprox.second.isApprox(linApprox.second, precision)); + } // end of i loop +} + +} // namespace bilinear_interpolation +} // namespace ocs2 diff --git a/ocs2_perceptive/test/interpolation/testTrilinearInterpolation.cpp b/ocs2_perceptive/test/interpolation/testTrilinearInterpolation.cpp new file mode 100644 index 000000000..88e832b23 --- /dev/null +++ b/ocs2_perceptive/test/interpolation/testTrilinearInterpolation.cpp @@ -0,0 +1,180 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include +#include + +#include + +#include +#include + +#include "ocs2_perceptive/interpolation/TrilinearInterpolation.h" + +namespace ocs2 { +namespace trilinear_interpolation { + +class TestTrilinearInterpolation : public ::testing::Test { + protected: + using array8_t = std::array; + using vector3_t = Eigen::Matrix; + + TestTrilinearInterpolation() { + auto adTrilinearInterpolation = [](const ad_vector_t& x, const ad_vector_t& p, ad_vector_t& out) { + const ad_scalar_t resolution = p(0); + const ad_vector_t referenceCorner = (ad_vector_t(3) << p(1), p(2), p(3)).finished(); + const std::array cornerValues = {p(4), p(5), p(6), p(7), p(8), p(9), p(10), p(11)}; + + const ad_vector_t positionRed = (x.head(3) - referenceCorner) / resolution; + const ad_vector_t positionRedFlip = ad_vector_t::Ones(3) - positionRed; + + const ad_scalar_t v00 = positionRedFlip(0) * cornerValues[0] + positionRed(0) * cornerValues[1]; + const ad_scalar_t v10 = positionRedFlip(0) * cornerValues[2] + positionRed(0) * cornerValues[3]; + const ad_scalar_t v01 = positionRedFlip(0) * cornerValues[4] + positionRed(0) * cornerValues[5]; + const ad_scalar_t v11 = positionRedFlip(0) * cornerValues[6] + positionRed(0) * cornerValues[7]; + const ad_scalar_t v0 = positionRedFlip(1) * v00 + positionRed(1) * v10; + const ad_scalar_t v1 = positionRedFlip(1) * v01 + positionRed(1) * v11; + + out.resize(1); + out(0) = positionRedFlip(2) * v0 + positionRed(2) * v1; + out(0) *= x(3); + }; + + cppadInterfacePtr.reset(new CppAdInterface(adTrilinearInterpolation, variableDim, parameterDim, "TestTrilinearInterpolation")); + cppadInterfacePtr->createModels(CppAdInterface::ApproximationOrder::First, verbose); + } + + scalar_t trilinearInterpolation(scalar_t resolution, const vector3_t& referenceCorner, const array8_t& cornerValues, + const vector3_t& position) { + const vector3_t positionRed = (position - referenceCorner) / resolution; + const vector3_t positionRedFlip = vector3_t::Ones() - positionRed; + + const scalar_t v00 = positionRedFlip(0) * cornerValues[0] + positionRed(0) * cornerValues[1]; + const scalar_t v10 = positionRedFlip(0) * cornerValues[2] + positionRed(0) * cornerValues[3]; + const scalar_t v01 = positionRedFlip(0) * cornerValues[4] + positionRed(0) * cornerValues[5]; + const scalar_t v11 = positionRedFlip(0) * cornerValues[6] + positionRed(0) * cornerValues[7]; + const scalar_t v0 = positionRedFlip(1) * v00 + positionRed(1) * v10; + const scalar_t v1 = positionRedFlip(1) * v01 + positionRed(1) * v11; + + const scalar_t value = positionRedFlip(2) * v0 + positionRed(2) * v1; + + return value; + } + + /** param = (resolution, referenceCornerXYZ, cornerValues[0:7]) */ + vector_t toParameter(scalar_t resolution, const vector3_t& referenceCorner, const array8_t& cornerValues) { + vector_t p(12); + p(0) = resolution; + p(1) = referenceCorner.x(); + p(2) = referenceCorner.y(); + p(3) = referenceCorner.z(); + p(4) = cornerValues[0]; // f_000 + p(5) = cornerValues[1]; // f_100 + p(6) = cornerValues[2]; // f_010 + p(7) = cornerValues[3]; // f_110 + p(8) = cornerValues[4]; // f_001 + p(9) = cornerValues[5]; // f_101 + p(10) = cornerValues[6]; // f_011 + p(11) = cornerValues[7]; // f_111 + return p; + } + + static constexpr bool verbose = true; + static constexpr size_t numSamples = 100; + static constexpr size_t variableDim = 4; // (x, y, z, 1.0) + static constexpr size_t parameterDim = 12; // (resolution, referenceCornerXYZ, cornerValues[0:7]) + static constexpr scalar_t precision = 1e-6; + static constexpr scalar_t resolution = 0.05; + + std::unique_ptr cppadInterfacePtr; +}; + +constexpr bool TestTrilinearInterpolation::verbose; +constexpr size_t TestTrilinearInterpolation::numSamples; +constexpr size_t TestTrilinearInterpolation::variableDim; +constexpr size_t TestTrilinearInterpolation::parameterDim; +constexpr scalar_t TestTrilinearInterpolation::precision; +constexpr scalar_t TestTrilinearInterpolation::resolution; + +TEST_F(TestTrilinearInterpolation, testAdFunction) { + for (size_t i = 0; i < numSamples; i++) { + const vector_t randValues = vector_t::Random(parameterDim - 1 + variableDim - 1); + const vector3_t position(randValues(0), randValues(1), randValues(2)); + const vector3_t referenceCorner(randValues(3), randValues(4), randValues(5)); + const array8_t cornerValues = {randValues(6), randValues(7), randValues(8), randValues(9), + randValues(10), randValues(11), randValues(12), randValues(13)}; + + const scalar_t trueValue = trilinearInterpolation(resolution, referenceCorner, cornerValues, position); + + const vector_t input = (vector_t(4) << position, 1.0).finished(); + const vector_t param = toParameter(resolution, referenceCorner, cornerValues); + // true value + const scalar_t value = cppadInterfacePtr->getFunctionValue(input, param)(0); + // true linear approximation + const matrix_t jacAug = cppadInterfacePtr->getJacobian(input, param); + const std::pair linApprox = {jacAug(0, 3), vector3_t(jacAug(0, 0), jacAug(0, 1), jacAug(0, 2))}; + + EXPECT_TRUE(std::abs(trueValue - value) < precision) << "the true value is " << trueValue << " while the value is " << value; + EXPECT_TRUE(std::abs(trueValue - linApprox.first) < precision) + << "the true value is " << trueValue << " while the linApprox.first is " << linApprox.first; + } // end of i loop +} + +TEST_F(TestTrilinearInterpolation, testTrilinearInterpolation) { + for (size_t i = 0; i < numSamples; i++) { + const vector_t randValues = vector_t::Random(parameterDim - 1 + variableDim - 1); + const vector3_t position(randValues(0), randValues(1), randValues(2)); + const vector3_t referenceCorner(randValues(3), randValues(4), randValues(5)); + const array8_t cornerValues = {randValues(6), randValues(7), randValues(8), randValues(9), + randValues(10), randValues(11), randValues(12), randValues(13)}; + + const vector_t input = (vector_t(4) << position, 1.0).finished(); + const vector_t param = toParameter(resolution, referenceCorner, cornerValues); + // true value + const scalar_t trueValue = cppadInterfacePtr->getFunctionValue(input, param)(0); + // true linear approximation + const matrix_t jacAug = cppadInterfacePtr->getJacobian(input, param); + const std::pair trueLinApprox = {jacAug(0, 3), vector3_t(jacAug(0, 0), jacAug(0, 1), jacAug(0, 2))}; + + // value and linear approximation + const auto value = trilinear_interpolation::getValue(resolution, referenceCorner, cornerValues, position); + const auto linApprox = trilinear_interpolation::getLinearApproximation(resolution, referenceCorner, cornerValues, position); + + EXPECT_TRUE(std::abs(trueValue - value) < precision) << "the true value is " << trueValue << " while the value is " << value; + EXPECT_TRUE(std::abs(trueValue - linApprox.first) < precision) + << "the true value is " << trueValue << " while linApprox.first is " << linApprox.first; + EXPECT_TRUE(trueLinApprox.second.isApprox(linApprox.second, precision)) + << "the true value is (" << trueLinApprox.second.transpose() << ") while linApprox.second is (" << linApprox.second.transpose() + << ") "; + } // end of i loop +} + +} // namespace trilinear_interpolation +} // namespace ocs2 diff --git a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp index 21fdd2119..2d99c9789 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp +++ b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp @@ -81,7 +81,7 @@ void CentroidalModelRbdConversions::computeBaseKinematicsFromCentroidalModel(con const Vector3 derivativeEulerAnglesZyx = vPinocchio.segment<3>(3); baseVelocity.tail<3>() = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives(baseOrientation, derivativeEulerAnglesZyx); - const Matrix6 Adot = pinocchio::dccrba(model, data, qPinocchio, vPinocchio); + const auto Adot = pinocchio::dccrba(model, data, qPinocchio, vPinocchio); Vector6 centroidalMomentumRate = info.robotMass * getNormalizedCentroidalMomentumRate(pinocchioInterface_, info, input); centroidalMomentumRate.noalias() -= Adot * vPinocchio; centroidalMomentumRate.noalias() -= Aj * jointAccelerations.head(info.actuatedDofNum); diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/include/ocs2_pinocchio_interface/implementation/PinocchioInterface.h b/ocs2_pinocchio/ocs2_pinocchio_interface/include/ocs2_pinocchio_interface/implementation/PinocchioInterface.h index ef5d35668..3553b51f8 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/include/ocs2_pinocchio_interface/implementation/PinocchioInterface.h +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/include/ocs2_pinocchio_interface/implementation/PinocchioInterface.h @@ -41,7 +41,7 @@ namespace ocs2 { template PinocchioInterfaceTpl::PinocchioInterfaceTpl(const Model& model, const std::shared_ptr urdfModelPtr) { robotModelPtr_ = std::make_shared(model); - robotDataPtr_ = std::unique_ptr(new Data(*robotModelPtr_)); + robotDataPtr_ = std::make_unique(*robotModelPtr_); if (urdfModelPtr) { // This makes a copy of the urdfModelPtr, which guarantees constness of the urdf urdfModelPtr_ = std::make_shared(*urdfModelPtr); diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp index e2306a176..49a1a9ec8 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematics.cpp @@ -219,7 +219,7 @@ std::vector PinocchioEndEffectorKinematics::g pinocchio::getFrameJacobian(model, data, frameId, rf, J); const matrix_t Jqdist = (quaternionDistanceJacobian(q, referenceOrientations[i]) * angularVelocityToQuaternionTimeDerivative(q)) * J.bottomRows<3>(); - std::tie(err.dfdx, std::ignore) = mappingPtr_->getOcs2Jacobian(state, Jqdist, matrix_t::Zero(0, model.nv)); + std::tie(err.dfdx, std::ignore) = mappingPtr_->getOcs2Jacobian(state, Jqdist, matrix_t::Zero(3, model.nv)); errors.emplace_back(std::move(err)); } return errors; diff --git a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp index 0adabf6b4..a76a22329 100644 --- a/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp +++ b/ocs2_pinocchio/ocs2_pinocchio_interface/src/PinocchioEndEffectorKinematicsCppAd.cpp @@ -253,7 +253,7 @@ auto PinocchioEndEffectorKinematicsCppAd::getOrientationError(const vector_t& st -> std::vector { vector_t params(4 * endEffectorIds_.size()); for (int i = 0; i < endEffectorIds_.size(); i++) { - params.segment<4>(i) = referenceOrientations[i].coeffs(); + params.segment<4>(4 * i) = referenceOrientations[i].coeffs(); } const vector_t errorValues = orientationErrorCppAdInterfacePtr_->getFunctionValue(state, params); @@ -272,7 +272,7 @@ std::vector PinocchioEndEffectorKinematicsCpp const vector_t& state, const std::vector& referenceOrientations) const { vector_t params(4 * endEffectorIds_.size()); for (int i = 0; i < endEffectorIds_.size(); i++) { - params.segment<4>(i) = referenceOrientations[i].coeffs(); + params.segment<4>(4 * i) = referenceOrientations[i].coeffs(); } const vector_t errorValues = orientationErrorCppAdInterfacePtr_->getFunctionValue(state, params); diff --git a/ocs2_python_interface/src/PythonInterface.cpp b/ocs2_python_interface/src/PythonInterface.cpp index e6badf4eb..640e31847 100644 --- a/ocs2_python_interface/src/PythonInterface.cpp +++ b/ocs2_python_interface/src/PythonInterface.cpp @@ -202,7 +202,7 @@ vector_t PythonInterface::valueFunctionStateDerivative(scalar_t t, Eigen::Ref x, Eigen::Ref u) { problem_.preComputationPtr->request(Request::Constraint, t, x, u); - return problem_.equalityConstraintPtr->getValue(t, x, u, *problem_.preComputationPtr); + return toVector(problem_.equalityConstraintPtr->getValue(t, x, u, *problem_.preComputationPtr)); } /******************************************************************************************************/ diff --git a/ocs2_python_interface/test/testDummyPyBindings.cpp b/ocs2_python_interface/test/testDummyPyBindings.cpp index c858e00b1..836a93b8b 100644 --- a/ocs2_python_interface/test/testDummyPyBindings.cpp +++ b/ocs2_python_interface/test/testDummyPyBindings.cpp @@ -1,3 +1,31 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 @@ -25,8 +53,8 @@ class DummyInterface final : public RobotInterface { const matrix_t Q = (matrix_t(2, 2) << 1, 0, 0, 1).finished(); const matrix_t R = (matrix_t(1, 1) << 1).finished(); const matrix_t Qf = (matrix_t(2, 2) << 2, 0, 0, 2).finished(); - problem_.costPtr->add("cost", std::unique_ptr(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique(Qf)); problem_.targetTrajectoriesPtr = &targetTrajectories_; @@ -41,7 +69,7 @@ class DummyInterface final : public RobotInterface { mpc::Settings mpcSettings; ddp::Settings ddpSettings; ddpSettings.algorithm_ = ddp::Algorithm::SLQ; - return std::unique_ptr(new GaussNewtonDDP_MPC(mpcSettings, ddpSettings, *rolloutPtr_, problem_, *initializerPtr_)); + return std::make_unique(std::move(mpcSettings), std::move(ddpSettings), *rolloutPtr_, problem_, *initializerPtr_); } const OptimalControlProblem& getOptimalControlProblem() const override { return problem_; } diff --git a/ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info b/ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info index b4c9212ea..404503643 100644 --- a/ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info +++ b/ocs2_raisim/ocs2_legged_robot_raisim/config/raisim.info @@ -30,7 +30,7 @@ rollout [11] RH_KFE } - controlMode 0 ; 0: FORCE_AND_TORQUE, 1: PD_PLUS_FEEDFORWARD_TORQUE + controlMode 1 ; 0: FORCE_AND_TORQUE, 1: PD_PLUS_FEEDFORWARD_TORQUE ; PD control on torque level (if controlMode = 1) pGains diff --git a/ocs2_raisim/ocs2_legged_robot_raisim/src/LeggedRobotRaisimDummyNode.cpp b/ocs2_raisim/ocs2_legged_robot_raisim/src/LeggedRobotRaisimDummyNode.cpp index eef002bdd..cd96fab31 100644 --- a/ocs2_raisim/ocs2_legged_robot_raisim/src/LeggedRobotRaisimDummyNode.cpp +++ b/ocs2_raisim/ocs2_legged_robot_raisim/src/LeggedRobotRaisimDummyNode.cpp @@ -101,8 +101,8 @@ int main(int argc, char** argv) { CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo()); PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping, interface.modelSettings().contactNames3DoF); - std::shared_ptr leggedRobotRaisimVisualizer(new LeggedRobotRaisimVisualizer( - interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + auto leggedRobotRaisimVisualizer = std::make_shared( + interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle); leggedRobotRaisimVisualizer->updateTerrain(); // legged robot dummy @@ -123,4 +123,4 @@ int main(int argc, char** argv) { leggedRobotDummy.run(initObservation, initTargetTrajectories); return 0; -} \ No newline at end of file +} diff --git a/ocs2_raisim/ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h b/ocs2_raisim/ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h index 88888ed28..3d0311abb 100644 --- a/ocs2_raisim/ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h +++ b/ocs2_raisim/ocs2_raisim_core/include/ocs2_raisim_core/RaisimRollout.h @@ -81,6 +81,9 @@ class RaisimRollout final : public RolloutBase { //! Copy constructor RaisimRollout(const RaisimRollout& other); + //! Destructor + ~RaisimRollout() override; + void resetRollout() override { raisimRolloutSettings_.setSimulatorStateOnRolloutRunOnce_ = true; } RaisimRollout* clone() const override { return new RaisimRollout(*this); } diff --git a/ocs2_raisim/ocs2_raisim_core/src/RaisimRollout.cpp b/ocs2_raisim/ocs2_raisim_core/src/RaisimRollout.cpp index 14790e77a..122b483ec 100644 --- a/ocs2_raisim/ocs2_raisim_core/src/RaisimRollout.cpp +++ b/ocs2_raisim/ocs2_raisim_core/src/RaisimRollout.cpp @@ -94,6 +94,15 @@ RaisimRollout::RaisimRollout(const RaisimRollout& other) } } +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +RaisimRollout::~RaisimRollout() { + if (raisimRolloutSettings_.raisimServer_) { + serverPtr_->killServer(); + } +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp b/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp index 35de3dcaf..04bbee0b8 100644 --- a/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp +++ b/ocs2_raisim/ocs2_raisim_ros/src/RaisimHeightmapRosConverter.cpp @@ -83,8 +83,7 @@ std::unique_ptr RaisimHeightmapRosConverter::convertGridmapTo std::vector height(gridMap->data[0].data.rbegin(), gridMap->data[0].data.rend()); - std::unique_ptr heightMap(new raisim::HeightMap(xSamples, ySamples, xSize, ySize, centerX, centerY, height)); - return heightMap; + return std::make_unique(xSamples, ySamples, xSize, ySize, centerX, centerY, height); } void RaisimHeightmapRosConverter::publishGridmap(const raisim::HeightMap& heightMap, const std::string& frameId) { diff --git a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt index c2dfdd74f..577c679ce 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot/CMakeLists.txt @@ -7,8 +7,9 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CATKIN_PACKAGE_DEPENDENCIES pybind11_catkin ocs2_core - ocs2_ddp ocs2_mpc + ocs2_ddp + ocs2_slp ocs2_sqp ocs2_robotic_tools ocs2_python_interface diff --git a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info index 13865354a..15295ac88 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_ballbot/config/mpc/task.info @@ -12,8 +12,31 @@ templateSwitchingTimes { } -; multiple_shooting settings -multiple_shooting +; Multiple_Shooting SLP settings +slp +{ + dt 0.1 + slpIteration 5 + scalingIteration 3 + deltaTol 1e-3 + printSolverStatistics true + printSolverStatus false + printLinesearch false + integratorType RK2 + nThreads 4 + pipg + { + maxNumIterations 7000 + absoluteTolerance 1e-3 + relativeTolerance 1e-2 + lowerBoundH 0.2 + checkTerminationInterval 10 + displayShortSummary false + } +} + +; Multiple_Shooting SQP settings +sqp { dt 0.1 sqpIteration 5 diff --git a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h index a097f6bbd..f8a1388c7 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h +++ b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotInterface.h @@ -37,7 +37,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include +#include // Ballbot #include "ocs2_ballbot/dynamics/BallbotSystemDynamics.h" @@ -69,7 +70,9 @@ class BallbotInterface final : public RobotInterface { const vector_t& getInitialState() { return initialState_; } - multiple_shooting::Settings& sqpSettings() { return sqpSettings_; } + slp::Settings& slpSettings() { return slpSettings_; } + + sqp::Settings& sqpSettings() { return sqpSettings_; } ddp::Settings& ddpSettings() { return ddpSettings_; } @@ -86,7 +89,8 @@ class BallbotInterface final : public RobotInterface { private: ddp::Settings ddpSettings_; mpc::Settings mpcSettings_; - multiple_shooting::Settings sqpSettings_; + sqp::Settings sqpSettings_; + slp::Settings slpSettings_; OptimalControlProblem problem_; std::shared_ptr referenceManagerPtr_; diff --git a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotPyBindings.h b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotPyBindings.h index 9e733e466..099dea163 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotPyBindings.h +++ b/ocs2_robotic_examples/ocs2_ballbot/include/ocs2_ballbot/BallbotPyBindings.h @@ -59,9 +59,9 @@ class BallbotPyBindings final : public PythonInterface { BallbotInterface ballbotInterface(taskFile, libraryFolder); // MPC - std::unique_ptr mpcPtr( - new GaussNewtonDDP_MPC(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), - ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer())); + auto mpcPtr = + std::make_unique(ballbotInterface.mpcSettings(), ballbotInterface.ddpSettings(), ballbotInterface.getRollout(), + ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(ballbotInterface.getReferenceManagerPtr()); // Python interface diff --git a/ocs2_robotic_examples/ocs2_ballbot/package.xml b/ocs2_robotic_examples/ocs2_ballbot/package.xml index 64b1eb1b4..3c65f409c 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot/package.xml @@ -16,8 +16,9 @@ pybind11_catkin ocs2_core - ocs2_ddp ocs2_mpc + ocs2_ddp + ocs2_slp ocs2_sqp ocs2_python_interface ocs2_robotic_tools diff --git a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp index 59dfeb88d..da3adf5e5 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotInterface.cpp @@ -67,7 +67,8 @@ BallbotInterface::BallbotInterface(const std::string& taskFile, const std::strin // DDP SQP MPC settings ddpSettings_ = ddp::loadSettings(taskFile, "ddp"); mpcSettings_ = mpc::loadSettings(taskFile, "mpc"); - sqpSettings_ = multiple_shooting::loadSettings(taskFile, "multiple_shooting"); + sqpSettings_ = sqp::loadSettings(taskFile, "sqp"); + slpSettings_ = slp::loadSettings(taskFile, "slp"); /* * ReferenceManager & SolverSynchronizedModule @@ -88,8 +89,8 @@ BallbotInterface::BallbotInterface(const std::string& taskFile, const std::strin std::cerr << "R: \n" << R << "\n"; std::cerr << "Q_final:\n" << Qf << "\n"; - problem_.costPtr->add("cost", std::unique_ptr(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique(Qf)); // Dynamics bool recompileLibraries; // load the flag to generate library files from taskFile diff --git a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotSimpleExample.cpp b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotSimpleExample.cpp index c78e0180b..78421eb4f 100644 --- a/ocs2_robotic_examples/ocs2_ballbot/src/BallbotSimpleExample.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot/src/BallbotSimpleExample.cpp @@ -79,7 +79,7 @@ int main(int argc, char** argv) { * Rollout */ problem.dynamicsPtr.reset(new BallbotSystemDynamics(libraryFolder, true)); - std::unique_ptr ballbotRolloutPtr(new TimeTriggeredRollout(*problem.dynamicsPtr, rolloutSettings)); + auto ballbotRolloutPtr = std::make_unique(*problem.dynamicsPtr, rolloutSettings); /* * Cost function @@ -93,15 +93,13 @@ int main(int argc, char** argv) { vector_t xInit(STATE_DIM); loadData::loadEigenMatrix(taskFile, "initialState", xInit); - std::unique_ptr L(new QuadraticStateInputCost(Q, R)); - std::unique_ptr Phi(new QuadraticStateCost(QFinal)); - problem.costPtr->add("cost", std::move(L)); - problem.finalCostPtr->add("finalCost", std::move(Phi)); + problem.costPtr->add("cost", std::make_unique(Q, R)); + problem.finalCostPtr->add("finalCost", std::make_unique(QFinal)); /* * Initialization */ - std::unique_ptr ballbotInitializerPtr(new DefaultInitializer(INPUT_DIM)); + auto ballbotInitializerPtr = std::make_unique(INPUT_DIM); /* * Time partitioning which defines the time horizon and the number of data partitioning diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt index 5121de843..99f6d33bf 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/CMakeLists.txt @@ -14,6 +14,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_slp ocs2_ros_interfaces ocs2_robotic_tools ocs2_ballbot @@ -42,6 +43,8 @@ catkin_package( ${EIGEN3_INCLUDE_DIRS} CATKIN_DEPENDS ${CATKIN_PACKAGE_DEPENDENCIES} + LIBRARIES + ${PROJECT_NAME} DEPENDS Boost ) @@ -57,27 +60,40 @@ include_directories( ${catkin_INCLUDE_DIRS} ) +# main library +add_library(${PROJECT_NAME} + src/BallbotDummyVisualization.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) + # Mpc node -add_executable(ballbot_mpc - src/BallbotMpcNode.cpp +add_executable(ballbot_ddp + src/BallbotDdpMpcNode.cpp ) -add_dependencies(ballbot_mpc +add_dependencies(ballbot_ddp ${catkin_EXPORTED_TARGETS} ) -target_link_libraries(ballbot_mpc +target_link_libraries(ballbot_ddp ${catkin_LIBRARIES} ) -target_compile_options(ballbot_mpc PRIVATE ${OCS2_CXX_FLAGS}) +target_compile_options(ballbot_ddp PRIVATE ${OCS2_CXX_FLAGS}) # Dummy node add_executable(ballbot_dummy_test src/DummyBallbotNode.cpp - src/BallbotDummyVisualization.cpp ) add_dependencies(ballbot_dummy_test + ${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ) target_link_libraries(ballbot_dummy_test + ${PROJECT_NAME} ${catkin_LIBRARIES} ) target_compile_options(ballbot_dummy_test PRIVATE ${OCS2_CXX_FLAGS}) @@ -110,7 +126,7 @@ target_compile_options(ballbot_mpc_mrt PRIVATE ${OCS2_CXX_FLAGS}) ## SQP node for ballbot add_executable(ballbot_sqp - src/MultipleShootingBallbotNode.cpp + src/BallbotSqpMpcNode.cpp ) add_dependencies(ballbot_sqp ${catkin_EXPORTED_TARGETS} @@ -120,6 +136,17 @@ target_link_libraries(ballbot_sqp ) target_compile_options(ballbot_sqp PRIVATE ${OCS2_CXX_FLAGS}) +## SLP node for ballbot +add_executable(ballbot_slp + src/BallbotSlpMpcNode.cpp +) +add_dependencies(ballbot_slp + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(ballbot_slp + ${catkin_LIBRARIES} +) +target_compile_options(ballbot_slp PRIVATE ${OCS2_CXX_FLAGS}) ######################### ### CLANG TOOLING ### @@ -129,7 +156,9 @@ if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target ocs2_ballbot") add_clang_tooling( TARGETS - ballbot_mpc + ${PROJECT_NAME} + ballbot_ddp + ballbot_sqp ballbot_dummy_test ballbot_target SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include @@ -142,10 +171,16 @@ endif(cmake_clang_tools_FOUND) ## Install ## ############# +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} ) -install(TARGETS ballbot_mpc ballbot_dummy_test ballbot_target ballbot_sqp +install(TARGETS ballbot_ddp ballbot_dummy_test ballbot_target ballbot_sqp RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY launch rviz diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot.launch b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch similarity index 91% rename from ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot.launch rename to ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch index 1f91a76f3..0b43d96ad 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot.launch +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/launch/ballbot_ddp.launch @@ -11,7 +11,7 @@ - + + + + + + + + + + + + + + + + + + diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml index 3a9e207f5..00a7b00b6 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/package.xml @@ -24,6 +24,7 @@ ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_slp ocs2_ros_interfaces ocs2_ballbot ocs2_robotic_tools diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp similarity index 95% rename from ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp rename to ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp index c0fc7cd63..627cf6d49 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotDdpMpcNode.cpp @@ -57,8 +57,7 @@ int main(int argc, char** argv) { ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); // ROS ReferenceManager - std::shared_ptr rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp index 4d558f2ff..0c9b5aec4 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotMpcMrtNode.cpp @@ -91,8 +91,7 @@ int main(int argc, char** argv) { ballbotInterface.getOptimalControlProblem(), ballbotInterface.getInitializer()); // ROS ReferenceManager. This gives us the command interface. Requires the observations to be published - std::shared_ptr rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, ballbotInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); auto observationPublisher = nodeHandle.advertise(robotName + "_mpc_observation", 1); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp similarity index 92% rename from ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp rename to ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp index 090f3707d..606841c7f 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/MultipleShootingBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSlpMpcNode.cpp @@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include +#include #include @@ -62,8 +62,8 @@ int main(int argc, char** argv) { rosReferenceManagerPtr->subscribe(nodeHandle); // MPC - ocs2::MultipleShootingMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.sqpSettings(), ballbotInterface.getOptimalControlProblem(), - ballbotInterface.getInitializer()); + ocs2::SlpMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.slpSettings(), ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // Launch MPC ROS node diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp new file mode 100644 index 000000000..64ef42c49 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/BallbotSqpMpcNode.cpp @@ -0,0 +1,74 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include + +#include +#include +#include + +#include + +int main(int argc, char** argv) { + const std::string robotName = "ballbot"; + + // task file + std::vector programArgs{}; + ::ros::removeROSArgs(argc, argv, programArgs); + if (programArgs.size() <= 1) { + throw std::runtime_error("No task file specified. Aborting."); + } + std::string taskFileFolderName = std::string(programArgs[1]); + + // Initialize ros node + ros::init(argc, argv, robotName + "_mpc"); + ros::NodeHandle nodeHandle; + + // Robot interface + const std::string taskFile = ros::package::getPath("ocs2_ballbot") + "/config/" + taskFileFolderName + "/task.info"; + const std::string libFolder = ros::package::getPath("ocs2_ballbot") + "/auto_generated"; + ocs2::ballbot::BallbotInterface ballbotInterface(taskFile, libFolder); + + // ROS ReferenceManager + auto rosReferenceManagerPtr = std::make_shared(robotName, ballbotInterface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // MPC + ocs2::SqpMpc mpc(ballbotInterface.mpcSettings(), ballbotInterface.sqpSettings(), ballbotInterface.getOptimalControlProblem(), + ballbotInterface.getInitializer()); + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + + // Launch MPC ROS node + ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes(nodeHandle); + + // Successful exit + return 0; +} diff --git a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp index 9d567ef9a..d969c468b 100644 --- a/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp +++ b/ocs2_robotic_examples/ocs2_ballbot_ros/src/DummyBallbotNode.cpp @@ -65,8 +65,7 @@ int main(int argc, char** argv) { mrt.launchNodes(nodeHandle); // Visualization - std::shared_ptr ballbotDummyVisualization( - new ocs2::ballbot::BallbotDummyVisualization(nodeHandle)); + auto ballbotDummyVisualization = std::make_shared(nodeHandle); // Dummy ballbot ocs2::MRT_ROS_Dummy_Loop dummyBallbot(mrt, ballbotInterface.mpcSettings().mrtDesiredFrequency_, diff --git a/ocs2_robotic_examples/ocs2_cartpole/src/CartPoleInterface.cpp b/ocs2_robotic_examples/ocs2_cartpole/src/CartPoleInterface.cpp index 8e13f7faa..db0527118 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/src/CartPoleInterface.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole/src/CartPoleInterface.cpp @@ -92,8 +92,8 @@ CartPoleInterface::CartPoleInterface(const std::string& taskFile, const std::str std::cerr << "Q_final:\n" << Qf << "\n"; } - problem_.costPtr->add("cost", std::unique_ptr(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique(Qf)); // Dynamics CartPoleParameters cartPoleParameters; @@ -117,7 +117,7 @@ CartPoleInterface::CartPoleInterface(const std::string& taskFile, const std::str const vector_t e = (vector_t(numIneqConstraint) << cartPoleParameters.maxInput_, cartPoleParameters.maxInput_).finished(); const vector_t D = (vector_t(numIneqConstraint) << 1.0, -1.0).finished(); const matrix_t C = matrix_t::Zero(numIneqConstraint, STATE_DIM); - return std::unique_ptr(new LinearStateInputConstraint(e, C, D)); + return std::make_unique(e, C, D); }; problem_.inequalityLagrangianPtr->add("InputLimits", create(getConstraint(), getPenalty())); diff --git a/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp b/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp index 98ecb4aaa..87a7115b5 100644 --- a/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole/test/testCartpole.cpp @@ -39,7 +39,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include #include "ocs2_cartpole/CartPoleInterface.h" #include "ocs2_cartpole/package_path.h" @@ -74,7 +74,7 @@ class TestCartpole : public testing::TestWithParammpcSettings().timeHorizon_); // scale cost - return std::unique_ptr(new QuadraticStateCost(Qf)); + return std::make_unique(Qf); }; cartPoleInterfacePtr->optimalControlProblem().finalCostPtr->add(finalCostName, createFinalCost()); @@ -98,14 +98,14 @@ class TestCartpole : public testing::TestWithParam(new SLQ(ddpSettings, cartPoleInterfacePtr->getRollout(), - createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), - cartPoleInterfacePtr->getInitializer())); + return std::make_unique(std::move(ddpSettings), cartPoleInterfacePtr->getRollout(), + createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), + cartPoleInterfacePtr->getInitializer()); case ddp::Algorithm::ILQR: - return std::unique_ptr(new ILQR(ddpSettings, cartPoleInterfacePtr->getRollout(), - createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), - cartPoleInterfacePtr->getInitializer())); + return std::make_unique(std::move(ddpSettings), cartPoleInterfacePtr->getRollout(), + createOptimalControlProblem(PenaltyType::ModifiedRelaxedBarrierPenalty), + cartPoleInterfacePtr->getInitializer()); default: throw std::runtime_error("[TestCartpole::getAlgorithm] undefined algorithm"); @@ -199,12 +199,12 @@ TEST_P(TestCartpole, testDDP) { ddpPtr->getReferenceManager().setTargetTrajectories(initTargetTrajectories); // observer for InputLimits violation - std::unique_ptr inputLimitsObserverModulePtr(new AugmentedLagrangianObserver("InputLimits")); - inputLimitsObserverModulePtr->setMetricsCallback( + auto inputLimitsObserverModulePtr = SolverObserver::LagrangianTermObserver( + SolverObserver::Type::Intermediate, "InputLimits", [&](const scalar_array_t& timeTrajectory, const std::vector& termMetrics) { testInputLimitsViolation(timeTrajectory, termMetrics); }); - ddpPtr->addAugmentedLagrangianObserver(std::move(inputLimitsObserverModulePtr)); + ddpPtr->addSolverObserver(std::move(inputLimitsObserverModulePtr)); // run solver ddpPtr->run(0.0, cartPoleInterfacePtr->getInitialState(), timeHorizon); diff --git a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp index 8cb9e1fd1..6c932202c 100644 --- a/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_cartpole_ros/src/CartpoleMpcNode.cpp @@ -35,7 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include int main(int argc, char** argv) { const std::string robotName = "cartpole"; @@ -62,22 +62,24 @@ int main(int argc, char** argv) { cartPoleInterface.getOptimalControlProblem(), cartPoleInterface.getInitializer()); // observer for the input limits constraints - const std::string observingLagrangianTerm = "InputLimits"; - const ocs2::scalar_array_t observingTimePoints{0.0, 0.5}; - std::vector metricsTopicNames; - std::vector multiplierTopicNames; - for (const auto& t : observingTimePoints) { - const int timeMs = static_cast(t * 1000.0); - metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); - multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); - } - std::unique_ptr stateInputBoundsObserverPtr( - new ocs2::AugmentedLagrangianObserver(observingLagrangianTerm)); - stateInputBoundsObserverPtr->setMetricsCallback(ocs2::ros::createMetricsCallback( - nodeHandle, observingTimePoints, metricsTopicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation)); - stateInputBoundsObserverPtr->setMultiplierCallback(ocs2::ros::createMultiplierCallback( - nodeHandle, observingTimePoints, multiplierTopicNames, ocs2::ros::CallbackInterpolationStrategy::linear_interpolation)); - mpc.getSolverPtr()->addAugmentedLagrangianObserver(std::move(stateInputBoundsObserverPtr)); + auto createStateInputBoundsObserver = [&]() { + const std::string observingLagrangianTerm = "InputLimits"; + const ocs2::scalar_array_t observingTimePoints{0.0, 0.5}; + std::vector metricsTopicNames; + std::vector multiplierTopicNames; + for (const auto& t : observingTimePoints) { + const int timeMs = static_cast(t * 1000.0); + metricsTopicNames.push_back("metrics/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); + multiplierTopicNames.push_back("multipliers/" + observingLagrangianTerm + "/" + std::to_string(timeMs) + "MsLookAhead"); + } + auto lagrangianCallback = ocs2::ros::createLagrangianCallback(nodeHandle, observingTimePoints, metricsTopicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + auto multiplierCallback = ocs2::ros::createMultiplierCallback(nodeHandle, observingTimePoints, multiplierTopicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::LagrangianTermObserver(ocs2::SolverObserver::Type::Intermediate, observingLagrangianTerm, + std::move(lagrangianCallback), std::move(multiplierCallback)); + }; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver()); // Launch MPC ROS node ocs2::MPC_ROS_Interface mpcNode(mpc, robotName); diff --git a/ocs2_robotic_examples/ocs2_double_integrator/config/mpc/task.info b/ocs2_robotic_examples/ocs2_double_integrator/config/mpc/task.info index 56b56c267..f9a26589f 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_double_integrator/config/mpc/task.info @@ -34,9 +34,6 @@ ddp maxNumStepsPerSecond 100000 backwardPassIntegratorType ODE45 - inequalityConstraintMu 100.0 - inequalityConstraintDelta 1.1 - preComputeRiccatiTerms true useFeedbackPolicy true diff --git a/ocs2_robotic_examples/ocs2_double_integrator/include/ocs2_double_integrator/DoubleIntegratorPyBindings.h b/ocs2_robotic_examples/ocs2_double_integrator/include/ocs2_double_integrator/DoubleIntegratorPyBindings.h index f9f82726e..40ff2575e 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/include/ocs2_double_integrator/DoubleIntegratorPyBindings.h +++ b/ocs2_robotic_examples/ocs2_double_integrator/include/ocs2_double_integrator/DoubleIntegratorPyBindings.h @@ -30,9 +30,9 @@ class DoubleIntegratorPyBindings final : public PythonInterface { DoubleIntegratorInterface doubleIntegratorInterface(taskFile, libraryFolder); // MPC - std::unique_ptr mpcPtr(new GaussNewtonDDP_MPC( + auto mpcPtr = std::make_unique( doubleIntegratorInterface.mpcSettings(), doubleIntegratorInterface.ddpSettings(), doubleIntegratorInterface.getRollout(), - doubleIntegratorInterface.getOptimalControlProblem(), doubleIntegratorInterface.getInitializer())); + doubleIntegratorInterface.getOptimalControlProblem(), doubleIntegratorInterface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(doubleIntegratorInterface.getReferenceManagerPtr()); // Python interface diff --git a/ocs2_robotic_examples/ocs2_double_integrator/src/DoubleIntegratorInterface.cpp b/ocs2_robotic_examples/ocs2_double_integrator/src/DoubleIntegratorInterface.cpp index 25de87143..cf59812f5 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/src/DoubleIntegratorInterface.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator/src/DoubleIntegratorInterface.cpp @@ -88,8 +88,8 @@ DoubleIntegratorInterface::DoubleIntegratorInterface(const std::string& taskFile std::cerr << "R: \n" << R << "\n"; std::cerr << "Q_final:\n" << Qf << "\n"; - problem_.costPtr->add("cost", std::unique_ptr(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique(Qf)); // Dynamics const matrix_t A = (matrix_t(STATE_DIM, STATE_DIM) << 0.0, 1.0, 0.0, 0.0).finished(); diff --git a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp index 6eaff3204..79401cac7 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp @@ -66,8 +66,8 @@ class DoubleIntegratorIntegrationTest : public testing::Test { ddpSettings.maxNumIterations_ = 5; } - std::unique_ptr mpcPtr(new GaussNewtonDDP_MPC(mpcSettings, ddpSettings, interface.getRollout(), - interface.getOptimalControlProblem(), interface.getInitializer())); + auto mpcPtr = std::make_unique(std::move(mpcSettings), std::move(ddpSettings), interface.getRollout(), + interface.getOptimalControlProblem(), interface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(interface.getReferenceManagerPtr()); return mpcPtr; @@ -153,6 +153,7 @@ TEST_F(DoubleIntegratorIntegrationTest, coldStartMPC) { ASSERT_NEAR(observation.state(0), goalState(0), tolerance); } +#ifdef NDEBUG TEST_F(DoubleIntegratorIntegrationTest, asynchronousTracking) { auto mpcPtr = getMpc(true); MPC_MRT_Interface mpcInterface(*mpcPtr); @@ -207,3 +208,4 @@ TEST_F(DoubleIntegratorIntegrationTest, asynchronousTracking) { ASSERT_NEAR(observation.state(0), goalState(0), tolerance); } +#endif diff --git a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp index 0963bf8a8..1e5d5c1f9 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator_ros/src/DoubleIntegratorMpcNode.cpp @@ -59,8 +59,7 @@ int main(int argc, char** argv) { interface_t doubleIntegratorInterface(taskFile, libFolder); // ROS ReferenceManager - std::shared_ptr rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, doubleIntegratorInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared(robotName, doubleIntegratorInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC diff --git a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt index 19f7c45d5..c6d07ed02 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt @@ -10,6 +10,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model @@ -94,6 +95,7 @@ add_library(${PROJECT_NAME} src/foot_planner/SwingTrajectoryPlanner.cpp src/gait/Gait.cpp src/gait/GaitSchedule.cpp + src/gait/LegLogic.cpp src/gait/ModeSequenceTemplate.cpp src/LeggedRobotInterface.cpp src/LeggedRobotPreComputation.cpp diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info index c1cf596d7..017dc40c3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info @@ -9,7 +9,7 @@ legged_robot_interface model_settings { - positionErrorGain 20.0 + positionErrorGain 0.0 ; 20.0 phaseTransitionStanceTime 0.4 verboseCppAd true @@ -26,8 +26,8 @@ swing_trajectory_config swingTimeScale 0.15 } -; multiple_shooting settings -multiple_shooting +; Multiple_Shooting SQP settings +sqp { nThreads 3 dt 0.015 @@ -46,6 +46,39 @@ multiple_shooting threadPriority 50 } +; Multiple_Shooting IPM settings +ipm +{ + nThreads 3 + dt 0.015 + ipmIteration 1 + deltaTol 1e-4 + g_max 10.0 + g_min 1e-6 + computeLagrangeMultipliers true + printSolverStatistics true + printSolverStatus false + printLinesearch false + useFeedbackPolicy true + integratorType RK2 + threadPriority 50 + + initialBarrierParameter 1e-4 + targetBarrierParameter 1e-4 + barrierLinearDecreaseFactor 0.2 + barrierSuperlinearDecreasePower 1.5 + barrierReductionCostTol 1e-3 + barrierReductionConstraintTol 1e-3 + + fractionToBoundaryMargin 0.995 + usePrimalStepSizeForDual false + + initialSlackLowerBound 1e-4 + initialDualLowerBound 1e-4 + initialSlackMarginRate 1e-2 + initialDualMarginRate 1e-2 +} + ; DDP settings ddp { diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml new file mode 100644 index 000000000..163176caf --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/friction_cone.xml @@ -0,0 +1,312 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Constraint value + + + + true + + 30 + LF_FOOT_frictionCone + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Constraint value + + + + true + + 30 + RF_FOOT_frictionCone + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Constraint value + + + + true + + 30 + LH_FOOT_frictionCone + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_frictionCone/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Constraint value + + + + true + + 30 + RH_FOOT_frictionCone + + + + false +
+ diff --git a/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/zero_velocity.xml b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/zero_velocity.xml new file mode 100644 index 000000000..499e25910 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot/config/multiplot/zero_velocity.xml @@ -0,0 +1,720 @@ + + + + #ffffff + #000000 + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + X + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Y + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Z + + + + true + + 30 + LF_FOOT_zeroVelocity + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + X + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Y + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RF_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Z + + + + true + + 30 + RF_FOOT_zeroVelocity + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + X + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Y + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/LH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Z + + + + true + + 30 + LH_FOOT_zeroVelocity + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/0 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + X + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/1 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Y + + + + + time + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + value/2 + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /metrics/RH_FOOT_zeroVelocity/0MsLookAhead + ocs2_msgs/constraint + + + + #000000 + 0 + + + 1000 + 10 + 0 + + + 100 + Z + + + + true + + 30 + RH_FOOT_zeroVelocity + + + + false +
+ diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h index fa9c388e3..22a950a72 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h @@ -34,12 +34,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include #include #include -#include +#include #include "ocs2_legged_robot/common/ModelSettings.h" #include "ocs2_legged_robot/initialization/LeggedRobotInitializer.h" @@ -62,8 +63,10 @@ class LeggedRobotInterface final : public RobotInterface { * @param [in] taskFile: The absolute path to the configuration file for the MPC. * @param [in] urdfFile: The absolute path to the URDF file for the robot. * @param [in] referenceFile: The absolute path to the reference configuration file. + * @param [in] useHardFrictionConeConstraint: Which to use hard or soft friction cone constraints. */ - LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile); + LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, + bool useHardFrictionConeConstraint = false); ~LeggedRobotInterface() override = default; @@ -73,7 +76,8 @@ class LeggedRobotInterface final : public RobotInterface { const ddp::Settings& ddpSettings() const { return ddpSettings_; } const mpc::Settings& mpcSettings() const { return mpcSettings_; } const rollout::Settings& rolloutSettings() const { return rolloutSettings_; } - const multiple_shooting::Settings& sqpSettings() { return sqpSettings_; } + const sqp::Settings& sqpSettings() { return sqpSettings_; } + const ipm::Settings& ipmSettings() { return ipmSettings_; } const vector_t& getInitialState() const { return initialState_; } const RolloutBase& getRollout() const { return *rolloutPtr_; } @@ -93,8 +97,9 @@ class LeggedRobotInterface final : public RobotInterface { matrix_t initializeInputCostWeight(const std::string& taskFile, const CentroidalModelInfo& info); std::pair loadFrictionConeSettings(const std::string& taskFile, bool verbose) const; - std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); + std::unique_ptr getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient); + std::unique_ptr getFrictionConeSoftConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, + const RelaxedBarrierPenalty::Config& barrierPenaltyConfig); std::unique_ptr getZeroForceConstraint(size_t contactPointIndex); std::unique_ptr getZeroVelocityConstraint(const EndEffectorKinematics& eeKinematics, size_t contactPointIndex, bool useAnalyticalGradients); @@ -104,7 +109,9 @@ class LeggedRobotInterface final : public RobotInterface { ModelSettings modelSettings_; ddp::Settings ddpSettings_; mpc::Settings mpcSettings_; - multiple_shooting::Settings sqpSettings_; + sqp::Settings sqpSettings_; + ipm::Settings ipmSettings_; + const bool useHardFrictionConeConstraint_; std::unique_ptr pinocchioInterfacePtr_; CentroidalModelInfo centroidalModelInfo_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h index 1dd064b01..fe97199f4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/GaitSchedule.h @@ -44,6 +44,15 @@ class GaitSchedule { GaitSchedule(ModeSchedule initModeSchedule, ModeSequenceTemplate initModeSequenceTemplate, scalar_t phaseTransitionStanceTime); /** + * Sets the mode schedule. + * + * @param [in] modeSchedule: The mode schedule to be used. + */ + void setModeSchedule(const ModeSchedule& modeSchedule) { modeSchedule_ = modeSchedule; } + + /** + * Gets the mode schedule. + * * @param [in] lowerBoundTime: The smallest time for which the ModeSchedule should be defined. * @param [in] upperBoundTime: The greatest time for which the ModeSchedule should be defined. */ diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h new file mode 100644 index 000000000..6f105b980 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/gait/LegLogic.h @@ -0,0 +1,112 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include "ocs2_legged_robot/common/Types.h" + +namespace ocs2 { +namespace legged_robot { + +struct LegPhase { + scalar_t phase; + scalar_t duration; +}; + +struct ContactTiming { + scalar_t start; + scalar_t end; +}; + +struct SwingTiming { + scalar_t start; + scalar_t end; +}; + +/** + * @brief Get the contact phase for all legs. + * If leg in contact, returns a value between 0.0 (at start of contact phase) and 1.0 (at end of contact phase). + * If leg not in contact (i.e. in swing), returns -1.0. + * If mode schedule starts with contact phase, returns 1.0 during this phase. + * If mode schedule ends with contact phase, returns 0.0 during this phase. + * @param [in] time : Query time. + * @param [in] modeSchedule : Mode schedule. + * @return Contact phases for all legs. + */ +feet_array_t getContactPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule); + +/** + * @brief Get the swing phase for all legs. + * If leg in swing, returns a value between 0.0 (at start of swing phase) and 1.0 (at end of swing phase). + * If leg not in swing (i.e. in contact), returns -1.0. + * If mode schedule starts with swing phase, returns 1.0 during this phase. + * If mode schedule ends with swing phase, returns 0.0 during this phase. + * @param [in] time : Query time. + * @param [in] modeSchedule : Mode schedule. + * @return Swing phases for all legs. + */ +feet_array_t getSwingPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule); + +/** Extracts the contact timings for all legs from a modeSchedule */ +feet_array_t> extractContactTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule); + +/** Extracts the swing timings for all legs from a modeSchedule */ +feet_array_t> extractSwingTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule); + +/** Returns time of the next lift off. Returns nan if leg is not lifting off */ +scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector& contactTimings); + +/** Returns time of the touch down for all legs from a modeschedule. Returns nan if leg does not touch down */ +scalar_t getTimeOfNextTouchDown(scalar_t currentTime, const std::vector& contactTimings); + +/** + * Get {startTime, endTime} for all contact phases. Swingphases are always implied in between: endTime[i] < startTime[i+1] + * times are NaN if they cannot be identified at the boundaries + * Vector is empty if there are no contact phases + */ +std::vector extractContactTimings(const std::vector& eventTimes, const std::vector& contactFlags); + +/** + * Get {startTime, endTime} for all swing phases. Contact phases are always implied in between: endTime[i] < startTime[i+1] + * times are NaN if they cannot be identified at the boundaries + * Vector is empty if there are no swing phases + */ +std::vector extractSwingTimings(const std::vector& eventTimes, const std::vector& contactFlags); + +/** + * Extracts for each leg the contact sequence over the motion phase sequence. + * @param modeSequence : Sequence of contact modes. + * @return Sequence of contact flags per leg. + */ +feet_array_t> extractContactFlags(const std::vector& modeSequence); + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/reference_manager/SwitchedModelReferenceManager.h b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/reference_manager/SwitchedModelReferenceManager.h index a1ee327d9..9f47651c0 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/reference_manager/SwitchedModelReferenceManager.h +++ b/ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/reference_manager/SwitchedModelReferenceManager.h @@ -48,6 +48,8 @@ class SwitchedModelReferenceManager : public ReferenceManager { ~SwitchedModelReferenceManager() override = default; + void setModeSchedule(const ModeSchedule& modeSchedule) override; + contact_flag_t getContactFlags(scalar_t time) const; const std::shared_ptr& getGaitSchedule() { return gaitSchedulePtr_; } diff --git a/ocs2_robotic_examples/ocs2_legged_robot/package.xml b/ocs2_robotic_examples/ocs2_legged_robot/package.xml index bb62d1e9d..a9f76f9dd 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot/package.xml @@ -17,6 +17,7 @@ ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_assets ocs2_robotic_tools ocs2_pinocchio_interface diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp index 8f2ccd3ff..5bb70a26f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp @@ -64,7 +64,9 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile) { +LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile, + bool useHardFrictionConeConstraint) + : useHardFrictionConeConstraint_(useHardFrictionConeConstraint) { // check that task file exists boost::filesystem::path taskFilePath(taskFile); if (boost::filesystem::exists(taskFilePath)) { @@ -92,10 +94,11 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st // load setting from loading file modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose); - ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose); + ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose); + sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose); + ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose); rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose); - sqpSettings_ = multiple_shooting::loadSettings(taskFile, "multiple_shooting", verbose); // OptimalConrolProblem setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose); @@ -120,8 +123,8 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile modelSettings_.contactNames6DoF); // Swing trajectory planner - std::unique_ptr swingTrajectoryPlanner( - new SwingTrajectoryPlanner(loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4)); + auto swingTrajectoryPlanner = + std::make_unique(loadSwingTrajectorySettings(taskFile, "swing_trajectory_config", verbose), 4); // Mode schedule manager referenceManagerPtr_ = @@ -174,8 +177,12 @@ void LeggedRobotInterface::setupOptimalConrolProblem(const std::string& taskFile modelSettings_.recompileLibrariesCppAd, modelSettings_.verboseCppAd)); } - problemPtr_->softConstraintPtr->add(footName + "_frictionCone", - getFrictionConeConstraint(i, frictionCoefficient, barrierPenaltyConfig)); + if (useHardFrictionConeConstraint_) { + problemPtr_->inequalityConstraintPtr->add(footName + "_frictionCone", getFrictionConeConstraint(i, frictionCoefficient)); + } else { + problemPtr_->softConstraintPtr->add(footName + "_frictionCone", + getFrictionConeSoftConstraint(i, frictionCoefficient, barrierPenaltyConfig)); + } problemPtr_->equalityConstraintPtr->add(footName + "_zeroForce", getZeroForceConstraint(i)); problemPtr_->equalityConstraintPtr->add(footName + "_zeroVelocity", getZeroVelocityConstraint(*eeKinematicsPtr, i, useAnalyticalGradientsConstraints)); @@ -279,7 +286,7 @@ std::unique_ptr LeggedRobotInterface::getBaseTrackingCost(const std::cerr << " #### =============================================================================\n"; } - return std::unique_ptr(new LeggedRobotStateInputQuadraticCost(std::move(Q), std::move(R), info, *referenceManagerPtr_)); + return std::make_unique(std::move(Q), std::move(R), info, *referenceManagerPtr_); } /******************************************************************************************************/ @@ -310,22 +317,27 @@ std::pair LeggedRobotInterface::loadFri /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -std::unique_ptr LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, scalar_t frictionCoefficient, - const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { +std::unique_ptr LeggedRobotInterface::getFrictionConeConstraint(size_t contactPointIndex, + scalar_t frictionCoefficient) { FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient); - std::unique_ptr frictionConeConstraintPtr( - new FrictionConeConstraint(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, centroidalModelInfo_)); - - std::unique_ptr penalty(new RelaxedBarrierPenalty(barrierPenaltyConfig)); + return std::make_unique(*referenceManagerPtr_, std::move(frictionConeConConfig), contactPointIndex, + centroidalModelInfo_); +} - return std::unique_ptr(new StateInputSoftConstraint(std::move(frictionConeConstraintPtr), std::move(penalty))); +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::unique_ptr LeggedRobotInterface::getFrictionConeSoftConstraint( + size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) { + return std::make_unique(getFrictionConeConstraint(contactPointIndex, frictionCoefficient), + std::make_unique(barrierPenaltyConfig)); } /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr LeggedRobotInterface::getZeroForceConstraint(size_t contactPointIndex) { - return std::unique_ptr(new ZeroForceConstraint(*referenceManagerPtr_, contactPointIndex, centroidalModelInfo_)); + return std::make_unique(*referenceManagerPtr_, contactPointIndex, centroidalModelInfo_); } /******************************************************************************************************/ @@ -349,8 +361,8 @@ std::unique_ptr LeggedRobotInterface::getZeroVelocityConst throw std::runtime_error( "[LeggedRobotInterface::getZeroVelocityConstraint] The analytical end-effector zero velocity constraint is not implemented!"); } else { - return std::unique_ptr(new ZeroVelocityConstraintCppAd(*referenceManagerPtr_, eeKinematics, contactPointIndex, - eeZeroVelConConfig(modelSettings_.positionErrorGain))); + return std::make_unique(*referenceManagerPtr_, eeKinematics, contactPointIndex, + eeZeroVelConConfig(modelSettings_.positionErrorGain)); } } @@ -364,7 +376,7 @@ std::unique_ptr LeggedRobotInterface::getNormalVelocityCon throw std::runtime_error( "[LeggedRobotInterface::getNormalVelocityConstraint] The analytical end-effector normal velocity constraint is not implemented!"); } else { - return std::unique_ptr(new NormalVelocityConstraintCppAd(*referenceManagerPtr_, eeKinematics, contactPointIndex)); + return std::make_unique(*referenceManagerPtr_, eeKinematics, contactPointIndex); } } diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp new file mode 100644 index 000000000..d61ae5b71 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/gait/LegLogic.cpp @@ -0,0 +1,340 @@ +/****************************************************************************** +Copyright (c) 2021, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_legged_robot/gait/LegLogic.h" + +#include "ocs2_legged_robot/gait/MotionPhaseDefinition.h" + +namespace { + +inline ocs2::scalar_t timingNaN() { + return std::numeric_limits::quiet_NaN(); +} + +inline bool hasStartTime(const ocs2::legged_robot::ContactTiming& timing) { + return !std::isnan(timing.start); +} +inline bool hasEndTime(const ocs2::legged_robot::ContactTiming& timing) { + return !std::isnan(timing.end); +} + +inline bool hasStartTime(const ocs2::legged_robot::SwingTiming& timing) { + return !std::isnan(timing.start); +} +inline bool hasEndTime(const ocs2::legged_robot::SwingTiming& timing) { + return !std::isnan(timing.end); +} + +inline bool startsWithSwingPhase(const std::vector& timings) { + return timings.empty() || hasStartTime(timings.front()); +} +inline bool startsWithContactPhase(const std::vector& timings) { + return !startsWithSwingPhase(timings); +} +inline bool endsWithSwingPhase(const std::vector& timings) { + return timings.empty() || hasEndTime(timings.back()); +} +inline bool endsWithContactPhase(const std::vector& timings) { + return !endsWithSwingPhase(timings); +} + +inline bool startsWithContactPhase(const std::vector& timings) { + return timings.empty() || hasStartTime(timings.front()); +} +inline bool startsWithSwingPhase(const std::vector& timings) { + return !startsWithContactPhase(timings); +} +inline bool endsWithContactPhase(const std::vector& timings) { + return timings.empty() || hasEndTime(timings.back()); +} +inline bool endsWithSwingPhase(const std::vector& timings) { + return !endsWithContactPhase(timings); +} + +inline bool touchesDownAtLeastOnce(const std::vector& timings) { + return std::any_of(timings.begin(), timings.end(), [](const ocs2::legged_robot::ContactTiming& timing) { return hasStartTime(timing); }); +} + +inline bool liftsOffAtLeastOnce(const std::vector& timings) { + return !timings.empty() && hasEndTime(timings.front()); +} + +inline bool touchesDownAtLeastOnce(const std::vector& timings) { + return !timings.empty() && hasEndTime(timings.front()); +} + +inline bool liftsOffAtLeastOnce(const std::vector& timings) { + return std::any_of(timings.begin(), timings.end(), [](const ocs2::legged_robot::SwingTiming& timing) { return hasStartTime(timing); }); +} + +} // anonymous namespace + +namespace ocs2 { +namespace legged_robot { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +feet_array_t getContactPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule) { + feet_array_t contactPhasePerLeg; + + // Convert mode sequence to a contact timing vector per leg + const auto contactTimingsPerLeg = extractContactTimingsPerLeg(modeSchedule); + + // Extract contact phases per leg + for (size_t leg = 0; leg < contactPhasePerLeg.size(); ++leg) { + if (contactTimingsPerLeg[leg].empty()) { + // Leg is always in swing phase + contactPhasePerLeg[leg].phase = -1.0; + contactPhasePerLeg[leg].duration = std::numeric_limits::quiet_NaN(); + } else if (startsWithContactPhase(contactTimingsPerLeg[leg]) && (time <= contactTimingsPerLeg[leg].front().end)) { + // It is assumed that contact phase started at minus infinity, so current time will be always close to ContactTiming.end + contactPhasePerLeg[leg].phase = 1.0; + contactPhasePerLeg[leg].duration = std::numeric_limits::infinity(); + } else if (endsWithContactPhase(contactTimingsPerLeg[leg]) && (time >= contactTimingsPerLeg[leg].back().start)) { + // It is assumed that contact phase ends at infinity, so current time will be always close to ContactTiming.start + contactPhasePerLeg[leg].phase = 0.0; + contactPhasePerLeg[leg].duration = std::numeric_limits::infinity(); + } else { + // Check if leg is in contact interval at current time + auto it = std::find_if(contactTimingsPerLeg[leg].begin(), contactTimingsPerLeg[leg].end(), + [time](ContactTiming timing) { return (timing.start <= time) && (time <= timing.end); }); + if (it == contactTimingsPerLeg[leg].end()) { + // Leg is not in contact for current time + contactPhasePerLeg[leg].phase = -1.0; + contactPhasePerLeg[leg].duration = std::numeric_limits::quiet_NaN(); + } else { + // Leg is in contact for current time + const auto& currentContactTiming = *it; + contactPhasePerLeg[leg].phase = (time - currentContactTiming.start) / (currentContactTiming.end - currentContactTiming.start); + contactPhasePerLeg[leg].duration = currentContactTiming.end - currentContactTiming.start; + } + } + } + + return contactPhasePerLeg; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +feet_array_t getSwingPhasePerLeg(scalar_t time, const ocs2::ModeSchedule& modeSchedule) { + feet_array_t swingPhasePerLeg; + + // Convert mode sequence to a swing timing vector per leg + const auto swingTimingsPerLeg = extractSwingTimingsPerLeg(modeSchedule); + + // Extract swing phases per leg + for (size_t leg = 0; leg < swingPhasePerLeg.size(); ++leg) { + if (swingTimingsPerLeg[leg].empty()) { + // Leg is always in contact phase + swingPhasePerLeg[leg].phase = -1.0; + swingPhasePerLeg[leg].duration = std::numeric_limits::quiet_NaN(); + } else if (startsWithSwingPhase(swingTimingsPerLeg[leg]) && (time <= swingTimingsPerLeg[leg].front().end)) { + // It is assumed that swing phase started at minus infinity, so current time will be always close to SwingTiming.end + swingPhasePerLeg[leg].phase = 1.0; + swingPhasePerLeg[leg].duration = std::numeric_limits::infinity(); + } else if (endsWithSwingPhase(swingTimingsPerLeg[leg]) && (time >= swingTimingsPerLeg[leg].back().start)) { + // It is assumed that swing phase ends at infinity, so current time will be always close to SwingTiming.start + swingPhasePerLeg[leg].phase = 0.0; + swingPhasePerLeg[leg].duration = std::numeric_limits::infinity(); + } else { + // Check if leg is in swing interval at current time + auto it = std::find_if(swingTimingsPerLeg[leg].begin(), swingTimingsPerLeg[leg].end(), + [time](SwingTiming timing) { return (timing.start <= time) && (time <= timing.end); }); + if (it == swingTimingsPerLeg[leg].end()) { + // Leg is not swinging for current time + swingPhasePerLeg[leg].phase = scalar_t(-1.0); + swingPhasePerLeg[leg].duration = std::numeric_limits::quiet_NaN(); + } else { + // Leg is swinging for current time + const auto& currentSwingTiming = *it; + swingPhasePerLeg[leg].phase = (time - currentSwingTiming.start) / (currentSwingTiming.end - currentSwingTiming.start); + swingPhasePerLeg[leg].duration = currentSwingTiming.end - currentSwingTiming.start; + } + } + } + + return swingPhasePerLeg; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +feet_array_t> extractContactTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule) { + feet_array_t> contactTimingsPerLeg; + + // Convert mode sequence to a contact flag vector per leg + const auto contactSequencePerLeg = extractContactFlags(modeSchedule.modeSequence); + + // Extract timings per leg + for (size_t leg = 0; leg < contactTimingsPerLeg.size(); ++leg) { + contactTimingsPerLeg[leg] = extractContactTimings(modeSchedule.eventTimes, contactSequencePerLeg[leg]); + } + + return contactTimingsPerLeg; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +feet_array_t> extractSwingTimingsPerLeg(const ocs2::ModeSchedule& modeSchedule) { + feet_array_t> swingTimingsPerLeg; + + // Convert mode sequence to a contact flag vector per leg + const auto contactSequencePerLeg = extractContactFlags(modeSchedule.modeSequence); + + // Extract timings per leg + for (size_t leg = 0; leg < swingTimingsPerLeg.size(); ++leg) { + swingTimingsPerLeg[leg] = extractSwingTimings(modeSchedule.eventTimes, contactSequencePerLeg[leg]); + } + + return swingTimingsPerLeg; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +scalar_t getTimeOfNextLiftOff(scalar_t currentTime, const std::vector& contactTimings) { + for (const auto& contactPhase : contactTimings) { + if (hasEndTime(contactPhase) && contactPhase.end > currentTime) { + return contactPhase.end; + } + } + return timingNaN(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +scalar_t getTimeOfNextTouchDown(scalar_t currentTime, const std::vector& contactTimings) { + for (const auto& contactPhase : contactTimings) { + if (hasStartTime(contactPhase) && contactPhase.start > currentTime) { + return contactPhase.start; + } + } + return timingNaN(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::vector extractContactTimings(const std::vector& eventTimes, const std::vector& contactFlags) { + assert(eventTimes.size() + 1 == contactFlags.size()); + const int numPhases = contactFlags.size(); + + std::vector contactTimings; + contactTimings.reserve(1 + eventTimes.size() / 2); // Approximate upper bound + int currentPhase = 0; + + while (currentPhase < numPhases) { + // Search where contact phase starts + while (currentPhase < numPhases && !contactFlags[currentPhase]) { + ++currentPhase; + } + if (currentPhase >= numPhases) { + break; // No more contact phases + } + + // Register start of the contact phase + const scalar_t startTime = (currentPhase == 0) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase - 1]; + + // Find when the contact phase ends + while (currentPhase + 1 < numPhases && contactFlags[currentPhase + 1]) { + ++currentPhase; + } + + // Register end of the contact phase + const scalar_t endTime = (currentPhase + 1 >= numPhases) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase]; + + // Add to phases + contactTimings.push_back({startTime, endTime}); + ++currentPhase; + } + return contactTimings; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +std::vector extractSwingTimings(const std::vector& eventTimes, const std::vector& contactFlags) { + assert(eventTimes.size() + 1 == contactFlags.size()); + const int numPhases = contactFlags.size(); + + std::vector swingTimings; + swingTimings.reserve(1 + eventTimes.size() / 2); // Approximate upper bound + int currentPhase = 0; + + while (currentPhase < numPhases) { + // Search where swing phase starts + while (currentPhase < numPhases && contactFlags[currentPhase]) { + ++currentPhase; + } + if (currentPhase >= numPhases) { + break; // No more swing phases + } + + // Register start of the swing phase + const scalar_t startTime = (currentPhase == 0) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase - 1]; + + // Find when the swing phase ends + while (currentPhase + 1 < numPhases && !contactFlags[currentPhase + 1]) { + ++currentPhase; + } + + // Register end of the contact phase + const scalar_t endTime = (currentPhase + 1 >= numPhases) ? std::numeric_limits::quiet_NaN() : eventTimes[currentPhase]; + + // Add to phases + swingTimings.push_back({startTime, endTime}); + ++currentPhase; + } + return swingTimings; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +feet_array_t> extractContactFlags(const std::vector& modeSequence) { + const size_t numPhases = modeSequence.size(); + + feet_array_t> contactFlagStock; + std::fill(contactFlagStock.begin(), contactFlagStock.end(), std::vector(numPhases)); + + for (size_t i = 0; i < numPhases; i++) { + const auto contactFlag = modeNumber2StanceLeg(modeSequence[i]); + for (size_t j = 0; j < contactFlagStock.size(); j++) { + contactFlagStock[j][i] = contactFlag[j]; + } + } + return contactFlagStock; +} + +} // namespace legged_robot +} // namespace ocs2 diff --git a/ocs2_robotic_examples/ocs2_legged_robot/src/reference_manager/SwitchedModelReferenceManager.cpp b/ocs2_robotic_examples/ocs2_legged_robot/src/reference_manager/SwitchedModelReferenceManager.cpp index 0fbf8a61f..2e7907ed8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/src/reference_manager/SwitchedModelReferenceManager.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/src/reference_manager/SwitchedModelReferenceManager.cpp @@ -41,6 +41,14 @@ SwitchedModelReferenceManager::SwitchedModelReferenceManager(std::shared_ptrsetModeSchedule(modeSchedule); +} + /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_robotic_examples/ocs2_legged_robot/test/AnymalFactoryFunctions.cpp b/ocs2_robotic_examples/ocs2_legged_robot/test/AnymalFactoryFunctions.cpp index b792e4027..b38aa7aa2 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/test/AnymalFactoryFunctions.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/test/AnymalFactoryFunctions.cpp @@ -50,7 +50,7 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ std::unique_ptr createAnymalPinocchioInterface() { - return std::unique_ptr(new PinocchioInterface(centroidal_model::createPinocchioInterface(URDF_FILE))); + return std::make_unique(centroidal_model::createPinocchioInterface(URDF_FILE)); } /******************************************************************************************************/ @@ -71,11 +71,11 @@ std::shared_ptr createReferenceManager(size_t num const auto defaultModeSequenceTemplate = loadModeSequenceTemplate(REFERENCE_FILE, "defaultModeSequenceTemplate", false); const ModelSettings modelSettings; - std::shared_ptr gaitSchedule( - new GaitSchedule(initModeSchedule, defaultModeSequenceTemplate, modelSettings.phaseTransitionStanceTime)); - std::unique_ptr swingTrajectoryPlanner( - new SwingTrajectoryPlanner(loadSwingTrajectorySettings(TASK_FILE, "swing_trajectory_config", false), numFeet)); - return std::make_shared(gaitSchedule, std::move(swingTrajectoryPlanner)); + auto gaitSchedule = + std::make_shared(initModeSchedule, defaultModeSequenceTemplate, modelSettings.phaseTransitionStanceTime); + auto swingTrajectoryPlanner = + std::make_unique(loadSwingTrajectorySettings(TASK_FILE, "swing_trajectory_config", false), numFeet); + return std::make_shared(std::move(gaitSchedule), std::move(swingTrajectoryPlanner)); } } // namespace legged_robot diff --git a/ocs2_robotic_examples/ocs2_legged_robot/test/constraint/testEndEffectorLinearConstraint.cpp b/ocs2_robotic_examples/ocs2_legged_robot/test/constraint/testEndEffectorLinearConstraint.cpp index 7b03222f3..f79647ae4 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot/test/constraint/testEndEffectorLinearConstraint.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot/test/constraint/testEndEffectorLinearConstraint.cpp @@ -116,9 +116,9 @@ class testEndEffectorLinearConstraint : public ::testing::Test { }; TEST_F(testEndEffectorLinearConstraint, testValue) { - auto eeVelConstraintPtr = std::unique_ptr(new EndEffectorLinearConstraint(*eeKinematicsPtr, 3)); + auto eeVelConstraintPtr = std::make_unique(*eeKinematicsPtr, 3); eeVelConstraintPtr->configure(config); - auto eeVelConstraintAdPtr = std::unique_ptr(new EndEffectorLinearConstraint(*eeKinematicsAdPtr, 3)); + auto eeVelConstraintAdPtr = std::make_unique(*eeKinematicsAdPtr, 3); eeVelConstraintAdPtr->configure(config); dynamic_cast(eeVelConstraintPtr->getEndEffectorKinematics()) @@ -143,9 +143,9 @@ TEST_F(testEndEffectorLinearConstraint, testValue) { } TEST_F(testEndEffectorLinearConstraint, testLinearApproximation) { - auto eeVelConstraintPtr = std::unique_ptr(new EndEffectorLinearConstraint(*eeKinematicsPtr, 3)); + auto eeVelConstraintPtr = std::make_unique(*eeKinematicsPtr, 3); eeVelConstraintPtr->configure(config); - auto eeVelConstraintAdPtr = std::unique_ptr(new EndEffectorLinearConstraint(*eeKinematicsAdPtr, 3)); + auto eeVelConstraintAdPtr = std::make_unique(*eeKinematicsAdPtr, 3); eeVelConstraintAdPtr->configure(config); dynamic_cast(eeVelConstraintPtr->getEndEffectorKinematics()) diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt index d5c30446e..a8819c8f8 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/CMakeLists.txt @@ -16,6 +16,7 @@ set(CATKIN_PACKAGE_DEPENDENCIES ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model @@ -118,6 +119,21 @@ target_link_libraries(legged_robot_sqp_mpc ) target_compile_options(legged_robot_sqp_mpc PRIVATE ${OCS2_CXX_FLAGS}) +## IPM-MPC node for legged robot +add_executable(legged_robot_ipm_mpc + src/LeggedRobotIpmMpcNode.cpp +) +add_dependencies(legged_robot_ipm_mpc + ${PROJECT_NAME} + ${${PROJECT_NAME}_EXPORTED_TARGETS} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(legged_robot_ipm_mpc + ${PROJECT_NAME} + ${catkin_LIBRARIES} +) +target_compile_options(legged_robot_ipm_mpc PRIVATE ${OCS2_CXX_FLAGS}) + # Dummy node add_executable(legged_robot_dummy src/LeggedRobotDummyNode.cpp @@ -174,6 +190,7 @@ if(cmake_clang_tools_FOUND) ${PROJECT_NAME} legged_robot_ddp_mpc legged_robot_sqp_mpc + legged_robot_ipm_mpc legged_robot_dummy legged_robot_target legged_robot_gait_command @@ -198,6 +215,7 @@ install( TARGETS legged_robot_ddp_mpc legged_robot_sqp_mpc + legged_robot_ipm_mpc legged_robot_dummy legged_robot_target legged_robot_gait_command diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h index e6327327d..d69946deb 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/include/ocs2_legged_robot_ros/gait/GaitReceiver.h @@ -43,9 +43,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace ocs2 { namespace legged_robot { + class GaitReceiver : public SolverSynchronizedModule { public: - GaitReceiver(ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName); + GaitReceiver(::ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName); void preSolverRun(scalar_t initTime, scalar_t finalTime, const vector_t& currentState, const ReferenceManagerInterface& referenceManager) override; @@ -57,7 +58,7 @@ class GaitReceiver : public SolverSynchronizedModule { std::shared_ptr gaitSchedulePtr_; - ros::Subscriber mpcModeSequenceSubscriber_; + ::ros::Subscriber mpcModeSequenceSubscriber_; std::mutex receivedGaitMutex_; std::atomic_bool gaitUpdated_; diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch index f9bb4c359..d0d0f7611 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ddp.launch @@ -28,6 +28,7 @@ + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch new file mode 100644 index 000000000..085a29175 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_ipm.launch @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch index 36f6fe62f..8e11f2138 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/legged_robot_sqp.launch @@ -28,6 +28,7 @@ + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch index 96fd2a252..e08f5659f 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/launch/multiplot.launch @@ -1,6 +1,12 @@ + + + + diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml index aa2943272..7bc71c34e 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/package.xml @@ -24,6 +24,7 @@ ocs2_ddp ocs2_mpc ocs2_sqp + ocs2_ipm ocs2_robotic_tools ocs2_pinocchio_interface ocs2_centroidal_model diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp index b37272d69..355ba0234 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDdpMpcNode.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" @@ -44,10 +45,12 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + ::ros::init(argc, argv, robotName + "_mpc"); + ::ros::NodeHandle nodeHandle; // Get node parameters + bool multiplot = false; std::string taskFile, urdfFile, referenceFile; + nodeHandle.getParam("/multiplot", multiplot); nodeHandle.getParam("/taskFile", taskFile); nodeHandle.getParam("/referenceFile", referenceFile); nodeHandle.getParam("/urdfFile", urdfFile); @@ -69,6 +72,21 @@ int main(int argc, char** argv) { mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + if (multiplot) { + auto createStateInputBoundsObserver = [&](const std::string& termName) { + const ocs2::scalar_array_t observingTimePoints{0.0}; + const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; + auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + }; + for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + } + } + // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); mpcNode.launchNodes(nodeHandle); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp index 4fc71d2de..85a47e4f3 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotDummyNode.cpp @@ -65,8 +65,8 @@ int main(int argc, char** argv) { CentroidalModelPinocchioMapping pinocchioMapping(interface.getCentroidalModelInfo()); PinocchioEndEffectorKinematics endEffectorKinematics(interface.getPinocchioInterface(), pinocchioMapping, interface.modelSettings().contactNames3DoF); - std::shared_ptr leggedRobotVisualizer( - new LeggedRobotVisualizer(interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle)); + auto leggedRobotVisualizer = std::make_shared( + interface.getPinocchioInterface(), interface.getCentroidalModelInfo(), endEffectorKinematics, nodeHandle); // Dummy legged robot MRT_ROS_Dummy_Loop leggedRobotDummySimulator(mrt, interface.mpcSettings().mrtDesiredFrequency_, diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp new file mode 100644 index 000000000..e754012b9 --- /dev/null +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp @@ -0,0 +1,96 @@ +/****************************************************************************** +Copyright (c) 2017, Farbod Farshidian. All rights reserved. + +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. + + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include + +#include +#include +#include +#include +#include + +#include "ocs2_legged_robot_ros/gait/GaitReceiver.h" + +using namespace ocs2; +using namespace legged_robot; + +int main(int argc, char** argv) { + const std::string robotName = "legged_robot"; + + // Initialize ros node + ::ros::init(argc, argv, robotName + "_mpc"); + ::ros::NodeHandle nodeHandle; + // Get node parameters + bool multiplot = false; + std::string taskFile, urdfFile, referenceFile; + nodeHandle.getParam("/multiplot", multiplot); + nodeHandle.getParam("/taskFile", taskFile); + nodeHandle.getParam("/urdfFile", urdfFile); + nodeHandle.getParam("/referenceFile", referenceFile); + + // Robot interface + constexpr bool useHardFrictionConeConstraint = true; + LeggedRobotInterface interface(taskFile, urdfFile, referenceFile, useHardFrictionConeConstraint); + + // Gait receiver + auto gaitReceiverPtr = + std::make_shared(nodeHandle, interface.getSwitchedModelReferenceManagerPtr()->getGaitSchedule(), robotName); + + // ROS ReferenceManager + auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(nodeHandle); + + // MPC + IpmMpc mpc(interface.mpcSettings(), interface.ipmSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); + mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + + // observer for friction cone constraints (only add this for debugging as it slows down the solver) + if (multiplot) { + auto createStateInputBoundsObserver = [&](const std::string& termName) { + const ocs2::scalar_array_t observingTimePoints{0.0}; + const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; + auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + }; + for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_frictionCone")); + } + } + + // Launch MPC ROS node + MPC_ROS_Interface mpcNode(mpc, robotName); + mpcNode.launchNodes(nodeHandle); + + // Successful exit + return 0; +} diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp index 13953b284..9b3e51641 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotSqpMpcNode.cpp @@ -33,7 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include +#include #include "ocs2_legged_robot_ros/gait/GaitReceiver.h" @@ -44,10 +45,12 @@ int main(int argc, char** argv) { const std::string robotName = "legged_robot"; // Initialize ros node - ros::init(argc, argv, robotName + "_mpc"); - ros::NodeHandle nodeHandle; + ::ros::init(argc, argv, robotName + "_mpc"); + ::ros::NodeHandle nodeHandle; // Get node parameters + bool multiplot = false; std::string taskFile, urdfFile, referenceFile; + nodeHandle.getParam("/multiplot", multiplot); nodeHandle.getParam("/taskFile", taskFile); nodeHandle.getParam("/urdfFile", urdfFile); nodeHandle.getParam("/referenceFile", referenceFile); @@ -64,11 +67,25 @@ int main(int argc, char** argv) { rosReferenceManagerPtr->subscribe(nodeHandle); // MPC - MultipleShootingMpc mpc(interface.mpcSettings(), interface.sqpSettings(), interface.getOptimalControlProblem(), - interface.getInitializer()); + SqpMpc mpc(interface.mpcSettings(), interface.sqpSettings(), interface.getOptimalControlProblem(), interface.getInitializer()); mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + // observer for zero velocity constraints (only add this for debugging as it slows down the solver) + if (multiplot) { + auto createStateInputBoundsObserver = [&](const std::string& termName) { + const ocs2::scalar_array_t observingTimePoints{0.0}; + const std::vector topicNames{"metrics/" + termName + "/0MsLookAhead"}; + auto callback = ocs2::ros::createConstraintCallback(nodeHandle, {0.0}, topicNames, + ocs2::ros::CallbackInterpolationStrategy::linear_interpolation); + return ocs2::SolverObserver::ConstraintTermObserver(ocs2::SolverObserver::Type::Intermediate, termName, std::move(callback)); + }; + for (size_t i = 0; i < interface.getCentroidalModelInfo().numThreeDofContacts; i++) { + const std::string& footName = interface.modelSettings().contactNames3DoF[i]; + mpc.getSolverPtr()->addSolverObserver(createStateInputBoundsObserver(footName + "_zeroVelocity")); + } + } + // Launch MPC ROS node MPC_ROS_Interface mpcNode(mpc, robotName); mpcNode.launchNodes(nodeHandle); diff --git a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp index 82c452459..cd98a3b2d 100644 --- a/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp +++ b/ocs2_robotic_examples/ocs2_legged_robot_ros/src/gait/GaitReceiver.cpp @@ -37,7 +37,7 @@ namespace legged_robot { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -GaitReceiver::GaitReceiver(ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName) +GaitReceiver::GaitReceiver(::ros::NodeHandle nodeHandle, std::shared_ptr gaitSchedulePtr, const std::string& robotName) : gaitSchedulePtr_(std::move(gaitSchedulePtr)), receivedGait_({0.0, 1.0}, {ModeNumber::STANCE}), gaitUpdated_(false) { mpcModeSequenceSubscriber_ = nodeHandle.subscribe(robotName + "_mpc_mode_schedule", 1, &GaitReceiver::mpcModeSequenceCallback, this, ::ros::TransportHints().udp()); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp index f48a344b7..1fdee8f26 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/src/MobileManipulatorInterface.cpp @@ -245,7 +245,7 @@ std::unique_ptr MobileManipulatorInterface::getQuadraticInputCos std::cerr << "inputCost.R: \n" << R << '\n'; std::cerr << " #### =============================================================================\n"; - return std::unique_ptr(new QuadraticInputCost(std::move(R), manipulatorModelInfo_.stateDim)); + return std::make_unique(std::move(R), manipulatorModelInfo_.stateDim); } /******************************************************************************************************/ @@ -285,10 +285,10 @@ std::unique_ptr MobileManipulatorInterface::getEndEffectorConstraint( } std::vector> penaltyArray(6); - std::generate_n(penaltyArray.begin(), 3, [&] { return std::unique_ptr(new QuadraticPenalty(muPosition)); }); - std::generate_n(penaltyArray.begin() + 3, 3, [&] { return std::unique_ptr(new QuadraticPenalty(muOrientation)); }); + std::generate_n(penaltyArray.begin(), 3, [&] { return std::make_unique(muPosition); }); + std::generate_n(penaltyArray.begin() + 3, 3, [&] { return std::make_unique(muOrientation); }); - return std::unique_ptr(new StateSoftConstraint(std::move(constraint), std::move(penaltyArray))); + return std::make_unique(std::move(constraint), std::move(penaltyArray)); } /******************************************************************************************************/ @@ -323,17 +323,17 @@ std::unique_ptr MobileManipulatorInterface::getSelfCollisionConstrain std::unique_ptr constraint; if (usePreComputation) { - constraint = std::unique_ptr(new MobileManipulatorSelfCollisionConstraint( - MobileManipulatorPinocchioMapping(manipulatorModelInfo_), std::move(geometryInterface), minimumDistance)); + constraint = std::make_unique(MobileManipulatorPinocchioMapping(manipulatorModelInfo_), + std::move(geometryInterface), minimumDistance); } else { - constraint = std::unique_ptr(new SelfCollisionConstraintCppAd( + constraint = std::make_unique( pinocchioInterface, MobileManipulatorPinocchioMapping(manipulatorModelInfo_), std::move(geometryInterface), minimumDistance, - "self_collision", libraryFolder, recompileLibraries, false)); + "self_collision", libraryFolder, recompileLibraries, false); } - std::unique_ptr penalty(new RelaxedBarrierPenalty({mu, delta})); + auto penalty = std::make_unique(RelaxedBarrierPenalty::Config{mu, delta}); - return std::unique_ptr(new StateSoftConstraint(std::move(constraint), std::move(penalty))); + return std::make_unique(std::move(constraint), std::move(penalty)); } /******************************************************************************************************/ @@ -431,7 +431,7 @@ std::unique_ptr MobileManipulatorInterface::getJointLimitSoftCon } } - auto boxConstraints = std::unique_ptr(new StateInputSoftBoxConstraint(stateLimits, inputLimits)); + auto boxConstraints = std::make_unique(stateLimits, inputLimits); boxConstraints->initializeOffset(0.0, vector_t::Zero(manipulatorModelInfo_.stateDim), vector_t::Zero(manipulatorModelInfo_.inputDim)); return boxConstraints; } diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp index 9fef6927f..b957e2def 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp @@ -113,10 +113,10 @@ class DummyMobileManipulatorParametersTests std::unique_ptr getMpc() { auto& interface = *mobileManipulatorInterfacePtr; - std::unique_ptr mpcPtr(new GaussNewtonDDP_MPC( + auto mpcPtr = std::make_unique( interface.mpcSettings(), interface.ddpSettings(), interface.getRollout(), interface.getOptimalControlProblem(), - interface.getInitializer())); + interface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager( mobileManipulatorInterfacePtr->getReferenceManagerPtr()); return mpcPtr; @@ -199,6 +199,7 @@ TEST_P(DummyMobileManipulatorParametersTests, synchronousTracking) { verifyTrackingQuality(observation.state); } +#ifdef NDEBUG TEST_P(DummyMobileManipulatorParametersTests, asynchronousTracking) { // Obtain mpc initialize(getTaskFile(), getLibFolder(), getUrdfFile()); @@ -258,6 +259,7 @@ TEST_P(DummyMobileManipulatorParametersTests, asynchronousTracking) { verifyTrackingQuality(observation.state); } +#endif /******************************************************************************************************/ /******************************************************************************************************/ diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp index d354c643f..b472ae59b 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorDummyMRT.cpp @@ -64,8 +64,7 @@ int main(int argc, char** argv) { mrt.launchNodes(nodeHandle); // Visualization - std::shared_ptr dummyVisualization( - new mobile_manipulator::MobileManipulatorDummyVisualization(nodeHandle, interface)); + auto dummyVisualization = std::make_shared(nodeHandle, interface); // Dummy MRT MRT_ROS_Dummy_Loop dummy(mrt, interface.mpcSettings().mrtDesiredFrequency_, interface.mpcSettings().mpcDesiredFrequency_); diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp index 4eaae89eb..64ebb951f 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator_ros/src/MobileManipulatorMpcNode.cpp @@ -57,8 +57,7 @@ int main(int argc, char** argv) { MobileManipulatorInterface interface(taskFile, libFolder, urdfFile); // ROS ReferenceManager - std::shared_ptr rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, interface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared(robotName, interface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC diff --git a/ocs2_robotic_examples/ocs2_quadrotor/include/ocs2_quadrotor/QuadrotorPyBindings.h b/ocs2_robotic_examples/ocs2_quadrotor/include/ocs2_quadrotor/QuadrotorPyBindings.h index 5a3d617cf..964aeba96 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/include/ocs2_quadrotor/QuadrotorPyBindings.h +++ b/ocs2_robotic_examples/ocs2_quadrotor/include/ocs2_quadrotor/QuadrotorPyBindings.h @@ -59,9 +59,9 @@ class QuadrotorPyBindings final : public PythonInterface { QuadrotorInterface quadrotorInterface(taskFile, libraryFolder); // MPC - std::unique_ptr mpcPtr( - new GaussNewtonDDP_MPC(quadrotorInterface.mpcSettings(), quadrotorInterface.ddpSettings(), quadrotorInterface.getRollout(), - quadrotorInterface.getOptimalControlProblem(), quadrotorInterface.getInitializer())); + auto mpcPtr = std::make_unique(quadrotorInterface.mpcSettings(), quadrotorInterface.ddpSettings(), + quadrotorInterface.getRollout(), quadrotorInterface.getOptimalControlProblem(), + quadrotorInterface.getInitializer()); mpcPtr->getSolverPtr()->setReferenceManager(quadrotorInterface.getReferenceManagerPtr()); // Python interface diff --git a/ocs2_robotic_examples/ocs2_quadrotor/src/QuadrotorInterface.cpp b/ocs2_robotic_examples/ocs2_quadrotor/src/QuadrotorInterface.cpp index 17f02ccf5..a5b9f9f3a 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor/src/QuadrotorInterface.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor/src/QuadrotorInterface.cpp @@ -88,8 +88,8 @@ QuadrotorInterface::QuadrotorInterface(const std::string& taskFile, const std::s std::cerr << "R: \n" << R << "\n"; std::cerr << "Q_final:\n" << Qf << "\n"; - problem_.costPtr->add("cost", std::unique_ptr(new QuadraticStateInputCost(Q, R))); - problem_.finalCostPtr->add("finalCost", std::unique_ptr(new QuadraticStateCost(Qf))); + problem_.costPtr->add("cost", std::make_unique(Q, R)); + problem_.finalCostPtr->add("finalCost", std::make_unique(Qf)); // Dynamics auto quadrotorParameters = quadrotor::loadSettings(taskFile, "QuadrotorParameters", true); diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp index bf8fcb160..bc8adf82c 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/DummyQuadrotorNode.cpp @@ -64,8 +64,7 @@ int main(int argc, char** argv) { mrt.launchNodes(nodeHandle); // Visualization - std::shared_ptr quadrotorDummyVisualization( - new ocs2::quadrotor::QuadrotorDummyVisualization()); + auto quadrotorDummyVisualization = std::make_shared(); // Dummy loop ocs2::MRT_ROS_Dummy_Loop dummyQuadrotor(mrt, quadrotorInterface.mpcSettings().mrtDesiredFrequency_, diff --git a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp index a897bcbdb..485588d97 100644 --- a/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp +++ b/ocs2_robotic_examples/ocs2_quadrotor_ros/src/QuadrotorMpcNode.cpp @@ -56,8 +56,7 @@ int main(int argc, char** argv) { ocs2::quadrotor::QuadrotorInterface quadrotorInterface(taskFile, libFolder); // ROS ReferenceManager - std::shared_ptr rosReferenceManagerPtr( - new ocs2::RosReferenceManager(robotName, quadrotorInterface.getReferenceManagerPtr())); + auto rosReferenceManagerPtr = std::make_shared(robotName, quadrotorInterface.getReferenceManagerPtr()); rosReferenceManagerPtr->subscribe(nodeHandle); // MPC diff --git a/ocs2_ros_interfaces/CMakeLists.txt b/ocs2_ros_interfaces/CMakeLists.txt index 30895fc29..0fb39c4c2 100644 --- a/ocs2_ros_interfaces/CMakeLists.txt +++ b/ocs2_ros_interfaces/CMakeLists.txt @@ -5,7 +5,6 @@ set(CATKIN_PACKAGE_DEPENDENCIES roscpp ocs2_msgs ocs2_core - ocs2_ddp ocs2_mpc std_msgs visualization_msgs @@ -64,7 +63,7 @@ add_library(${PROJECT_NAME} src/mrt/MRT_ROS_Dummy_Loop.cpp src/mrt/MRT_ROS_Interface.cpp src/synchronized_module/RosReferenceManager.cpp - src/synchronized_module/RosAugmentedLagrangianCallbacks.cpp + src/synchronized_module/SolverObserverRosCallbacks.cpp src/visualization/VisualizationHelpers.cpp src/visualization/VisualizationColors.cpp ) diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h index 435b2e70c..e390d0241 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/common/RosMsgConversions.h @@ -35,9 +35,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include -#include +#include // MPC messages +#include #include #include #include @@ -78,8 +79,11 @@ ocs2_msgs::mpc_performance_indices createPerformanceIndicesMsg(scalar_t initTime /** Reads the performance indices message. */ PerformanceIndex readPerformanceIndicesMsg(const ocs2_msgs::mpc_performance_indices& performanceIndicesMsg); +/** Creates constraint message. */ +ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint); + /** Creates lagrangian_metrics message. */ -ocs2_msgs::lagrangian_metrics createMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics); +ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics); /** Creates multiplier message. */ ocs2_msgs::multiplier createMultiplierMsg(scalar_t time, MultiplierConstRef multiplier); diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h index 9d46ea51f..cd3611392 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h @@ -85,8 +85,8 @@ class RosReferenceManager final : public ReferenceManagerDecorator { /******************************************************************************************************/ template std::unique_ptr RosReferenceManager::create(const std::string& topicPrefix, Args&&... args) { - std::shared_ptr referenceManagerPtr(new ReferenceManagerType(std::forward(args)...)); - return std::unique_ptr(new RosReferenceManager(topicPrefix, std::move(referenceManagerPtr))); + auto referenceManagerPtr = std::make_shared(std::forward(args)...); + return std::make_unique(topicPrefix, std::move(referenceManagerPtr)); } } // namespace ocs2 diff --git a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h similarity index 72% rename from ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h rename to ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h index 7da2f92bd..b76dd4278 100644 --- a/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h +++ b/ocs2_ros_interfaces/include/ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h @@ -31,7 +31,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include +#include namespace ocs2 { namespace ros { @@ -42,7 +42,20 @@ enum class CallbackInterpolationStrategy { }; /** - * Creates a ROS-based callback for AugmentedLagrangianObserver that publishes a term's LagrangianMetrics at the + * Creates a ROS-based callback for SolverObserver that publishes a constraint term at the requested lookahead time points. + * + * @param [in] nodeHandle: ROS node handle. + * @param [in] observingTimePoints: An array of lookahead times for which we want to publish the values of constraint. + * @param [in] topicNames: An array of topic names. For each observing time points, you should provide a unique topic name. + * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. + * @return A callback which can be set to SolverObserverModule in order to observe a requested term's constraint. + */ +SolverObserver::constraint_callback_t createConstraintCallback( + ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); + +/** + * Creates a ROS-based callback for SolverObserver that publishes a term's LagrangianMetrics at the * requested lookahead time points. * * @param [in] nodeHandle: ROS node handle. @@ -51,12 +64,12 @@ enum class CallbackInterpolationStrategy { * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. * @return A callback which can be set to SolverObserverModule in order to observe a requested term's LagrangianMetrics. */ -AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback( +SolverObserver::lagrangian_callback_t createLagrangianCallback( ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); /** - * Creates a ROS-based callback for AugmentedLagrangianObserver that publishes a term's Lagrange multiplier at the + * Creates a ROS-based callback for SolverObserver that publishes a term's Lagrange multiplier at the * requested lookahead time points. * * @param [in] nodeHandle: ROS node handle. @@ -65,7 +78,7 @@ AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback( * @param [in] interpolationStrategy: The interpolation method used for acquiring data at each time point. * @return A callback which can be set to SolverObserverModule in order to observe a requested term's multiplier. */ -AugmentedLagrangianObserver::multiplier_callback_t createMultiplierCallback( +SolverObserver::multiplier_callback_t createMultiplierCallback( ::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, const std::vector& topicNames, CallbackInterpolationStrategy interpolationStrategy = CallbackInterpolationStrategy::nearest_time); diff --git a/ocs2_ros_interfaces/package.xml b/ocs2_ros_interfaces/package.xml index a18ee22b9..3398852f0 100644 --- a/ocs2_ros_interfaces/package.xml +++ b/ocs2_ros_interfaces/package.xml @@ -19,7 +19,6 @@ roscpp ocs2_msgs ocs2_core - ocs2_ddp ocs2_mpc std_msgs visualization_msgs diff --git a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp index c492d1c97..7c5e82bcd 100644 --- a/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp +++ b/ocs2_ros_interfaces/src/common/RosMsgConversions.cpp @@ -216,7 +216,22 @@ TargetTrajectories readTargetTrajectoriesMsg(const ocs2_msgs::mpc_target_traject /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -ocs2_msgs::lagrangian_metrics createMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics) { +ocs2_msgs::constraint createConstraintMsg(scalar_t time, const vector_t& constraint) { + ocs2_msgs::constraint constraintMsg; + + constraintMsg.time = time; + constraintMsg.value.resize(constraint.size()); + for (size_t i = 0; i < constraint.size(); i++) { + constraintMsg.value[i] = constraint(i); + } // end of i loop + + return constraintMsg; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +ocs2_msgs::lagrangian_metrics createLagrangianMetricsMsg(scalar_t time, LagrangianMetricsConstRef metrics) { ocs2_msgs::lagrangian_metrics metricsMsg; metricsMsg.time = time; diff --git a/ocs2_ros_interfaces/src/lintTarget.cpp b/ocs2_ros_interfaces/src/lintTarget.cpp index b92babbb4..c816d2a99 100644 --- a/ocs2_ros_interfaces/src/lintTarget.cpp +++ b/ocs2_ros_interfaces/src/lintTarget.cpp @@ -17,6 +17,7 @@ // synchronized_module #include +#include // dummy target for clang toolchain int main() { diff --git a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp index 60397deff..9d013071d 100644 --- a/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp +++ b/ocs2_ros_interfaces/src/mrt/MRT_ROS_Interface.cpp @@ -190,9 +190,9 @@ void MRT_ROS_Interface::readPolicyMsg(const ocs2_msgs::mpc_flattened_controller& /******************************************************************************************************/ void MRT_ROS_Interface::mpcPolicyCallback(const ocs2_msgs::mpc_flattened_controller::ConstPtr& msg) { // read new policy and command from msg - std::unique_ptr commandPtr(new CommandData); - std::unique_ptr primalSolutionPtr(new PrimalSolution); - std::unique_ptr performanceIndicesPtr(new PerformanceIndex); + auto commandPtr = std::make_unique(); + auto primalSolutionPtr = std::make_unique(); + auto performanceIndicesPtr = std::make_unique(); readPolicyMsg(*msg, *commandPtr, *primalSolutionPtr, *performanceIndicesPtr); this->moveToBuffer(std::move(commandPtr), std::move(primalSolutionPtr), std::move(performanceIndicesPtr)); diff --git a/ocs2_ros_interfaces/src/synchronized_module/RosAugmentedLagrangianCallbacks.cpp b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp similarity index 54% rename from ocs2_ros_interfaces/src/synchronized_module/RosAugmentedLagrangianCallbacks.cpp rename to ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp index e1e12907d..be5de2ad2 100644 --- a/ocs2_ros_interfaces/src/synchronized_module/RosAugmentedLagrangianCallbacks.cpp +++ b/ocs2_ros_interfaces/src/synchronized_module/SolverObserverRosCallbacks.cpp @@ -27,7 +27,7 @@ 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 "ocs2_ros_interfaces/synchronized_module/RosAugmentedLagrangianCallbacks.h" +#include "ocs2_ros_interfaces/synchronized_module/SolverObserverRosCallbacks.h" #include "ocs2_core/misc/LinearInterpolation.h" #include "ocs2_ros_interfaces/common/RosMsgConversions.h" @@ -38,12 +38,55 @@ namespace ros { /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback(::ros::NodeHandle& nodeHandle, - const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::constraint_callback_t createConstraintCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { + using vector_ref_array_t = std::vector>; + + if (observingTimePoints.size() != topicNames.size()) { + throw std::runtime_error("[createConstraintCallback] For each observing time points, you should provide a unique topic name!"); + } + + std::vector<::ros::Publisher> constraintPublishers; + for (const auto& name : topicNames) { + constraintPublishers.push_back(nodeHandle.advertise(name, 1, true)); + } + + // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher + // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks + // associated with that handle will stop being called.") + return [=](const scalar_array_t& timeTrajectory, const vector_ref_array_t& termConstraintArray) { + if (!timeTrajectory.empty()) { + for (size_t i = 0; i < observingTimePoints.size(); i++) { + const auto t = timeTrajectory.front() + observingTimePoints[i]; + const auto indexAlpha = LinearInterpolation::timeSegment(t, timeTrajectory); + const auto constraint = [&]() -> vector_t { + switch (interpolationStrategy) { + case CallbackInterpolationStrategy::nearest_time: + return (indexAlpha.second > 0.5) ? termConstraintArray[indexAlpha.first].get() + : termConstraintArray[indexAlpha.first + 1].get(); + case CallbackInterpolationStrategy::linear_interpolation: + return LinearInterpolation::interpolate( + indexAlpha, termConstraintArray, + [](const vector_ref_array_t& array, size_t t) -> const vector_t& { return array[t].get(); }); + default: + throw std::runtime_error("[createConstraintCallback] This CallbackInterpolationStrategy is not implemented!"); + } + }(); + constraintPublishers[i].publish(ros_msg_conversions::createConstraintMsg(t, constraint)); + } + } + }; +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +SolverObserver::lagrangian_callback_t createLagrangianCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createMetricsCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error("[createLagrangianCallback] For each observing time points, you should provide a unique topic name!"); } std::vector<::ros::Publisher> metricsPublishers; @@ -52,8 +95,8 @@ AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback(::ros::Nod } // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // has internal ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks associated - // with that handle will stop being called.") + // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks + // associated with that handle will stop being called.") return [=](const scalar_array_t& timeTrajectory, const std::vector& termMetricsArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { @@ -67,10 +110,10 @@ AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback(::ros::Nod case CallbackInterpolationStrategy::linear_interpolation: return LinearInterpolation::interpolate(indexAlpha, termMetricsArray); default: - throw std::runtime_error("[createMetricsCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error("[createLagrangianCallback] This CallbackInterpolationStrategy is not implemented!"); } }(); - metricsPublishers[i].publish(ros_msg_conversions::createMetricsMsg(t, metrics)); + metricsPublishers[i].publish(ros_msg_conversions::createLagrangianMetricsMsg(t, metrics)); } } }; @@ -79,12 +122,11 @@ AugmentedLagrangianObserver::metrics_callback_t createMetricsCallback(::ros::Nod /******************************************************************************************************/ /******************************************************************************************************/ /******************************************************************************************************/ -AugmentedLagrangianObserver::multiplier_callback_t createMultiplierCallback(::ros::NodeHandle& nodeHandle, - const scalar_array_t& observingTimePoints, - const std::vector& topicNames, - CallbackInterpolationStrategy interpolationStrategy) { +SolverObserver::multiplier_callback_t createMultiplierCallback(::ros::NodeHandle& nodeHandle, const scalar_array_t& observingTimePoints, + const std::vector& topicNames, + CallbackInterpolationStrategy interpolationStrategy) { if (observingTimePoints.size() != topicNames.size()) { - throw std::runtime_error("[createMetricsCallback] For each observing time points, you should provide a unique topic name!"); + throw std::runtime_error("[createMultiplierCallback] For each observing time points, you should provide a unique topic name!"); } std::vector<::ros::Publisher> multiplierPublishers; @@ -93,8 +135,8 @@ AugmentedLagrangianObserver::multiplier_callback_t createMultiplierCallback(::ro } // note that we need to copy the publishers as the local ones will go out of scope. Good news is that ROS publisher - // has internal ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks associated - // with that handle will stop being called.") + // behaves like std::sharted_ptr ("Once all copies of a specific Publisher go out of scope, any subscriber status callbacks + // associated with that handle will stop being called.") return [=](const scalar_array_t& timeTrajectory, const std::vector& termMultiplierArray) { if (!timeTrajectory.empty()) { for (size_t i = 0; i < observingTimePoints.size(); i++) { @@ -108,7 +150,7 @@ AugmentedLagrangianObserver::multiplier_callback_t createMultiplierCallback(::ro case CallbackInterpolationStrategy::linear_interpolation: return LinearInterpolation::interpolate(indexAlpha, termMultiplierArray); default: - throw std::runtime_error("[createMetricsCallback] This CallbackInterpolationStrategy is not implemented!"); + throw std::runtime_error("[createMultiplierCallback] This CallbackInterpolationStrategy is not implemented!"); } }(); multiplierPublishers[i].publish(ros_msg_conversions::createMultiplierMsg(t, multiplier)); diff --git a/ocs2_slp/CMakeLists.txt b/ocs2_slp/CMakeLists.txt new file mode 100644 index 000000000..3ac73e7de --- /dev/null +++ b/ocs2_slp/CMakeLists.txt @@ -0,0 +1,111 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ocs2_slp) + +set(CATKIN_PACKAGE_DEPENDENCIES + ocs2_core + ocs2_mpc + ocs2_oc + ocs2_qp_solver +) + +find_package(catkin REQUIRED COMPONENTS + ${CATKIN_PACKAGE_DEPENDENCIES} +) + +find_package(Boost REQUIRED COMPONENTS + system + filesystem +) + +find_package(Eigen3 3.3 REQUIRED NO_MODULE) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIRS} + CATKIN_DEPENDS + ${CATKIN_PACKAGE_DEPENDENCIES} + LIBRARIES + ${PROJECT_NAME} + DEPENDS + Boost +) + +########### +## Build ## +########### +include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +add_library(${PROJECT_NAME} + src/pipg/PipgSettings.cpp + src/pipg/PipgSolver.cpp + src/pipg/SingleThreadPipg.cpp + src/Helpers.cpp + src/SlpSettings.cpp + src/SlpSolver.cpp +) +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${Boost_LIBRARIES} +) +target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS}) + +add_executable(${PROJECT_NAME}_lintTarget + src/lintTarget.cpp +) + +######################### +### CLANG TOOLING ### +######################### +find_package(cmake_clang_tools QUIET) +if(cmake_clang_tools_FOUND) + message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget) + add_clang_tooling( + TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test + CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include + CF_WERROR + ) +endif(cmake_clang_tools_FOUND) + +############# +## Install ## +############# +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +############# +## Testing ## +############# + +catkin_add_gtest(test_${PROJECT_NAME} + test/testHelpers.cpp + test/testPipgSolver.cpp + test/testSlpSolver.cpp +) +add_dependencies(test_${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) +target_link_libraries(test_${PROJECT_NAME} + ${PROJECT_NAME} + ${catkin_LIBRARIES} + gtest_main +) diff --git a/ocs2_slp/include/ocs2_slp/Helpers.h b/ocs2_slp/include/ocs2_slp/Helpers.h new file mode 100644 index 000000000..769ae24a6 --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/Helpers.h @@ -0,0 +1,158 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include + +namespace ocs2 { +namespace slp { + +/** + * Computes the upper bound of eigenvalues for the total cost hessian matrix. The bound is computed based on the Gershgorin Circle Theorem. + * + * As the hessian matrix is a real symmetric matrix, all eigenvalues are real. Therefore, instead of estimating the + * upper bound on a complex plane, we can estimate the bound along a 1D real number axis. The estimated upper bound is + * defined as max_{i} [H_{ii} + R_{i}], where R_{i} is the radius of i'th Gershgorin discs. The Gershgorin discs radius is + * defined ad R_{i} = sum_{j \neq i} |H_{ij}|. Thus the upper bound of eigenvalues yields from max_{i}sum{j} |H_{ij}|. + * + * Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * totalCost = 0.5 z' H z + z' h + h0 + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * @param [in] ocpSize: The size of optimal control problem. + * @param [in] cost: Quadratic approximation of the cost over the time horizon. + * @return: The upper bound of eigenvalues for H. + */ +scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector& cost); + +/** + * Computes the upper bound of eigenvalues for the matrix G G' ( in parallel). The bound is computed based on the Gershgorin Circle Theorem. + * + * As the G G' matrix is a real symmetric matrix, all eigenvalues are real. Therefore, instead of estimating the + * upper bound on a complex plane, we can estimate the bound along a 1D real number axis. The estimated upper bound is + * defined as max_{i} [GG'_{ii} + R_{i}], where R_{i} is the radius of i'th Gershgorin discs. The Gershgorin discs radius is + * defined ad R_{i} = sum_{j \neq i} |GG'_{ij}|. Thus the upper bound of eigenvalues yields from max_{i}sum{j} |GG'_{ij}|. + * + * Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * G z = g + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * @param [in] threadPool: The thread pool. + * @param [in] ocpSize: The size of optimal control problem. + * @param [in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param [in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param [in] scalingVectorsPtr: Vector representation for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @return: The upper bound of eigenvalues for G G'. + */ +scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector& dynamics, + const std::vector* constraintsPtr, + const vector_array_t* scalingVectorsPtr); + +/** + * Computes the row-wise absolute sum of the cost hessian matrix, H. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * totalCost = 0.5 z' H z + z' h + h0 + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * @param [in] ocpSize: The size of optimal control problem. + * @param [in] cost: Quadratic approximation of the cost over the time horizon. + * @return: The absolute sum of rows of matrix H. + */ +vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector& cost); + +/** + * Computes the row-wise absolute sum of matrix G G' in parallel. Also refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * z = [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * G z = g + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * @param [in] threadPool: The thread pool. + * @param [in] ocpSize: The size of optimal control problem. + * @param [in] dynamics: Linear approximation of the dynamics over the time horizon. + * @param [in] constraints: Linear approximation of the constraints over the time horizon. Pass nullptr if there is no constraints. + * @param [in] scalingVectorsPtr: Vector representation for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @return The absolute sum of rows of matrix G G'. + */ +vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector& dynamics, + const std::vector* constraintsPtr, + const vector_array_t* scalingVectorsPtr); + +} // namespace slp +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_slp/include/ocs2_slp/SlpMpc.h b/ocs2_slp/include/ocs2_slp/SlpMpc.h new file mode 100644 index 000000000..517ad094c --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/SlpMpc.h @@ -0,0 +1,72 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include "ocs2_slp/SlpSolver.h" + +namespace ocs2 { + +class SlpMpc final : public MPC_BASE { + public: + /** + * Constructor + * + * @param [in] mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use + * slp::Settings directly. + * @param [in] settings : settings for the multiple shooting SLP solver. + * @param [in] optimalControlProblem: The optimal control problem formulation. + * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. + */ + SlpMpc(mpc::Settings mpcSettings, slp::Settings slpSettings, const OptimalControlProblem& optimalControlProblem, + const Initializer& initializer) + : MPC_BASE(std::move(mpcSettings)) { + solverPtr_.reset(new SlpSolver(std::move(slpSettings), optimalControlProblem, initializer)); + }; + + ~SlpMpc() override = default; + + SlpSolver* getSolverPtr() override { return solverPtr_.get(); } + const SlpSolver* getSolverPtr() const override { return solverPtr_.get(); } + + protected: + void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override { + if (settings().coldStart_) { + solverPtr_->reset(); + } + solverPtr_->run(initTime, initState, finalTime); + } + + private: + std::unique_ptr solverPtr_; +}; + +} // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/SlpSettings.h b/ocs2_slp/include/ocs2_slp/SlpSettings.h new file mode 100644 index 000000000..766505100 --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/SlpSettings.h @@ -0,0 +1,92 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include + +#include "ocs2_slp/pipg/PipgSettings.h" + +namespace ocs2 { +namespace slp { + +/** Multiple-shooting SLP (Successive Linear Programming) settings */ +struct Settings { + size_t slpIteration = 10; // Maximum number of SLP iterations + size_t scalingIteration = 3; // Number of pre-conditioning iterations + scalar_t deltaTol = 1e-6; // Termination condition : RMS update of x(t) and u(t) are both below this value + scalar_t costTol = 1e-4; // Termination condition : (cost{i+1} - (cost{i}) < costTol AND constraints{i+1} < g_min + + // Linesearch - step size rules + scalar_t alpha_decay = 0.5; // multiply the step size by this factor every time a linesearch step is rejected. + scalar_t alpha_min = 1e-4; // terminate linesearch if the attempted step size is below this threshold + + // Linesearch - step acceptance criteria with c = costs, g = the norm of constraint violation, and w = [x; u] + scalar_t g_max = 1e6; // (1): IF g{i+1} > g_max REQUIRE g{i+1} < (1-gamma_c) * g{i} + scalar_t g_min = 1e-6; // (2): ELSE IF (g{i} < g_min AND g{i+1} < g_min AND dc/dw'{i} * delta_w < 0) REQUIRE armijo condition + scalar_t armijoFactor = 1e-4; // Armijo condition: c{i+1} < c{i} + armijoFactor * dc/dw'{i} * delta_w + scalar_t gamma_c = 1e-6; // (3): ELSE REQUIRE c{i+1} < (c{i} - gamma_c * g{i}) OR g{i+1} < (1-gamma_c) * g{i} + + // Discretization method + scalar_t dt = 0.01; // user-defined time discretization + SensitivityIntegratorType integratorType = SensitivityIntegratorType::RK2; + + // Inequality penalty relaxed barrier parameters + scalar_t inequalityConstraintMu = 0.0; + scalar_t inequalityConstraintDelta = 1e-6; + + // Extract the Lagrange multiplier of the projected state-input constraint Cx+Du+e + bool extractProjectionMultiplier = false; + + // Printing + bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem + bool printSolverStatistics = false; // Print benchmarking of the multiple shooting method + bool printLinesearch = false; // Print linesearch information + + // Threading + size_t nThreads = 4; + int threadPriority = 50; + + // LP subproblem solver settings + pipg::Settings pipgSettings = pipg::Settings(); +}; + +/** + * Loads the multiple shooting SLP settings from a given file. + * + * @param [in] filename: File name which contains the configuration data. + * @param [in] fieldName: Field name which contains the configuration data. + * @param [in] verbose: Flag to determine whether to print out the loaded settings or not. + * @return The settings + */ +Settings loadSettings(const std::string& filename, const std::string& fieldName = "multiple_shooting", bool verbose = true); + +} // namespace slp +} // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/SlpSolver.h b/ocs2_slp/include/ocs2_slp/SlpSolver.h new file mode 100644 index 000000000..7d0623f0e --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/SlpSolver.h @@ -0,0 +1,200 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "ocs2_slp/SlpSettings.h" +#include "ocs2_slp/SlpSolverStatus.h" +#include "ocs2_slp/pipg/PipgSolver.h" + +namespace ocs2 { + +class SlpSolver : public SolverBase { + public: + /** + * Constructor + * + * @param [in] settings : settings for the multiple shooting SLP solver. + * @param [in] optimalControlProblem: The optimal control problem formulation. + * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. + */ + SlpSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); + + ~SlpSolver() override; + + void reset() override; + + scalar_t getFinalTime() const override { return primalSolution_.timeTrajectory_.back(); }; + + void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } + + const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; } + + size_t getNumIterations() const override { return totalNumIterations_; } + + const OptimalControlProblem& getOptimalControlProblem() const override { return ocpDefinitions_.front(); } + + const PerformanceIndex& getPerformanceIndeces() const override { return getIterationsLog().back(); }; + + const std::vector& getIterationsLog() const override; + + ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override { + throw std::runtime_error("[SlpSolver] getValueFunction() not available yet."); + }; + + ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) override { + throw std::runtime_error("[SlpSolver] getHamiltonian() not available yet."); + } + + vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override { + throw std::runtime_error("[SlpSolver] getStateInputEqualityConstraintLagrangian() not available yet."); + } + + MultiplierCollection getIntermediateDualSolution(scalar_t time) const override { + throw std::runtime_error("[SqpSolver] getIntermediateDualSolution() not available yet."); + } + + private: + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override; + + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const ControllerBase* externalControllerPtr) override { + if (externalControllerPtr == nullptr) { + runImpl(initTime, initState, finalTime); + } else { + throw std::runtime_error("[SlpSolver::run] This solver does not support external controller!"); + } + } + + void runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime, const PrimalSolution& primalSolution) override { + // Copy all except the controller + primalSolution_.timeTrajectory_ = primalSolution.timeTrajectory_; + primalSolution_.stateTrajectory_ = primalSolution.stateTrajectory_; + primalSolution_.inputTrajectory_ = primalSolution.inputTrajectory_; + primalSolution_.postEventIndices_ = primalSolution.postEventIndices_; + primalSolution_.modeSchedule_ = primalSolution.modeSchedule_; + runImpl(initTime, initState, finalTime); + } + + /** Run a task in parallel with settings.nThreads */ + void runParallel(std::function taskFunction); + + /** Get profiling information as a string */ + std::string getBenchmarkingInformation() const; + + std::string getBenchmarkingInformationPIPG() const; + + /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ + PerformanceIndex setupQuadraticSubproblem(const std::vector& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, std::vector& metrics); + + /** Computes only the performance metrics at the current {t, x(t), u(t)} */ + PerformanceIndex computePerformance(const std::vector& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, std::vector& metrics); + + /** Returns solution of the QP subproblem in delta coordinates: */ + struct OcpSubproblemSolution { + vector_array_t deltaXSol; // delta_x(t) + vector_array_t deltaUSol; // delta_u(t) + scalar_t armijoDescentMetric; // inner product of the cost gradient and decision variable step + }; + OcpSubproblemSolution getOCPSolution(const vector_t& delta_x0); + + /** Constructs the primal solution based on the optimized state and input trajectories */ + PrimalSolution toPrimalSolution(const std::vector& time, vector_array_t&& x, vector_array_t&& u); + + /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ + slp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector& timeDiscretization, const vector_t& initState, + const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, + std::vector& metrics); + + /** Determine convergence after a step */ + slp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const; + + // Problem definition + const slp::Settings settings_; + DynamicsDiscretizer discretizer_; + DynamicsSensitivityDiscretizer sensitivityDiscretizer_; + std::vector ocpDefinitions_; + std::unique_ptr initializerPtr_; + FilterLinesearch filterLinesearch_; + + // Solver interface + PipgSolver pipgSolver_; + + // Threading + ThreadPool threadPool_; + + // Solution + PrimalSolution primalSolution_; + + // LQ approximation + std::vector cost_; + std::vector dynamics_; + std::vector stateInputEqConstraints_; + std::vector stateIneqConstraints_; + std::vector stateInputIneqConstraints_; + std::vector constraintsProjection_; + + // Lagrange multipliers + std::vector projectionMultiplierCoefficients_; + + // Iteration performance log + std::vector performanceIndeces_; + + // The ProblemMetrics associated to primalSolution_ + ProblemMetrics problemMetrics_; + + // Benchmarking + size_t numProblems_{0}; + size_t totalNumIterations_{0}; + benchmark::RepeatedTimer initializationTimer_; + benchmark::RepeatedTimer linearQuadraticApproximationTimer_; + benchmark::RepeatedTimer solveQpTimer_; + benchmark::RepeatedTimer linesearchTimer_; + benchmark::RepeatedTimer computeControllerTimer_; + + // PIPG Solver + benchmark::RepeatedTimer lambdaEstimation_; + benchmark::RepeatedTimer sigmaEstimation_; + benchmark::RepeatedTimer preConditioning_; + benchmark::RepeatedTimer pipgSolverTimer_; +}; + +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp b/ocs2_slp/include/ocs2_slp/SlpSolverStatus.h similarity index 67% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp rename to ocs2_slp/include/ocs2_slp/SlpSolverStatus.h index 091a4b0e9..9d70d36cf 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolverStatus.cpp +++ b/ocs2_slp/include/ocs2_slp/SlpSolverStatus.h @@ -27,29 +27,37 @@ 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 "ocs2_sqp/MultipleShootingSolverStatus.h" +#pragma once + +#include + +#include +#include +#include namespace ocs2 { -namespace multiple_shooting { +namespace slp { -std::string toString(const StepInfo::StepType& stepType) { - using StepType = StepInfo::StepType; - switch (stepType) { - case StepType::COST: - return "Cost"; - case StepType::CONSTRAINT: - return "Constraint"; - case StepType::DUAL: - return "Dual"; - case StepType::ZERO: - return "Zero"; - case StepType::UNKNOWN: - default: - return "Unknown"; - } -} +/** Different types of convergence */ +enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; + +/** Struct to contain the result and logging data of the stepsize computation */ +struct StepInfo { + // Step size and type + scalar_t stepSize = 0.0; + FilterLinesearch::StepType stepType = FilterLinesearch::StepType::UNKNOWN; + + // Step in primal variables + scalar_t dx_norm = 0.0; // norm of the state trajectory update + scalar_t du_norm = 0.0; // norm of the input trajectory update + + // Performance result after the step + PerformanceIndex performanceAfterStep; + scalar_t totalConstraintViolationAfterStep; // constraint metric used in the line search +}; -std::string toString(const Convergence& convergence) { +/** Transforms pipg::Convergence to string */ +inline std::string toString(const Convergence& convergence) { switch (convergence) { case Convergence::ITERATIONS: return "Maximum number of iterations reached"; @@ -65,5 +73,5 @@ std::string toString(const Convergence& convergence) { } } -} // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace slp +} // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgBounds.h b/ocs2_slp/include/ocs2_slp/pipg/PipgBounds.h new file mode 100644 index 000000000..c0659a93d --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgBounds.h @@ -0,0 +1,76 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +namespace ocs2 { +namespace pipg { + +/** + * Defines the lower and upper bounds of the total cost hessian, H, and the upper bound of \f$ G^TG \f$. + * Refer to "Proportional-Integral Projected Gradient Method for Model Predictive Control" + * Link: https://arxiv.org/abs/2009.06980 + * + * z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * min 0.5 z' H z + z' h + * s.t. G z = g + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + */ +struct PipgBounds { + PipgBounds(scalar_t muArg, scalar_t lambdaArg, scalar_t sigmaArg) : mu(muArg), lambda(lambdaArg), sigma(sigmaArg) {} + + scalar_t dualStepSize(size_t iteration) const { return (static_cast(iteration) + 1.0) * mu / (2.0 * sigma); } + + scalar_t primalStepSize(size_t iteration) const { return 2.0 / ((static_cast(iteration) + 1.0) * mu + 2.0 * lambda); } + + scalar_t mu = 1e-5; // mu I <= H + scalar_t lambda = 1.0; // H <= lambda I + scalar_t sigma = 1.0; // G' G <= sigma I +}; + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h new file mode 100644 index 000000000..1df95244a --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h @@ -0,0 +1,62 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +namespace ocs2 { +namespace pipg { + +struct Settings { + /** Maximum number of iterations of PIPG. */ + size_t maxNumIterations = 3000; + /** Termination criteria. **/ + scalar_t absoluteTolerance = 1e-3; + scalar_t relativeTolerance = 1e-2; + /** Number of iterations between consecutive calculation of termination conditions. **/ + size_t checkTerminationInterval = 1; + /** The static lower bound of the cost hessian H. **/ + scalar_t lowerBoundH = 5e-6; + /** This value determines to display the a summary log. */ + bool displayShortSummary = false; +}; + +/** + * Loads the PIPG settings from a given file. + * + * @param [in] filename: File name which contains the configuration data. + * @param [in] fieldName: Field name which contains the configuration data. + * @param [in] verbose: Flag to determine whether to print out the loaded settings or not. + * @return The settings + */ +Settings loadSettings(const std::string& filename, const std::string& fieldName = "pipg", bool verbose = true); + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h new file mode 100644 index 000000000..5a631e0cd --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h @@ -0,0 +1,110 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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. +******************************************************************************/ + +#pragma once + +#include + +#include + +#include +#include +#include +#include + +#include "ocs2_slp/pipg/PipgBounds.h" +#include "ocs2_slp/pipg/PipgSettings.h" +#include "ocs2_slp/pipg/PipgSolverStatus.h" + +namespace ocs2 { + +/* + * First order primal-dual method for solving optimal control problem based on: + * "Proportional-Integral Projected Gradient Method for Model Predictive Control" + * https://arxiv.org/abs/2009.06980 + */ +class PipgSolver { + public: + /** + * Constructor. + * @param[in] Settings: PIPG setting + */ + explicit PipgSolver(pipg::Settings settings); + + /** + * Solve the optimal control in parallel. + * + * @param [in] threadPool : The external thread pool. + * @param [in] x0 : Initial state + * @param [in] dynamics : Dynamics array. + * @param [in] cost : Cost array. + * @param [in] constraints : Constraints array. Pass nullptr for an unconstrained problem. + * @param [in] scalingVectors : Vector representation for the identity parts of the dynamics inside the constraint matrix. After scaling, + * they become arbitrary diagonal matrices. Pass nullptr to get them filled with identity matrices. + * @param [in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. + * @param [in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. + * @param [out] xTrajectory : The optimized state trajectory. + * @param [out] uTrajectory : The optimized input trajectory. + * @return The solver status. + */ + pipg::SolverStatus solve(ThreadPool& threadPool, const vector_t& x0, std::vector& dynamics, + const std::vector& cost, + const std::vector* constraints, const vector_array_t& scalingVectors, + const vector_array_t* EInv, const pipg::PipgBounds& pipgBounds, vector_array_t& xTrajectory, + vector_array_t& uTrajectory); + + void resize(const OcpSize& size); + + int getNumDecisionVariables() const { return numDecisionVariables_; } + int getNumDynamicsConstraints() const { return numDynamicsConstraints_; } + + const OcpSize& size() const { return ocpSize_; } + const pipg::Settings& settings() const { return settings_; } + + private: + void verifySizes(const std::vector& dynamics, + const std::vector& cost, + const std::vector* constraints) const; + + void verifyOcpSize(const OcpSize& ocpSize) const; + + // Settings + const pipg::Settings settings_; + + // Problem size + OcpSize ocpSize_; + int numDecisionVariables_; + int numDynamicsConstraints_; + + // Data buffer for parallelized PIPG + vector_array_t X_, W_, V_, U_; + vector_array_t XNew_, UNew_, WNew_; +}; + +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp b/ocs2_slp/include/ocs2_slp/pipg/PipgSolverStatus.h similarity index 75% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp rename to ocs2_slp/include/ocs2_slp/pipg/PipgSolverStatus.h index ebce7ef06..b12f76600 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingInitialization.cpp +++ b/ocs2_slp/include/ocs2_slp/pipg/PipgSolverStatus.h @@ -27,17 +27,30 @@ 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 "ocs2_sqp/MultipleShootingInitialization.h" +#pragma once -#include +#include namespace ocs2 { -namespace multiple_shooting { - -std::pair initializeIntermediateNode(PrimalSolution& primalSolution, scalar_t t, scalar_t tNext, const vector_t& x) { - return {LinearInterpolation::interpolate(t, primalSolution.timeTrajectory_, primalSolution.inputTrajectory_), - LinearInterpolation::interpolate(tNext, primalSolution.timeTrajectory_, primalSolution.stateTrajectory_)}; +namespace pipg { + +enum class SolverStatus { + SUCCESS, + MAX_ITER, + UNDEFINED, +}; + +/** Transforms pipg::SolverStatus to string */ +inline std::string toString(SolverStatus s) { + switch (s) { + case SolverStatus::SUCCESS: + return std::string("SUCCESS"); + case SolverStatus::MAX_ITER: + return std::string("MAX_ITER"); + default: + return std::string("UNDEFINED"); + } } -} // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h new file mode 100644 index 000000000..9274bfe20 --- /dev/null +++ b/ocs2_slp/include/ocs2_slp/pipg/SingleThreadPipg.h @@ -0,0 +1,87 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include +#include + +#include "ocs2_slp/pipg/PipgBounds.h" +#include "ocs2_slp/pipg/PipgSettings.h" +#include "ocs2_slp/pipg/PipgSolverStatus.h" + +namespace ocs2 { +namespace pipg { + +/** + * First order primal-dual method for solving optimal control problem based on: + * "Proportional-Integral Projected Gradient Method for Model Predictive Control" + * https://arxiv.org/abs/2009.06980 + * + * z := [u_{0}; x_{1}; ...; u_{n}; x_{n+1}]. + * + * min 0.5 z' H z + z' h + * s.t. G z = g + * + * H = [ R0 + * * Q1 P1' + * * P1 R1 + * * * * Qn Pn' + * * * * Pn Rn + * * * * * * Q{n+1}] + * h = [(P0 x0 + r0); q1; r1 ...; qn; rn; q_{n+1}] + * + * G = [-B0 I + * * -A1 -B1 I + * + * * * * -An -Bn I + * D0 0 + * * C1 D1 0 + * + * * * * Cn Dn 0] + * g = [(A0 x0 + b0); b1; ...; bn, -(C0 x0 + e0); -e1; ...; en] + * + * For constructing H, h, G, and g, refer to "ocs2_oc/oc_problem/OcpToKkt.h". + * + * @param [in] settings : The PIPG settings. + * @param [in] H : The hessian matrix of the total cost. + * @param [in] h : The jacobian vector of the total cost. + * @param [in] G : The jacobian matrix of the constarinst. + * @param [in] g : The constraints vector. + * @param [in] EInv : Inverse of the scaling factor E. Used to calculate un-sacled termination criteria. + * @param [in] pipgBounds : The PipgBounds used to define the primal and dual stepsizes. + * @param [out] stackedSolution : The concatenated state-input trajectories, z. + * @return The solver status. + */ +SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::SparseMatrix& H, const vector_t& h, + const Eigen::SparseMatrix& G, const vector_t& g, const vector_t& EInv, const PipgBounds& pipgBounds, + vector_t& stackedSolution); + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_slp/package.xml b/ocs2_slp/package.xml new file mode 100644 index 000000000..52ba012d7 --- /dev/null +++ b/ocs2_slp/package.xml @@ -0,0 +1,20 @@ + + + ocs2_slp + 0.0.0 + A numerical implementation of a first order primal-dual MPC basee on PIPG. + + Farbod Farshidian + Zhengyu Fu + + BSD3 + + catkin + ocs2_core + ocs2_mpc + ocs2_oc + + + ocs2_qp_solver + + \ No newline at end of file diff --git a/ocs2_slp/src/Helpers.cpp b/ocs2_slp/src/Helpers.cpp new file mode 100644 index 000000000..8c3c397c3 --- /dev/null +++ b/ocs2_slp/src/Helpers.cpp @@ -0,0 +1,163 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_slp/Helpers.h" + +#include +#include + +namespace { +int getNumDecisionVariables(const ocs2::OcpSize& ocpSize) { + return std::accumulate(ocpSize.numInputs.begin(), ocpSize.numInputs.end(), + std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0)); +} + +int getNumDynamicsConstraints(const ocs2::OcpSize& ocpSize) { + return std::accumulate(ocpSize.numStates.begin() + 1, ocpSize.numStates.end(), (int)0); +} + +int getNumGeneralEqualityConstraints(const ocs2::OcpSize& ocpSize) { + return std::accumulate(ocpSize.numIneqConstraints.begin(), ocpSize.numIneqConstraints.end(), (int)0); +} +} // anonymous namespace + +namespace ocs2 { +namespace slp { + +scalar_t hessianEigenvaluesUpperBound(const OcpSize& ocpSize, const std::vector& cost) { + const vector_t rowwiseAbsSumH = hessianAbsRowSum(ocpSize, cost); + return rowwiseAbsSumH.maxCoeff(); +} + +scalar_t GGTEigenvaluesUpperBound(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector& dynamics, + const std::vector* constraintsPtr, + const vector_array_t* scalingVectorsPtr) { + const vector_t rowwiseAbsSumGGT = GGTAbsRowSumInParallel(threadPool, ocpSize, dynamics, constraintsPtr, scalingVectorsPtr); + return rowwiseAbsSumGGT.maxCoeff(); +} + +vector_t hessianAbsRowSum(const OcpSize& ocpSize, const std::vector& cost) { + const int N = ocpSize.numStages; + const int nu_0 = ocpSize.numInputs[0]; + vector_t res(getNumDecisionVariables(ocpSize)); + res.head(nu_0) = cost[0].dfduu.cwiseAbs().rowwise().sum(); + + int curRow = nu_0; + for (int k = 1; k < N; k++) { + const int nx_k = ocpSize.numStates[k]; + const int nu_k = ocpSize.numInputs[k]; + if (cost[k].dfdxx.size() != 0) { + res.segment(curRow, nx_k) = cost[k].dfdxx.cwiseAbs().rowwise().sum(); + } + if (cost[k].dfdux.size() != 0) { + res.segment(curRow, nx_k) += cost[k].dfdux.transpose().cwiseAbs().rowwise().sum(); + } + if (cost[k].dfdux.size() != 0) { + res.segment(curRow + nx_k, nu_k) = cost[k].dfdux.cwiseAbs().rowwise().sum(); + } + if (cost[k].dfduu.size() != 0) { + res.segment(curRow + nx_k, nu_k) += cost[k].dfduu.cwiseAbs().rowwise().sum(); + } + curRow += nx_k + nu_k; + } + const int nx_N = ocpSize.numStates[N]; + res.tail(nx_N) = cost[N].dfdxx.cwiseAbs().rowwise().sum(); + return res; +} + +vector_t GGTAbsRowSumInParallel(ThreadPool& threadPool, const OcpSize& ocpSize, + const std::vector& dynamics, + const std::vector* constraints, + const vector_array_t* scalingVectorsPtr) { + const int N = ocpSize.numStages; + if (N < 1) { + throw std::runtime_error("[GGTAbsRowSumInParallel] The number of stages cannot be less than 1."); + } + if (scalingVectorsPtr != nullptr && scalingVectorsPtr->size() != N) { + throw std::runtime_error("[GGTAbsRowSumInParallel] The size of scalingVectors doesn't match the number of stage."); + } + Eigen::setNbThreads(1); // No multithreading within Eigen. + + matrix_array_t tempMatrixArray(N); + vector_array_t absRowSumArray(N); + + std::atomic_int timeIndex{0}; + auto task = [&](int workerId) { + int k; + while ((k = timeIndex++) < N) { + const auto nx_next = ocpSize.numStates[k + 1]; + const auto& B = dynamics[k].dfdu; + tempMatrixArray[k] = + (scalingVectorsPtr == nullptr ? matrix_t::Identity(nx_next, nx_next) + : (*scalingVectorsPtr)[k].cwiseProduct((*scalingVectorsPtr)[k]).asDiagonal().toDenseMatrix()); + tempMatrixArray[k] += B * B.transpose(); + + if (k != 0) { + const auto& A = dynamics[k].dfdx; + tempMatrixArray[k] += A * A.transpose(); + } + + absRowSumArray[k] = tempMatrixArray[k].cwiseAbs().rowwise().sum(); + + if (k != 0) { + const auto& A = dynamics[k].dfdx; + absRowSumArray[k] += (A * (scalingVectorsPtr == nullptr ? matrix_t::Identity(A.cols(), A.cols()) + : (*scalingVectorsPtr)[k - 1].asDiagonal().toDenseMatrix())) + .cwiseAbs() + .rowwise() + .sum(); + } + if (k != N - 1) { + const auto& ANext = dynamics[k + 1].dfdx; + absRowSumArray[k] += (ANext * (scalingVectorsPtr == nullptr ? matrix_t::Identity(ANext.cols(), ANext.cols()) + : (*scalingVectorsPtr)[k].asDiagonal().toDenseMatrix())) + .transpose() + .cwiseAbs() + .rowwise() + .sum(); + } + } + }; + threadPool.runParallel(std::move(task), threadPool.numThreads() + 1U); + + vector_t res = vector_t::Zero(getNumDynamicsConstraints(ocpSize)); + int curRow = 0; + for (const auto& v : absRowSumArray) { + res.segment(curRow, v.size()) = v; + curRow += v.size(); + } + + Eigen::setNbThreads(0); // Restore default setup. + + return res; +} + +} // namespace slp +} // namespace ocs2 diff --git a/ocs2_slp/src/SlpSettings.cpp b/ocs2_slp/src/SlpSettings.cpp new file mode 100644 index 000000000..8e0f4a550 --- /dev/null +++ b/ocs2_slp/src/SlpSettings.cpp @@ -0,0 +1,85 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_slp/SlpSettings.h" + +#include + +#include +#include + +#include + +namespace ocs2 { +namespace slp { + +Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(filename, pt); + + Settings settings; + + if (verbose) { + std::cerr << "\n #### Multiple-Shooting SLP Settings:"; + std::cerr << "\n #### =============================================================================\n"; + } + + loadData::loadPtreeValue(pt, settings.slpIteration, fieldName + ".slpIteration", verbose); + loadData::loadPtreeValue(pt, settings.scalingIteration, fieldName + ".scalingIteration", verbose); + loadData::loadPtreeValue(pt, settings.deltaTol, fieldName + ".deltaTol", verbose); + loadData::loadPtreeValue(pt, settings.alpha_decay, fieldName + ".alpha_decay", verbose); + loadData::loadPtreeValue(pt, settings.alpha_min, fieldName + ".alpha_min", verbose); + loadData::loadPtreeValue(pt, settings.gamma_c, fieldName + ".gamma_c", verbose); + loadData::loadPtreeValue(pt, settings.g_max, fieldName + ".g_max", verbose); + loadData::loadPtreeValue(pt, settings.g_min, fieldName + ".g_min", verbose); + loadData::loadPtreeValue(pt, settings.armijoFactor, fieldName + ".armijoFactor", verbose); + loadData::loadPtreeValue(pt, settings.costTol, fieldName + ".costTol", verbose); + loadData::loadPtreeValue(pt, settings.dt, fieldName + ".dt", verbose); + auto integratorName = sensitivity_integrator::toString(settings.integratorType); + loadData::loadPtreeValue(pt, integratorName, fieldName + ".integratorType", verbose); + settings.integratorType = sensitivity_integrator::fromString(integratorName); + loadData::loadPtreeValue(pt, settings.inequalityConstraintMu, fieldName + ".inequalityConstraintMu", verbose); + loadData::loadPtreeValue(pt, settings.inequalityConstraintDelta, fieldName + ".inequalityConstraintDelta", verbose); + loadData::loadPtreeValue(pt, settings.extractProjectionMultiplier, fieldName + ".extractProjectionMultiplier", verbose); + loadData::loadPtreeValue(pt, settings.printSolverStatus, fieldName + ".printSolverStatus", verbose); + loadData::loadPtreeValue(pt, settings.printSolverStatistics, fieldName + ".printSolverStatistics", verbose); + loadData::loadPtreeValue(pt, settings.printLinesearch, fieldName + ".printLinesearch", verbose); + loadData::loadPtreeValue(pt, settings.nThreads, fieldName + ".nThreads", verbose); + loadData::loadPtreeValue(pt, settings.threadPriority, fieldName + ".threadPriority", verbose); + settings.pipgSettings = pipg::loadSettings(filename, fieldName + ".pipg", verbose); + + if (verbose) { + std::cerr << " #### =============================================================================" << std::endl; + } + + return settings; +} + +} // namespace slp +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_slp/src/SlpSolver.cpp b/ocs2_slp/src/SlpSolver.cpp new file mode 100644 index 000000000..81c2a6c9c --- /dev/null +++ b/ocs2_slp/src/SlpSolver.cpp @@ -0,0 +1,544 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_slp/SlpSolver.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "ocs2_slp/Helpers.h" + +namespace ocs2 { + +SlpSolver::SlpSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) + : settings_(std::move(settings)), + pipgSolver_(settings_.pipgSettings), + threadPool_(std::max(settings_.nThreads - 1, size_t(1)) - 1, settings_.threadPriority) { + Eigen::setNbThreads(1); // No multithreading within Eigen. + Eigen::initParallel(); + + // Dynamics discretization + discretizer_ = selectDynamicsDiscretization(settings_.integratorType); + sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings_.integratorType); + + // Clone objects to have one for each worker + for (int w = 0; w < settings_.nThreads; w++) { + ocpDefinitions_.push_back(optimalControlProblem); + } + + // Operating points + initializerPtr_.reset(initializer.clone()); + + // Linesearch + filterLinesearch_.g_max = settings_.g_max; + filterLinesearch_.g_min = settings_.g_min; + filterLinesearch_.gamma_c = settings_.gamma_c; + filterLinesearch_.armijoFactor = settings_.armijoFactor; +} + +SlpSolver::~SlpSolver() { + if (settings_.printSolverStatistics) { + std::cerr << getBenchmarkingInformationPIPG() << "\n" << getBenchmarkingInformation() << std::endl; + } +} + +void SlpSolver::reset() { + // Clear solution + primalSolution_ = PrimalSolution(); + performanceIndeces_.clear(); + + // reset timers + numProblems_ = 0; + totalNumIterations_ = 0; + initializationTimer_.reset(); + linearQuadraticApproximationTimer_.reset(); + solveQpTimer_.reset(); + linesearchTimer_.reset(); + computeControllerTimer_.reset(); + lambdaEstimation_.reset(); + sigmaEstimation_.reset(); + preConditioning_.reset(); + pipgSolverTimer_.reset(); +} + +std::string SlpSolver::getBenchmarkingInformationPIPG() const { + const auto lambdaEstimation = lambdaEstimation_.getTotalInMilliseconds(); + const auto sigmaEstimation = sigmaEstimation_.getTotalInMilliseconds(); + const auto preConditioning = preConditioning_.getTotalInMilliseconds(); + const auto pipgRuntime = pipgSolverTimer_.getTotalInMilliseconds(); + + const auto benchmarkTotal = preConditioning + lambdaEstimation + sigmaEstimation + pipgRuntime; + + std::stringstream infoStream; + if (benchmarkTotal > 0.0) { + const scalar_t inPercent = 100.0; + infoStream << "\n########################################################################\n"; + infoStream << "The benchmarking is computed over " << preConditioning_.getNumTimedIntervals() << " iterations. \n"; + infoStream << "PIPG Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tpreConditioning :\t" << std::setw(10) << preConditioning_.getAverageInMilliseconds() << " [ms] \t(" + << preConditioning / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tlambdaEstimation :\t" << std::setw(10) << lambdaEstimation_.getAverageInMilliseconds() << " [ms] \t(" + << lambdaEstimation / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tsigmaEstimation :\t" << std::setw(10) << sigmaEstimation_.getAverageInMilliseconds() << " [ms] \t(" + << sigmaEstimation / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tPIPG runTime :\t" << std::setw(10) << pipgSolverTimer_.getAverageInMilliseconds() << " [ms] \t(" + << pipgRuntime / benchmarkTotal * inPercent << "%)\n"; + } + return infoStream.str(); +} + +std::string SlpSolver::getBenchmarkingInformation() const { + const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); + const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); + const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); + const auto computeControllerTotal = computeControllerTimer_.getTotalInMilliseconds(); + + const auto benchmarkTotal = linearQuadraticApproximationTotal + solveQpTotal + linesearchTotal + computeControllerTotal; + + std::stringstream infoStream; + if (benchmarkTotal > 0.0) { + const scalar_t inPercent = 100.0; + infoStream << "\n########################################################################\n"; + infoStream << "The benchmarking is computed over " << totalNumIterations_ << " iterations. \n"; + infoStream << "SLP Benchmarking\t :\tAverage time [ms] (% of total runtime)\n"; + infoStream << "\tLQ Approximation :\t" << std::setw(10) << linearQuadraticApproximationTimer_.getAverageInMilliseconds() + << " [ms] \t(" << linearQuadraticApproximationTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tSolve LP :\t" << std::setw(10) << solveQpTimer_.getAverageInMilliseconds() << " [ms] \t(" + << solveQpTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tLinesearch :\t" << std::setw(10) << linesearchTimer_.getAverageInMilliseconds() << " [ms] \t(" + << linesearchTotal / benchmarkTotal * inPercent << "%)\n"; + infoStream << "\tCompute Controller :\t" << std::setw(10) << computeControllerTimer_.getAverageInMilliseconds() << " [ms] \t(" + << computeControllerTotal / benchmarkTotal * inPercent << "%)\n"; + } + return infoStream.str(); +} + +const std::vector& SlpSolver::getIterationsLog() const { + if (performanceIndeces_.empty()) { + throw std::runtime_error("[SlpSolver]: No performance log yet, no problem solved yet?"); + } else { + return performanceIndeces_; + } +} + +void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++ SLP solver is initialized ++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + } + + // Determine time discretization, taking into account event times. + const auto& eventTimes = this->getReferenceManager().getModeSchedule().eventTimes; + const auto timeDiscretization = timeDiscretizationWithEvents(initTime, finalTime, settings_.dt, eventTimes); + + // Initialize references + for (auto& ocpDefinition : ocpDefinitions_) { + const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); + ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; + } + + // Trajectory spread of primalSolution_ + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + } + + // Initialize the state and input + vector_array_t x, u; + multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); + + // Bookkeeping + performanceIndeces_.clear(); + std::vector metrics; + + int iter = 0; + slp::Convergence convergence = slp::Convergence::FALSE; + while (convergence == slp::Convergence::FALSE) { + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\nPIPG iteration: " << iter << "\n"; + } + // Make QP approximation + linearQuadraticApproximationTimer_.startTimer(); + const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u, metrics); + linearQuadraticApproximationTimer_.endTimer(); + + // Solve LP + solveQpTimer_.startTimer(); + const vector_t delta_x0 = initState - x[0]; + const auto deltaSolution = getOCPSolution(delta_x0); + solveQpTimer_.endTimer(); + + // Apply step + linesearchTimer_.startTimer(); + const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, metrics); + performanceIndeces_.push_back(stepInfo.performanceAfterStep); + linesearchTimer_.endTimer(); + + // Check convergence + convergence = checkConvergence(iter, baselinePerformance, stepInfo); + + // Next iteration + ++iter; + ++totalNumIterations_; + } + + computeControllerTimer_.startTimer(); + primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); + computeControllerTimer_.endTimer(); + + ++numProblems_; + + if (settings_.printSolverStatus || settings_.printLinesearch) { + std::cerr << "\nConvergence : " << toString(convergence) << "\n"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++ SLP solver has terminated ++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + } +} + +void SlpSolver::runParallel(std::function taskFunction) { + threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); +} + +SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta_x0) { + // Solve the QP + OcpSubproblemSolution solution; + auto& deltaXSol = solution.deltaXSol; + auto& deltaUSol = solution.deltaUSol; + + // without constraints, or when using projection, we have an unconstrained QP. + pipgSolver_.resize(extractSizesFromProblem(dynamics_, cost_, nullptr)); + + // pre-condition the OCP + preConditioning_.startTimer(); + scalar_t c; + vector_array_t D, E; + vector_array_t scalingVectors; + precondition::ocpDataInPlaceInParallel(threadPool_, delta_x0, pipgSolver_.size(), settings_.scalingIteration, dynamics_, cost_, D, E, + scalingVectors, c); + preConditioning_.endTimer(); + + // estimate mu and lambda: mu I < H < lambda I + const auto muEstimated = [&]() { + scalar_t maxScalingFactor = -1; + for (auto& v : D) { + if (v.size() != 0) { + maxScalingFactor = std::max(maxScalingFactor, v.maxCoeff()); + } + } + return c * pipgSolver_.settings().lowerBoundH * maxScalingFactor * maxScalingFactor; + }(); + lambdaEstimation_.startTimer(); + const auto lambdaScaled = slp::hessianEigenvaluesUpperBound(pipgSolver_.size(), cost_); + lambdaEstimation_.endTimer(); + + // estimate sigma: G' G < sigma I + // However, since the G'G and GG' have exactly the same set of eigenvalues value: G G' < sigma I + sigmaEstimation_.startTimer(); + const auto sigmaScaled = slp::GGTEigenvaluesUpperBound(threadPool_, pipgSolver_.size(), dynamics_, nullptr, &scalingVectors); + sigmaEstimation_.endTimer(); + + pipgSolverTimer_.startTimer(); + vector_array_t EInv(E.size()); + std::transform(E.begin(), E.end(), EInv.begin(), [](const vector_t& v) { return v.cwiseInverse(); }); + const pipg::PipgBounds pipgBounds{muEstimated, lambdaScaled, sigmaScaled}; + const auto pipgStatus = + pipgSolver_.solve(threadPool_, delta_x0, dynamics_, cost_, nullptr, scalingVectors, &EInv, pipgBounds, deltaXSol, deltaUSol); + pipgSolverTimer_.endTimer(); + + // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + solution.armijoDescentMetric = armijoDescentMetric(cost_, deltaXSol, deltaUSol); + + precondition::descaleSolution(D, deltaXSol, deltaUSol); + + // remap the tilde delta u to real delta u + multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); + + return solution; +} + +PrimalSolution SlpSolver::toPrimalSolution(const std::vector& time, vector_array_t&& x, vector_array_t&& u) { + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u)); +} + +PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector& time, const vector_t& initState, + const vector_array_t& x, const vector_array_t& u, std::vector& metrics) { + // Problem horizon + const int N = static_cast(time.size()) - 1; + + std::vector performance(settings_.nThreads, PerformanceIndex()); + cost_.resize(N + 1); + dynamics_.resize(N); + stateInputEqConstraints_.resize(N); + stateIneqConstraints_.resize(N + 1); + stateInputIneqConstraints_.resize(N); + constraintsProjection_.resize(N); + projectionMultiplierCoefficients_.resize(N); + metrics.resize(N + 1); + + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; + PerformanceIndex workerPerformance; // Accumulate performance in local variable + + int i = timeIndex++; + while (i < N) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + // Event node + auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result); + cost_[i] = std::move(result.cost); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i].resize(0, x[i].size()); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); + stateInputIneqConstraints_[i].resize(0, x[i].size()); + constraintsProjection_[i].resize(0, x[i].size()); + projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); + } else { + // Normal, intermediate node + const scalar_t ti = getIntervalStart(time[i]); + const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); + auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result, dt); + multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); + cost_[i] = std::move(result.cost); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); + stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); + constraintsProjection_[i] = std::move(result.constraintsProjection); + projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + const scalar_t tN = getIntervalStart(time[N]); + auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result); + cost_[i] = std::move(result.cost); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); + } + + // Accumulate! Same worker might run multiple tasks + performance[workerId] += workerPerformance; + }; + runParallel(std::move(parallelTask)); + + // Account for init state in performance + performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); + + // Sum performance of the threads + PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); + totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + + return totalPerformance; +} + +PerformanceIndex SlpSolver::computePerformance(const std::vector& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, std::vector& metrics) { + // Problem size + const int N = static_cast(time.size()) - 1; + metrics.resize(N + 1); + + std::vector performance(settings_.nThreads, PerformanceIndex()); + std::atomic_int timeIndex{0}; + auto parallelTask = [&](int workerId) { + // Get worker specific resources + OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; + + int i = timeIndex++; + while (i < N) { + if (time[i].event == AnnotatedTime::Event::PreEvent) { + // Event node + metrics[i] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]); + performance[workerId] += toPerformanceIndex(metrics[i]); + } else { + // Normal, intermediate node + const scalar_t ti = getIntervalStart(time[i]); + const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); + metrics[i] = multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + performance[workerId] += toPerformanceIndex(metrics[i], dt); + } + + i = timeIndex++; + } + + if (i == N) { // Only one worker will execute this + const scalar_t tN = getIntervalStart(time[N]); + metrics[N] = multiple_shooting::computeTerminalMetrics(ocpDefinition, tN, x[N]); + performance[workerId] += toPerformanceIndex(metrics[N]); + } + }; + runParallel(std::move(parallelTask)); + + // Account for initial state in performance + const vector_t initDynamicsViolation = initState - x.front(); + metrics.front().dynamicsViolation += initDynamicsViolation; + performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); + + // Sum performance of the threads + PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); + totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; +} + +slp::StepInfo SlpSolver::takeStep(const PerformanceIndex& baseline, const std::vector& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u, std::vector& metrics) { + using StepType = FilterLinesearch::StepType; + + /* + * Filter linesearch based on: + * "On the implementation of an interior-point filter line-search algorithm for large-scale nonlinear programming" + * https://link.springer.com/article/10.1007/s10107-004-0559-y + */ + if (settings_.printLinesearch) { + std::cerr << std::setprecision(9) << std::fixed; + std::cerr << "\n=== Linesearch ===\n"; + std::cerr << "Baseline:\n" << baseline << "\n"; + } + + // Baseline costs + const scalar_t baselineConstraintViolation = FilterLinesearch::totalConstraintViolation(baseline); + + // Update norm + const auto& dx = subproblemSolution.deltaXSol; + const auto& du = subproblemSolution.deltaUSol; + const auto deltaUnorm = multiple_shooting::trajectoryNorm(du); + const auto deltaXnorm = multiple_shooting::trajectoryNorm(dx); + + scalar_t alpha = 1.0; + vector_array_t xNew(x.size()); + vector_array_t uNew(u.size()); + std::vector metricsNew(metrics.size()); + do { + // Compute step + multiple_shooting::incrementTrajectory(u, du, alpha, uNew); + multiple_shooting::incrementTrajectory(x, dx, alpha, xNew); + + // Compute cost and constraints + const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew, metricsNew); + + // Step acceptance and record step type + bool stepAccepted; + StepType stepType; + std::tie(stepAccepted, stepType) = + filterLinesearch_.acceptStep(baseline, performanceNew, alpha * subproblemSolution.armijoDescentMetric); + + if (settings_.printLinesearch) { + std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepType) + << (stepAccepted ? std::string{" (Accepted)"} : std::string{" (Rejected)"}) << "\n"; + std::cerr << "|dx| = " << alpha * deltaXnorm << "\t|du| = " << alpha * deltaUnorm << "\n"; + std::cerr << performanceNew << "\n"; + } + + if (stepAccepted) { // Return if step accepted + x = std::move(xNew); + u = std::move(uNew); + metrics = std::move(metricsNew); + + // Prepare step info + slp::StepInfo stepInfo; + stepInfo.stepSize = alpha; + stepInfo.stepType = stepType; + stepInfo.dx_norm = alpha * deltaXnorm; + stepInfo.du_norm = alpha * deltaUnorm; + stepInfo.performanceAfterStep = performanceNew; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(performanceNew); + return stepInfo; + + } else { // Try smaller step + alpha *= settings_.alpha_decay; + + // Detect too small step size during back-tracking to escape early. Prevents going all the way to alpha_min + if (alpha * deltaXnorm < settings_.deltaTol && alpha * deltaUnorm < settings_.deltaTol) { + if (settings_.printLinesearch) { + std::cerr << "Exiting linesearch early due to too small primal steps |dx|: " << alpha * deltaXnorm + << ", and or |du|: " << alpha * deltaUnorm << " are below deltaTol: " << settings_.deltaTol << "\n"; + } + break; + } + } + } while (alpha >= settings_.alpha_min); + + // Alpha_min reached -> Don't take a step + slp::StepInfo stepInfo; + stepInfo.stepSize = 0.0; + stepInfo.stepType = StepType::ZERO; + stepInfo.dx_norm = 0.0; + stepInfo.du_norm = 0.0; + stepInfo.performanceAfterStep = baseline; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); + + if (settings_.printLinesearch) { + std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; + } + + return stepInfo; +} + +slp::Convergence SlpSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const { + using Convergence = slp::Convergence; + if ((iteration + 1) >= settings_.slpIteration) { + // Converged because the next iteration would exceed the specified number of iterations + return Convergence::ITERATIONS; + } else if (stepInfo.stepSize < settings_.alpha_min) { + // Converged because step size is below the specified minimum + return Convergence::STEPSIZE; + } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.costTol && + FilterLinesearch::totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { + // Converged because the change in merit is below the specified tolerance while the constraint violation is below the minimum + return Convergence::METRICS; + } else if (stepInfo.dx_norm < settings_.deltaTol && stepInfo.du_norm < settings_.deltaTol) { + // Converged because the change in primal variables is below the specified tolerance + return Convergence::PRIMAL; + } else { + // None of the above convergence criteria were met -> not converged. + return Convergence::FALSE; + } +} + +} // namespace ocs2 diff --git a/ocs2_slp/src/lintTarget.cpp b/ocs2_slp/src/lintTarget.cpp new file mode 100644 index 000000000..80dba9fcb --- /dev/null +++ b/ocs2_slp/src/lintTarget.cpp @@ -0,0 +1,45 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +// dummy target for clang toolchain +int main() { + return 0; +} diff --git a/ocs2_slp/src/pipg/PipgSettings.cpp b/ocs2_slp/src/pipg/PipgSettings.cpp new file mode 100644 index 000000000..123dc372b --- /dev/null +++ b/ocs2_slp/src/pipg/PipgSettings.cpp @@ -0,0 +1,69 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_slp/pipg/PipgSettings.h" + +#include + +#include +#include + +#include + +namespace ocs2 { +namespace pipg { + +Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { + boost::property_tree::ptree pt; + boost::property_tree::read_info(filename, pt); + + Settings settings; + + if (verbose) { + std::cerr << " #### PIPG Settings: {\n"; + } + + loadData::loadPtreeValue(pt, settings.maxNumIterations, fieldName + ".maxNumIterations", verbose); + loadData::loadPtreeValue(pt, settings.absoluteTolerance, fieldName + ".absoluteTolerance", verbose); + loadData::loadPtreeValue(pt, settings.relativeTolerance, fieldName + ".relativeTolerance", verbose); + + loadData::loadPtreeValue(pt, settings.lowerBoundH, fieldName + ".lowerBoundH", verbose); + + loadData::loadPtreeValue(pt, settings.checkTerminationInterval, fieldName + ".checkTerminationInterval", verbose); + loadData::loadPtreeValue(pt, settings.displayShortSummary, fieldName + ".displayShortSummary", verbose); + + if (verbose) { + std::cerr << " #### }\n"; + } + + return settings; +} + +} // namespace pipg +} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_slp/src/pipg/PipgSolver.cpp b/ocs2_slp/src/pipg/PipgSolver.cpp new file mode 100644 index 000000000..f3da51a19 --- /dev/null +++ b/ocs2_slp/src/pipg/PipgSolver.cpp @@ -0,0 +1,328 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_slp/pipg/PipgSolver.h" + +#include +#include +#include +#include + +namespace ocs2 { + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +PipgSolver::PipgSolver(pipg::Settings settings) : settings_(std::move(settings)) { + Eigen::initParallel(); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +pipg::SolverStatus PipgSolver::solve(ThreadPool& threadPool, const vector_t& x0, std::vector& dynamics, + const std::vector& cost, + const std::vector* constraints, + const vector_array_t& scalingVectors, const vector_array_t* EInv, const pipg::PipgBounds& pipgBounds, + vector_array_t& xTrajectory, vector_array_t& uTrajectory) { + verifySizes(dynamics, cost, constraints); + const int N = ocpSize_.numStages; + if (N < 1) { + throw std::runtime_error("[PipgSolver::solve] The number of stages cannot be less than 1."); + } + if (scalingVectors.size() != N) { + throw std::runtime_error("[PipgSolver::solve] The size of scalingVectors doesn't match the number of stage."); + } + + // Disable Eigen's internal multithreading + Eigen::setNbThreads(1); + + vector_array_t primalResidualArray(N); + scalar_array_t constraintsViolationInfNormArray(N); + scalar_t constraintsViolationInfNorm; + + scalar_t solutionSSE, solutionSquaredNorm; + scalar_array_t solutionSEArray(N); + scalar_array_t solutionSquaredNormArray(N); + + // initial state + X_[0] = x0; + XNew_[0] = x0; + // cold start + for (int t = 0; t < N; t++) { + X_[t + 1].setZero(dynamics[t].dfdx.rows()); + U_[t].setZero(dynamics[t].dfdu.cols()); + W_[t].setZero(dynamics[t].dfdx.rows()); + // WNew_ will NOT be filled, but will be swapped to W_ in iteration 0. Thus, initialize WNew_ here. + WNew_[t].setZero(dynamics[t].dfdx.rows()); + } + + scalar_t alpha = pipgBounds.primalStepSize(0); + scalar_t beta = pipgBounds.primalStepSize(0); + scalar_t betaLast = 0; + + size_t k = 0; + std::atomic_int timeIndex{1}, finishedTaskCounter{0}; + std::atomic_bool keepRunning{true}, shouldWait{true}; + bool isConverged = false; + + std::mutex mux; + std::condition_variable iterationFinished; + std::vector threadsWorkloadCounter(threadPool.numThreads() + 1U, 0); + + auto updateVariablesTask = [&](int workerId) { + int t; + int workerOrder; + + while (keepRunning) { + // Reset workerOrder in case all tasks have been assigned and some workers cannot enter the following while loop, keeping the + // workerOrder from previous iterations. + workerOrder = 0; + while ((t = timeIndex++) <= N) { + if (t == N) { + std::lock_guard lk(mux); + shouldWait = true; + } + // Multi-thread performance analysis + ++threadsWorkloadCounter[workerId]; + + // PIPG algorithm + const auto& A = dynamics[t - 1].dfdx; + const auto& B = dynamics[t - 1].dfdu; + const auto& C = scalingVectors[t - 1]; + const auto& b = dynamics[t - 1].f; + + const auto& R = cost[t - 1].dfduu; + const auto& Q = cost[t].dfdxx; + const auto& P = cost[t - 1].dfdux; + const auto& q = cost[t].dfdx; + const auto& r = cost[t - 1].dfdu; + + if (k != 0) { + // Update W of the iteration k - 1. Move the update of W to the front of the calculation of V to prevent data race. + // vector_t primalResidual = C * X_[t] - A * X_[t - 1] - B * U_[t - 1] - b; + primalResidualArray[t - 1] = -b; + primalResidualArray[t - 1].array() += C.array() * X_[t].array(); + primalResidualArray[t - 1].noalias() -= A * X_[t - 1]; + primalResidualArray[t - 1].noalias() -= B * U_[t - 1]; + if (EInv != nullptr) { + constraintsViolationInfNormArray[t - 1] = (*EInv)[t - 1].cwiseProduct(primalResidualArray[t - 1]).lpNorm(); + } else { + constraintsViolationInfNormArray[t - 1] = primalResidualArray[t - 1].lpNorm(); + } + + WNew_[t - 1] = W_[t - 1] + betaLast * primalResidualArray[t - 1]; + + // What stored in UNew and XNew is the solution of iteration k - 2 and what stored in U and X is the solution of iteration k + // - 1. By convention, iteration starts from 0 and the solution of iteration -1 is the initial value. Reuse UNew and XNew + // memory to store the difference between the last solution and the one before last solution. + UNew_[t - 1] -= U_[t - 1]; + XNew_[t] -= X_[t]; + + solutionSEArray[t - 1] = UNew_[t - 1].squaredNorm() + XNew_[t].squaredNorm(); + solutionSquaredNormArray[t - 1] = U_[t - 1].squaredNorm() + X_[t].squaredNorm(); + } + + // V_[t - 1] = W_[t - 1] + (beta + betaLast) * (C * X_[t] - A * X_[t - 1] - B * U_[t - 1] - b); + V_[t - 1] = W_[t - 1] - (beta + betaLast) * b; + V_[t - 1].array() += (beta + betaLast) * C.array() * X_[t].array(); + V_[t - 1].noalias() -= (beta + betaLast) * (A * X_[t - 1]); + V_[t - 1].noalias() -= (beta + betaLast) * (B * U_[t - 1]); + + // UNew_[t - 1] = U_[t - 1] - alpha * (R * U_[t - 1] + P * X_[t - 1] + r - B.transpose() * V_[t - 1]); + UNew_[t - 1] = U_[t - 1] - alpha * r; + UNew_[t - 1].noalias() -= alpha * (R * U_[t - 1]); + UNew_[t - 1].noalias() -= alpha * (P * X_[t - 1]); + UNew_[t - 1].noalias() += alpha * (B.transpose() * V_[t - 1]); + + // XNew_[t] = X_[t] - alpha * (Q * X_[t] + q + C * V_[t - 1]); + XNew_[t] = X_[t] - alpha * q; + XNew_[t].array() -= alpha * C.array() * V_[t - 1].array(); + XNew_[t].noalias() -= alpha * (Q * X_[t]); + + if (t != N) { + const auto& ANext = dynamics[t].dfdx; + const auto& BNext = dynamics[t].dfdu; + const auto& CNext = scalingVectors[t]; + const auto& bNext = dynamics[t].f; + + // dfdux + const auto& PNext = cost[t].dfdux; + + // vector_t VNext = W_[t] + (beta + betaLast) * (CNext * X_[t + 1] - ANext * X_[t] - BNext * U_[t] - bNext); + vector_t VNext = W_[t] - (beta + betaLast) * bNext; + VNext.array() += (beta + betaLast) * CNext.array() * X_[t + 1].array(); + VNext.noalias() -= (beta + betaLast) * (ANext * X_[t]); + VNext.noalias() -= (beta + betaLast) * (BNext * U_[t]); + + XNew_[t].noalias() += alpha * (ANext.transpose() * VNext); + // Add dfdxu * du if it is not the final state. + XNew_[t].noalias() -= alpha * (PNext.transpose() * U_[t]); + } + + workerOrder = ++finishedTaskCounter; + } + + if (workerOrder != N) { + std::unique_lock lk(mux); + iterationFinished.wait(lk, [&shouldWait] { return !shouldWait; }); + lk.unlock(); + } else { + betaLast = beta; + // Adaptive step size + beta = pipgBounds.dualStepSize(k); + alpha = pipgBounds.primalStepSize(k); + + if (k != 0 && k % settings().checkTerminationInterval == 0) { + constraintsViolationInfNorm = + *(std::max_element(constraintsViolationInfNormArray.begin(), constraintsViolationInfNormArray.end())); + + solutionSSE = std::accumulate(solutionSEArray.begin(), solutionSEArray.end(), 0.0); + solutionSquaredNorm = std::accumulate(solutionSquaredNormArray.begin(), solutionSquaredNormArray.end(), 0.0); + + isConverged = constraintsViolationInfNorm <= settings().absoluteTolerance && + (solutionSSE <= settings().relativeTolerance * settings().relativeTolerance * solutionSquaredNorm || + solutionSSE <= settings().absoluteTolerance); + + keepRunning = k < settings().maxNumIterations && !isConverged; + } + + XNew_.swap(X_); + UNew_.swap(U_); + WNew_.swap(W_); + + ++k; + finishedTaskCounter = 0; + timeIndex = 1; + { + std::lock_guard lk(mux); + shouldWait = false; + } + iterationFinished.notify_all(); + } + } + }; + threadPool.runParallel(std::move(updateVariablesTask), threadPool.numThreads() + 1U); + + xTrajectory = X_; + uTrajectory = U_; + const auto status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; + + if (settings().displayShortSummary) { + scalar_t totalTasks = std::accumulate(threadsWorkloadCounter.cbegin(), threadsWorkloadCounter.cend(), 0.0); + std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ PIPG +++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++++++\n"; + std::cerr << "Solver status: " << pipg::toString(status) << "\n"; + std::cerr << "Number of Iterations: " << k << " out of " << settings().maxNumIterations << "\n"; + std::cerr << "Norm of delta primal solution: " << std::sqrt(solutionSSE) << "\n"; + std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; + std::cerr << "Thread workload(ID: # of finished tasks): "; + for (int i = 0; i < threadsWorkloadCounter.size(); i++) { + std::cerr << i << ": " << threadsWorkloadCounter[i] << "(" << static_cast(threadsWorkloadCounter[i]) / totalTasks * 100.0 + << "%) "; + } + } + + Eigen::setNbThreads(0); // Restore default setup. + + return status; +}; + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::resize(const OcpSize& ocpSize) { + if (ocpSize_ == ocpSize) { + return; + } + verifyOcpSize(ocpSize); + + ocpSize_ = ocpSize; + const int N = ocpSize_.numStages; + + numDecisionVariables_ = std::accumulate(std::next(ocpSize_.numStates.begin()), ocpSize_.numStates.end(), 0); + numDecisionVariables_ += std::accumulate(ocpSize_.numInputs.begin(), ocpSize_.numInputs.end(), 0); + numDynamicsConstraints_ = std::accumulate(std::next(ocpSize_.numStates.begin()), ocpSize_.numStates.end(), 0); + + X_.resize(N + 1); + W_.resize(N); + V_.resize(N); + U_.resize(N); + XNew_.resize(N + 1); + UNew_.resize(N); + WNew_.resize(N); +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::verifySizes(const std::vector& dynamics, + const std::vector& cost, + const std::vector* constraints) const { + if (dynamics.size() != ocpSize_.numStages) { + throw std::runtime_error("[PipgSolver::verifySizes] Inconsistent size of dynamics: " + std::to_string(dynamics.size()) + " with " + + std::to_string(ocpSize_.numStages) + " number of stages."); + } + if (cost.size() != ocpSize_.numStages + 1) { + throw std::runtime_error("[PipgSolver::verifySizes] Inconsistent size of cost: " + std::to_string(cost.size()) + " with " + + std::to_string(ocpSize_.numStages + 1) + " nodes."); + } + if (constraints != nullptr) { + if (constraints->size() != ocpSize_.numStages + 1) { + throw std::runtime_error("[PipgSolver::verifySizes] Inconsistent size of constraints: " + std::to_string(constraints->size()) + + " with " + std::to_string(ocpSize_.numStages + 1) + " nodes."); + } + } +} + +/******************************************************************************************************/ +/******************************************************************************************************/ +/******************************************************************************************************/ +void PipgSolver::verifyOcpSize(const OcpSize& ocpSize) const { + auto isNotEmpty = [](const std::vector& v) { return std::any_of(v.cbegin(), v.cend(), [](int s) { return s != 0; }); }; + + if (isNotEmpty(ocpSize.numInputBoxConstraints)) { + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support input box constraints."); + } + if (isNotEmpty(ocpSize.numStateBoxConstraints)) { + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support state box constraints."); + } + if (isNotEmpty(ocpSize.numInputBoxSlack)) { + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support input slack variables."); + } + if (isNotEmpty(ocpSize.numStateBoxSlack)) { + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support state slack variables."); + } + if (isNotEmpty(ocpSize.numIneqSlack)) { + throw std::runtime_error("[PipgSolver::verifyOcpSize] PIPG solver does not support inequality slack variables."); + } +} + +} // namespace ocs2 diff --git a/ocs2_slp/src/pipg/SingleThreadPipg.cpp b/ocs2_slp/src/pipg/SingleThreadPipg.cpp new file mode 100644 index 000000000..c0dfb1c63 --- /dev/null +++ b/ocs2_slp/src/pipg/SingleThreadPipg.cpp @@ -0,0 +1,111 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 "ocs2_slp/pipg/SingleThreadPipg.h" + +#include +#include + +namespace ocs2 { +namespace pipg { + +SolverStatus singleThreadPipg(const pipg::Settings& settings, const Eigen::SparseMatrix& H, const vector_t& h, + const Eigen::SparseMatrix& G, const vector_t& g, const vector_t& EInv, const PipgBounds& pipgBounds, + vector_t& stackedSolution) { + // Cold start + vector_t z = vector_t::Zero(H.cols()); + vector_t z_old = vector_t::Zero(H.cols()); + + vector_t v = vector_t::Zero(g.rows()); + vector_t w = vector_t::Zero(g.rows()); + vector_t constraintsViolation(g.rows()); + + // Iteration number + size_t k = 0; + bool isConverged = false; + scalar_t constraintsViolationInfNorm; + while (k < settings.maxNumIterations && !isConverged) { + const auto beta = pipgBounds.dualStepSize(k); + const auto alpha = pipgBounds.primalStepSize(k); + + z_old.swap(z); + + // v = w + beta * (G * z - g); + v = -g; + v.noalias() += G * z; + v *= beta; + v += w; + + // z = z_old - alpha * (H * z_old + h + G.transpose() * v); + z = -h; + z.noalias() -= (H * z_old); + z.noalias() -= (G.transpose() * v); + z *= alpha; + z.noalias() += z_old; + + // w = w + beta * (G * z - g); + w -= beta * g; + w.noalias() += beta * (G * z); + + if (k % settings.checkTerminationInterval == 0) { + const scalar_t zNorm = z.squaredNorm(); + + constraintsViolation.noalias() = G * z; + constraintsViolation -= g; + constraintsViolation.cwiseProduct(EInv); + constraintsViolationInfNorm = constraintsViolation.lpNorm(); + + const vector_t z_delta = z - z_old; + const scalar_t z_deltaNorm = z_delta.squaredNorm(); + isConverged = + constraintsViolationInfNorm <= settings.absoluteTolerance && + (z_deltaNorm <= settings.relativeTolerance * settings.relativeTolerance * zNorm || z_deltaNorm <= settings.absoluteTolerance); + } + + ++k; + } // end of while loop + + stackedSolution.swap(z); + pipg::SolverStatus status = isConverged ? pipg::SolverStatus::SUCCESS : pipg::SolverStatus::MAX_ITER; + + if (settings.displayShortSummary) { + std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ PIPG +++++++++++++++++++++++++"; + std::cerr << "\n+++++++++++++++++++++++++++++++++++++++++++++\n"; + std::cerr << "Solver status: " << pipg::toString(status) << "\n"; + std::cerr << "Number of Iterations: " << k << " out of " << settings.maxNumIterations << "\n"; + std::cerr << "Norm of delta primal solution: " << (stackedSolution - z_old).norm() << "\n"; + std::cerr << "Constraints violation : " << constraintsViolationInfNorm << "\n"; + } + + return status; +} + +} // namespace pipg +} // namespace ocs2 diff --git a/ocs2_slp/test/testHelpers.cpp b/ocs2_slp/test/testHelpers.cpp new file mode 100644 index 000000000..eaf0fa0fc --- /dev/null +++ b/ocs2_slp/test/testHelpers.cpp @@ -0,0 +1,93 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include +#include + +#include "ocs2_slp/Helpers.h" + +class HelperFunctionTest : public testing::Test { + protected: + // x_0, x_1, ... x_{N - 1}, X_{N} + static constexpr size_t N_ = 10; // numStages + static constexpr size_t nx_ = 4; + static constexpr size_t nu_ = 3; + static constexpr size_t nc_ = 0; + static constexpr size_t numDecisionVariables = N_ * (nx_ + nu_); + static constexpr size_t numConstraints = N_ * (nx_ + nc_); + static constexpr bool verbose = true; + + HelperFunctionTest() : threadPool_(5, 99) { + srand(10); + + // Construct OCP problem + x0 = ocs2::vector_t::Random(nx_); + + for (int i = 0; i < N_; i++) { + dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + } + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + ocpSize_ = ocs2::extractSizesFromProblem(dynamicsArray, costArray, nullptr); + + ocs2::getCostMatrix(ocpSize_, x0, costArray, costApproximation); + } + + ocs2::OcpSize ocpSize_; + ocs2::vector_t x0; + ocs2::ScalarFunctionQuadraticApproximation costApproximation; + + std::vector dynamicsArray; + std::vector costArray; + std::vector constraintsArray; + + ocs2::ThreadPool threadPool_; +}; + +TEST_F(HelperFunctionTest, hessianAbsRowSum) { + ocs2::vector_t rowwiseSum = ocs2::slp::hessianAbsRowSum(ocpSize_, costArray); + EXPECT_TRUE(rowwiseSum.isApprox(costApproximation.dfdxx.cwiseAbs().rowwise().sum())) << "rowSum:\n" << rowwiseSum.transpose(); +} + +TEST_F(HelperFunctionTest, GGTAbsRowSumInParallel) { + ocs2::VectorFunctionLinearApproximation constraintsApproximation; + ocs2::vector_array_t scalingVectors(N_); + for (auto& v : scalingVectors) { + v = ocs2::vector_t::Random(nx_); + } + + ocs2::getConstraintMatrix(ocpSize_, x0, dynamicsArray, nullptr, &scalingVectors, constraintsApproximation); + ocs2::vector_t rowwiseSum = ocs2::slp::GGTAbsRowSumInParallel(threadPool_, ocpSize_, dynamicsArray, nullptr, &scalingVectors); + ocs2::matrix_t GGT = constraintsApproximation.dfdx * constraintsApproximation.dfdx.transpose(); + EXPECT_TRUE(rowwiseSum.isApprox(GGT.cwiseAbs().rowwise().sum())); +} \ No newline at end of file diff --git a/ocs2_slp/test/testPipgSolver.cpp b/ocs2_slp/test/testPipgSolver.cpp new file mode 100644 index 000000000..09d907e77 --- /dev/null +++ b/ocs2_slp/test/testPipgSolver.cpp @@ -0,0 +1,186 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 +#include + +#include +#include +#include + +#include "ocs2_slp/pipg/PipgSolver.h" +#include "ocs2_slp/pipg/SingleThreadPipg.h" + +ocs2::pipg::Settings configurePipg(size_t maxNumIterations, ocs2::scalar_t absoluteTolerance, ocs2::scalar_t relativeTolerance, + bool verbose) { + ocs2::pipg::Settings settings; + settings.maxNumIterations = maxNumIterations; + settings.absoluteTolerance = absoluteTolerance; + settings.relativeTolerance = relativeTolerance; + settings.checkTerminationInterval = 1; + settings.displayShortSummary = verbose; + + return settings; +} + +class PIPGSolverTest : public testing::Test { + protected: + // x_0, x_1, ... x_{N - 1}, X_{N} + static constexpr size_t N_ = 10; // numStages + static constexpr size_t nx_ = 4; + static constexpr size_t nu_ = 3; + static constexpr size_t nc_ = 0; + static constexpr size_t numDecisionVariables_ = N_ * (nx_ + nu_); + static constexpr size_t numConstraints_ = N_ * (nx_ + nc_); + static constexpr size_t numThreads_ = 8; + static constexpr bool verbose_ = true; + + PIPGSolverTest() : solver(configurePipg(30000, 1e-10, 1e-3, verbose_)) { + srand(10); + + // Construct OCP problem + x0 = ocs2::vector_t::Random(nx_); + + for (int i = 0; i < N_; i++) { + dynamicsArray.push_back(ocs2::getRandomDynamics(nx_, nu_)); + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + } + costArray.push_back(ocs2::getRandomCost(nx_, nu_)); + constraintsArray.push_back(ocs2::getRandomConstraints(nx_, nu_, nc_)); + + solver.resize(ocs2::extractSizesFromProblem(dynamicsArray, costArray, &constraintsArray)); + ocs2::getCostMatrix(solver.size(), x0, costArray, costApproximation); + ocs2::getConstraintMatrix(solver.size(), x0, dynamicsArray, nullptr, nullptr, constraintsApproximation); + } + + ocs2::vector_t x0; + ocs2::ScalarFunctionQuadraticApproximation costApproximation; + ocs2::VectorFunctionLinearApproximation constraintsApproximation; + std::vector dynamicsArray; + std::vector costArray; + std::vector constraintsArray; + + ocs2::PipgSolver solver; + ocs2::ThreadPool threadPool{numThreads_ - 1u, 50}; +}; + +constexpr size_t PIPGSolverTest::N_; +constexpr size_t PIPGSolverTest::nx_; +constexpr size_t PIPGSolverTest::nu_; +constexpr size_t PIPGSolverTest::nc_; +constexpr size_t PIPGSolverTest::numDecisionVariables_; +constexpr size_t PIPGSolverTest::numConstraints_; +constexpr size_t PIPGSolverTest::numThreads_; +constexpr bool PIPGSolverTest::verbose_; + +TEST_F(PIPGSolverTest, correctness) { + // ocs2::qp_solver::SolveDenseQP use Gz + g = 0 for constraints + auto QPconstraints = constraintsApproximation; + QPconstraints.f = -QPconstraints.f; + ocs2::vector_t primalSolutionQP; + std::tie(primalSolutionQP, std::ignore) = ocs2::qp_solver::solveDenseQp(costApproximation, QPconstraints); + + Eigen::JacobiSVD svd(costApproximation.dfdxx); + ocs2::vector_t s = svd.singularValues(); + const ocs2::scalar_t lambda = s(0); + const ocs2::scalar_t mu = s(svd.rank() - 1); + Eigen::JacobiSVD svdGTG(constraintsApproximation.dfdx.transpose() * constraintsApproximation.dfdx); + const ocs2::scalar_t sigma = svdGTG.singularValues()(0); + const ocs2::pipg::PipgBounds pipgBounds{mu, lambda, sigma}; + + ocs2::vector_t primalSolutionPIPG; + std::ignore = ocs2::pipg::singleThreadPipg(solver.settings(), costApproximation.dfdxx.sparseView(), costApproximation.dfdx, + constraintsApproximation.dfdx.sparseView(), constraintsApproximation.f, + ocs2::vector_t::Ones(solver.getNumDynamicsConstraints()), pipgBounds, primalSolutionPIPG); + + ocs2::vector_array_t scalingVectors(N_, ocs2::vector_t::Ones(nx_)); + ocs2::vector_array_t X, U; + std::ignore = solver.solve(threadPool, x0, dynamicsArray, costArray, nullptr, scalingVectors, nullptr, pipgBounds, X, U); + + ocs2::vector_t primalSolutionPIPGParallel; + ocs2::toKktSolution(X, U, primalSolutionPIPGParallel); + + auto calculateConstraintViolation = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { + return (constraintsApproximation.dfdx * sol - constraintsApproximation.f).cwiseAbs().maxCoeff(); + }; + auto calculateCost = [&](const ocs2::vector_t& sol) -> ocs2::scalar_t { + return (0.5 * sol.transpose() * costApproximation.dfdxx * sol + costApproximation.dfdx.transpose() * sol)(0); + }; + ocs2::scalar_t QPCost = calculateCost(primalSolutionQP); + ocs2::scalar_t QPConstraintViolation = calculateConstraintViolation(primalSolutionQP); + + ocs2::scalar_t PIPGCost = calculateCost(primalSolutionPIPG); + ocs2::scalar_t PIPGConstraintViolation = calculateConstraintViolation(primalSolutionPIPG); + + ocs2::scalar_t PIPGParallelCost = calculateCost(primalSolutionPIPGParallel); + ocs2::scalar_t PIPGParallelCConstraintViolation = calculateConstraintViolation(primalSolutionPIPGParallel); + + if (verbose_) { + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; + std::cerr << "\n++++++++++++++ [TestPIPG] Correctness ++++++++++++++++"; + std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; + + std::cerr << "mu: " << mu << " lambda: " << lambda << " sigma: " << sigma << "\n"; + + std::cerr << "QP-PIPG: " << (primalSolutionQP - primalSolutionPIPG).cwiseAbs().sum() << "\n"; + std::cerr << "PIPG-PIPGParallel: " << (primalSolutionPIPG - primalSolutionPIPGParallel).cwiseAbs().sum() << "\n"; + + std::cerr << "QP:\n"; + std::cerr << "cost: " << QPCost << " " + << "constraint-violation: " << QPConstraintViolation << "\n\n"; + + std::cerr << "PIPG:\n"; + std::cerr << "cost: " << PIPGCost << " " + << "constraint-violation: " << PIPGConstraintViolation << "\n\n"; + + std::cerr << "PIPGParallel:\n"; + std::cerr << "cost: " << PIPGParallelCost << " " + << "constraint-violation: " << PIPGParallelCConstraintViolation << "\n\n" + << std::endl; + } + EXPECT_TRUE(primalSolutionQP.isApprox(primalSolutionPIPG, solver.settings().absoluteTolerance * 10.0)) + << "Inf-norm of (QP - PIPG): " << (primalSolutionQP - primalSolutionPIPG).cwiseAbs().maxCoeff(); + + EXPECT_TRUE(primalSolutionPIPGParallel.isApprox(primalSolutionPIPG, solver.settings().absoluteTolerance * 10.0)) + << "Inf-norm of (PIPG - PIPGParallel): " << (primalSolutionPIPGParallel - primalSolutionPIPG).cwiseAbs().maxCoeff(); + + // Threshold is the (absoluteTolerance) * (2-Norm of the hessian H)[lambda] + EXPECT_TRUE(std::abs(QPCost - PIPGCost) < solver.settings().absoluteTolerance * lambda) + << "Absolute diff is [" << std::abs(QPCost - PIPGCost) << "] which is larger than [" << solver.settings().absoluteTolerance * lambda + << "]"; + + EXPECT_TRUE(std::abs(PIPGParallelCost - PIPGCost) < solver.settings().absoluteTolerance) + << "Absolute diff is [" << std::abs(PIPGParallelCost - PIPGCost) << "] which is larger than [" + << solver.settings().absoluteTolerance * lambda << "]"; + + ASSERT_TRUE(std::abs(PIPGConstraintViolation) < solver.settings().absoluteTolerance); + EXPECT_TRUE(std::abs(QPConstraintViolation - PIPGConstraintViolation) < solver.settings().absoluteTolerance * 10.0); + EXPECT_TRUE(std::abs(PIPGParallelCConstraintViolation - PIPGConstraintViolation) < solver.settings().absoluteTolerance * 10.0); +} \ No newline at end of file diff --git a/ocs2_slp/test/testSlpSolver.cpp b/ocs2_slp/test/testSlpSolver.cpp new file mode 100644 index 000000000..fd7cd8d9b --- /dev/null +++ b/ocs2_slp/test/testSlpSolver.cpp @@ -0,0 +1,123 @@ +/****************************************************************************** +Copyright (c) 2020, Farbod Farshidian. All rights reserved. + +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. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +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 + +#include +#include +#include + +#include "ocs2_slp/SlpSolver.h" + +namespace ocs2 { +namespace { + +std::pair> solve(const VectorFunctionLinearApproximation& dynamicsMatrices, + const ScalarFunctionQuadraticApproximation& costMatrices, + const ocs2::scalar_t tol) { + int n = dynamicsMatrices.dfdu.rows(); + int m = dynamicsMatrices.dfdu.cols(); + + ocs2::OptimalControlProblem problem; + + // System + problem.dynamicsPtr = getOcs2Dynamics(dynamicsMatrices); + + // Cost + problem.costPtr->add("intermediateCost", ocs2::getOcs2Cost(costMatrices)); + problem.finalCostPtr->add("finalCost", ocs2::getOcs2StateCost(costMatrices)); + + // Reference Manager + ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Ones(n)}, {ocs2::vector_t::Ones(m)}); + std::shared_ptr referenceManagerPtr(new ReferenceManager(targetTrajectories)); + + problem.targetTrajectoriesPtr = &referenceManagerPtr->getTargetTrajectories(); + + problem.equalityConstraintPtr->add("intermediateCost", ocs2::getOcs2Constraints(getRandomConstraints(n, m, 0))); + + ocs2::DefaultInitializer zeroInitializer(m); + + // Solver settings + auto getPipgSettings = [&]() { + ocs2::pipg::Settings settings; + settings.maxNumIterations = 30000; + settings.absoluteTolerance = tol; + settings.relativeTolerance = 1e-2; + settings.lowerBoundH = 1e-3; + settings.checkTerminationInterval = 1; + settings.displayShortSummary = true; + return settings; + }; + + const auto slpSettings = [&]() { + ocs2::slp::Settings settings; + settings.dt = 0.05; + settings.slpIteration = 10; + settings.scalingIteration = 3; + settings.printSolverStatistics = true; + settings.printSolverStatus = true; + settings.printLinesearch = true; + settings.nThreads = 100; + settings.pipgSettings = getPipgSettings(); + return settings; + }(); + + // Additional problem definitions + const ocs2::scalar_t startTime = 0.0; + const ocs2::scalar_t finalTime = 1.0; + const ocs2::vector_t initState = ocs2::vector_t::Ones(n); + + // Construct solver + ocs2::SlpSolver solver(slpSettings, problem, zeroInitializer); + solver.setReferenceManager(referenceManagerPtr); + + // Solve + solver.run(startTime, initState, finalTime); + return {solver.primalSolution(finalTime), solver.getIterationsLog()}; +} + +} // namespace +} // namespace ocs2 + +TEST(testSlpSolver, test_unconstrained) { + int n = 3; + int m = 2; + const double tol = 1e-9; + const auto dynamics = ocs2::getRandomDynamics(n, m); + const auto costs = ocs2::getRandomCost(n, m); + const auto result = ocs2::solve(dynamics, costs, tol); + + /* + * Assert performance + * - Contains 2 performance indices, 1 for the initialization, 1 for the iteration. + * - Linear dynamics should be satisfied after the step. + */ + ASSERT_LE(result.second.size(), 2); + ASSERT_LT(result.second.back().dynamicsViolationSSE, tol); +} diff --git a/ocs2_sqp/hpipm_catkin/CMakeLists.txt b/ocs2_sqp/hpipm_catkin/CMakeLists.txt index ed6443e6e..9cad4c92a 100644 --- a/ocs2_sqp/hpipm_catkin/CMakeLists.txt +++ b/ocs2_sqp/hpipm_catkin/CMakeLists.txt @@ -5,6 +5,7 @@ project(hpipm_catkin) find_package(catkin REQUIRED COMPONENTS blasfeo_catkin ocs2_core + ocs2_oc ocs2_qp_solver ) @@ -79,7 +80,6 @@ include_directories( add_library(${PROJECT_NAME} src/HpipmInterface.cpp src/HpipmInterfaceSettings.cpp - src/OcpSize.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} diff --git a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h index c876217c7..2a62314d5 100644 --- a/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h +++ b/ocs2_sqp/hpipm_catkin/include/hpipm_catkin/HpipmInterface.h @@ -36,9 +36,9 @@ extern "C" { } #include +#include #include "hpipm_catkin/HpipmInterfaceSettings.h" -#include "hpipm_catkin/OcpSize.h" namespace ocs2 { @@ -48,7 +48,6 @@ namespace ocs2 { */ class HpipmInterface { public: - using OcpSize = hpipm_interface::OcpSize; using Settings = hpipm_interface::Settings; /** diff --git a/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp b/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp index 2bdaa2af7..1d3f7a6df 100644 --- a/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp +++ b/ocs2_sqp/hpipm_catkin/test/testHpipmInterface.cpp @@ -50,7 +50,7 @@ TEST(test_hpiphm_interface, solve_and_check_dynamic) { cost.emplace_back(ocs2::getRandomCost(nx, 0)); // Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); ocs2::HpipmInterface hpipmInterface(ocpSize); // Solve! @@ -87,7 +87,7 @@ TEST(test_hpiphm_interface, solve_after_resize) { cost.emplace_back(ocs2::getRandomCost(nx, 0)); // Resize Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); hpipmInterface.resize(ocpSize); // Solve! @@ -137,7 +137,7 @@ TEST(test_hpiphm_interface, knownSolution) { cost[N].dfdx = -cost[N].dfdxx * xSolGiven[N]; // Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); ocs2::HpipmInterface hpipmInterface(ocpSize); // Solve! @@ -174,7 +174,7 @@ TEST(test_hpiphm_interface, with_constraints) { constraints.emplace_back(ocs2::getRandomConstraints(nx, 0, nc)); // Resize Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); std::fill(ocpSize.numIneqConstraints.begin(), ocpSize.numIneqConstraints.end(), nc); // Set one of the constraints to empty @@ -241,7 +241,7 @@ TEST(test_hpiphm_interface, noInputs) { cost.emplace_back(ocs2::getRandomCost(nx, 0)); cost[N].dfdx = -cost[N].dfdxx * xSolGiven[N]; - const auto ocpSize = ocs2::hpipm_interface::extractSizesFromProblem(system, cost, nullptr); + const auto ocpSize = ocs2::extractSizesFromProblem(system, cost, nullptr); hpipmInterface.resize(ocpSize); // Solve! @@ -304,7 +304,7 @@ TEST(test_hpiphm_interface, retrieveRiccati) { } // Interface - ocs2::HpipmInterface::OcpSize ocpSize(N, nx, nu); + ocs2::OcpSize ocpSize(N, nx, nu); ocs2::HpipmInterface hpipmInterface(ocpSize); // Solve! diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt index 8f8e0296a..4811f579b 100644 --- a/ocs2_sqp/ocs2_sqp/CMakeLists.txt +++ b/ocs2_sqp/ocs2_sqp/CMakeLists.txt @@ -3,7 +3,6 @@ project(ocs2_sqp) set(CATKIN_PACKAGE_DEPENDENCIES ocs2_core - ocs2_ddp ocs2_mpc ocs2_oc ocs2_qp_solver @@ -51,14 +50,9 @@ include_directories( # Multiple shooting solver library add_library(${PROJECT_NAME} - src/ConstraintProjection.cpp - src/MultipleShootingInitialization.cpp src/MultipleShootingLogging.cpp - src/MultipleShootingSettings.cpp - src/MultipleShootingSolver.cpp - src/MultipleShootingSolverStatus.cpp - src/MultipleShootingTranscription.cpp - src/TimeDiscretization.cpp + src/SqpSettings.cpp + src/SqpSolver.cpp ) add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} @@ -76,9 +70,8 @@ find_package(cmake_clang_tools QUIET) if(cmake_clang_tools_FOUND) message(STATUS "Run clang tooling for target " ${PROJECT_NAME}) add_clang_tooling( - TARGETS - ${PROJECT_NAME} - SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include + TARGETS ${PROJECT_NAME} + SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include CF_WERROR ) @@ -103,15 +96,13 @@ install(DIRECTORY include/${PROJECT_NAME}/ catkin_add_gtest(test_${PROJECT_NAME} test/testCircularKinematics.cpp - test/testDiscretization.cpp - test/testLogging.cpp - test/testProjection.cpp test/testSwitchedProblem.cpp - test/testTranscription.cpp test/testUnconstrained.cpp test/testValuefunction.cpp ) -add_dependencies(test_${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) +add_dependencies(test_${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME} ${catkin_LIBRARIES} diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingMpc.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h similarity index 73% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingMpc.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h index 863786bec..3ff4d1f2a 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingMpc.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpMpc.h @@ -31,30 +31,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include +#include "ocs2_sqp/SqpSolver.h" namespace ocs2 { -class MultipleShootingMpc final : public MPC_BASE { + +class SqpMpc final : public MPC_BASE { public: /** * Constructor * - * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use multiple shooting - * settings directly. - * @param settings : settings for the multiple shooting solver. + * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use + * multiple shooting SQP settings directly. + * @param settings : settings for the multiple shooting SQP solver. * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ - MultipleShootingMpc(mpc::Settings mpcSettings, multiple_shooting::Settings settings, const OptimalControlProblem& optimalControlProblem, - const Initializer& initializer) + SqpMpc(mpc::Settings mpcSettings, sqp::Settings settings, const OptimalControlProblem& optimalControlProblem, + const Initializer& initializer) : MPC_BASE(std::move(mpcSettings)) { - solverPtr_.reset(new MultipleShootingSolver(std::move(settings), optimalControlProblem, initializer)); + solverPtr_.reset(new SqpSolver(std::move(settings), optimalControlProblem, initializer)); }; - ~MultipleShootingMpc() override = default; + ~SqpMpc() override = default; - MultipleShootingSolver* getSolverPtr() override { return solverPtr_.get(); } - const MultipleShootingSolver* getSolverPtr() const override { return solverPtr_.get(); } + SqpSolver* getSolverPtr() override { return solverPtr_.get(); } + const SqpSolver* getSolverPtr() const override { return solverPtr_.get(); } protected: void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override { @@ -65,6 +66,7 @@ class MultipleShootingMpc final : public MPC_BASE { } private: - std::unique_ptr solverPtr_; + std::unique_ptr solverPtr_; }; + } // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSettings.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h similarity index 95% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSettings.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h index dc8c42d10..a9570edf6 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSettings.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSettings.h @@ -35,7 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include namespace ocs2 { -namespace multiple_shooting { +namespace sqp { struct Settings { // Sqp settings @@ -67,7 +67,9 @@ struct Settings { // Inequality penalty relaxed barrier parameters scalar_t inequalityConstraintMu = 0.0; scalar_t inequalityConstraintDelta = 1e-6; + bool projectStateInputEqualityConstraints = true; // Use a projection method to resolve the state-input constraint Cx+Du+e + bool extractProjectionMultiplier = false; // Extract the Lagrange multiplier of the projected state-input constraint Cx+Du+e // Printing bool printSolverStatus = false; // Print HPIPM status after solving the QP subproblem @@ -85,7 +87,7 @@ struct Settings { }; /** - * Loads the multiple shooting settings from a given file. + * Loads the multiple shooting SQP settings from a given file. * * @param [in] filename: File name which contains the configuration data. * @param [in] fieldName: Field name which contains the configuration data. @@ -94,5 +96,5 @@ struct Settings { */ Settings loadSettings(const std::string& filename, const std::string& fieldName = "multiple_shooting", bool verbose = true); -} // namespace multiple_shooting +} // namespace sqp } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h similarity index 72% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h index 0ef3f507f..ea81861ce 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolver.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h @@ -34,32 +34,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include +#include #include #include +#include #include #include "ocs2_sqp/MultipleShootingLogging.h" -#include "ocs2_sqp/MultipleShootingSettings.h" -#include "ocs2_sqp/MultipleShootingSolverStatus.h" -#include "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_sqp/SqpSettings.h" +#include "ocs2_sqp/SqpSolverStatus.h" namespace ocs2 { -class MultipleShootingSolver : public SolverBase { +class SqpSolver : public SolverBase { public: - using Settings = multiple_shooting::Settings; - /** * Constructor * - * @param settings : settings for the multiple shooting solver. + * @param settings : settings for the multiple shooting SQP solver. * @param [in] optimalControlProblem: The optimal control problem formulation. * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available. */ - MultipleShootingSolver(Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); + SqpSolver(sqp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer); - ~MultipleShootingSolver() override; + ~SqpSolver() override; void reset() override; @@ -67,13 +67,7 @@ class MultipleShootingSolver : public SolverBase { void getPrimalSolution(scalar_t finalTime, PrimalSolution* primalSolutionPtr) const override { *primalSolutionPtr = primalSolution_; } - const DualSolution& getDualSolution() const override { - throw std::runtime_error("[MultipleShootingSolver] getDualSolution() not available yet."); - } - - const ProblemMetrics& getSolutionMetrics() const override { - throw std::runtime_error("[MultipleShootingSolver] getSolutionMetrics() not available yet."); - } + const ProblemMetrics& getSolutionMetrics() const override { return problemMetrics_; } size_t getNumIterations() const override { return totalNumIterations_; } @@ -86,15 +80,15 @@ class MultipleShootingSolver : public SolverBase { ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override; ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) override { - throw std::runtime_error("[MultipleShootingSolver] getHamiltonian() not available yet."); + throw std::runtime_error("[SqpSolver] getHamiltonian() not available yet."); } vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override { - throw std::runtime_error("[MultipleShootingSolver] getStateInputEqualityConstraintLagrangian() not available yet."); + throw std::runtime_error("[SqpSolver] getStateInputEqualityConstraintLagrangian() not available yet."); } MultiplierCollection getIntermediateDualSolution(scalar_t time) const override { - throw std::runtime_error("[MultipleShootingSolver] getIntermediateDualSolution() not available yet."); + throw std::runtime_error("[SqpSolver] getIntermediateDualSolution() not available yet."); } private: @@ -104,7 +98,7 @@ class MultipleShootingSolver : public SolverBase { if (externalControllerPtr == nullptr) { runImpl(initTime, initState, finalTime); } else { - throw std::runtime_error("[MultipleShootingSolver::run] This solver does not support external controller!"); + throw std::runtime_error("[SqpSolver::run] This solver does not support external controller!"); } } @@ -124,17 +118,13 @@ class MultipleShootingSolver : public SolverBase { /** Get profiling information as a string */ std::string getBenchmarkingInformation() const; - /** Initializes for the state-input trajectories */ - void initializeStateInputTrajectories(const vector_t& initState, const std::vector& timeDiscretization, - vector_array_t& stateTrajectory, vector_array_t& inputTrajectory); - /** Creates QP around t, x(t), u(t). Returns performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex setupQuadraticSubproblem(const std::vector& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u); + const vector_array_t& u, std::vector& metrics); /** Computes only the performance metrics at the current {t, x(t), u(t)} */ PerformanceIndex computePerformance(const std::vector& time, const vector_t& initState, const vector_array_t& x, - const vector_array_t& u); + const vector_array_t& u, std::vector& metrics); /** Returns solution of the QP subproblem in delta coordinates: */ struct OcpSubproblemSolution { @@ -147,30 +137,27 @@ class MultipleShootingSolver : public SolverBase { /** Extract the value function based on the last solved QP */ void extractValueFunction(const std::vector& time, const vector_array_t& x); - /** Set up the primal solution based on the optimized state and input trajectories */ - void setPrimalSolution(const std::vector& time, vector_array_t&& x, vector_array_t&& u); - - /** Compute 2-norm of the trajectory: sqrt(sum_i v[i]^2) */ - static scalar_t trajectoryNorm(const vector_array_t& v); - - /** Compute total constraint violation */ - scalar_t totalConstraintViolation(const PerformanceIndex& performance) const; + /** Constructs the primal solution based on the optimized state and input trajectories */ + PrimalSolution toPrimalSolution(const std::vector& time, vector_array_t&& x, vector_array_t&& u); /** Decides on the step to take and overrides given trajectories {x(t), u(t)} <- {x(t) + a*dx(t), u(t) + a*du(t)} */ - multiple_shooting::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector& timeDiscretization, - const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, - vector_array_t& u); + sqp::StepInfo takeStep(const PerformanceIndex& baseline, const std::vector& timeDiscretization, const vector_t& initState, + const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, vector_array_t& u, + std::vector& metrics); /** Determine convergence after a step */ - multiple_shooting::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, - const multiple_shooting::StepInfo& stepInfo) const; + sqp::Convergence checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const; // Problem definition - Settings settings_; + const sqp::Settings settings_; DynamicsDiscretizer discretizer_; DynamicsSensitivityDiscretizer sensitivityDiscretizer_; std::vector ocpDefinitions_; std::unique_ptr initializerPtr_; + FilterLinesearch filterLinesearch_; + + // Solver interface + HpipmInterface hpipmInterface_; // Threading ThreadPool threadPool_; @@ -181,20 +168,24 @@ class MultipleShootingSolver : public SolverBase { // Value function in absolute state coordinates (without the constant value) std::vector valueFunction_; - // Solver interface - HpipmInterface hpipmInterface_; - // LQ approximation - std::vector dynamics_; std::vector cost_; - std::vector constraints_; + std::vector dynamics_; + std::vector stateInputEqConstraints_; + std::vector stateIneqConstraints_; + std::vector stateInputIneqConstraints_; std::vector constraintsProjection_; + // Lagrange multipliers + std::vector projectionMultiplierCoefficients_; + // Iteration performance log std::vector performanceIndeces_; + // The ProblemMetrics associated to primalSolution_ + ProblemMetrics problemMetrics_; + // Benchmarking - size_t numProblems_{0}; size_t totalNumIterations_{0}; multiple_shooting::Logger logger_; benchmark::RepeatedTimer initializationTimer_; diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h similarity index 74% rename from ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h rename to ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h index 026a524dd..99df86b34 100644 --- a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/MultipleShootingSolverStatus.h +++ b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolverStatus.h @@ -29,19 +29,23 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include + #include -#include +#include +#include namespace ocs2 { -namespace multiple_shooting { +namespace sqp { + +/** Different types of convergence */ +enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; /** Struct to contain the result and logging data of the stepsize computation */ struct StepInfo { - enum class StepType { UNKNOWN, CONSTRAINT, DUAL, COST, ZERO }; - // Step size and type scalar_t stepSize = 0.0; - StepType stepType = StepType::UNKNOWN; + FilterLinesearch::StepType stepType = FilterLinesearch::StepType::UNKNOWN; // Step in primal variables scalar_t dx_norm = 0.0; // norm of the state trajectory update @@ -52,12 +56,22 @@ struct StepInfo { scalar_t totalConstraintViolationAfterStep; // constraint metric used in the line search }; -std::string toString(const StepInfo::StepType& stepType); - -/** Different types of convergence */ -enum class Convergence { FALSE, ITERATIONS, STEPSIZE, METRICS, PRIMAL }; - -std::string toString(const Convergence& convergence); - -} // namespace multiple_shooting -} // namespace ocs2 \ No newline at end of file +/** Transforms sqp::Convergence to string */ +inline std::string toString(const Convergence& convergence) { + switch (convergence) { + case Convergence::ITERATIONS: + return "Maximum number of iterations reached"; + case Convergence::STEPSIZE: + return "Step size below minimum"; + case Convergence::METRICS: + return "Cost decrease and constraint satisfaction below tolerance"; + case Convergence::PRIMAL: + return "Primal update below tolerance"; + case Convergence::FALSE: + default: + return "Not Converged"; + } +} + +} // namespace sqp +} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/package.xml b/ocs2_sqp/ocs2_sqp/package.xml index 132839f88..9402df863 100644 --- a/ocs2_sqp/ocs2_sqp/package.xml +++ b/ocs2_sqp/ocs2_sqp/package.xml @@ -10,7 +10,6 @@ catkin ocs2_core - ocs2_ddp ocs2_mpc ocs2_oc ocs2_qp_solver diff --git a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp b/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp deleted file mode 100644 index 338c7cd10..000000000 --- a/ocs2_sqp/ocs2_sqp/src/ConstraintProjection.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -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. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -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 "ocs2_sqp/ConstraintProjection.h" - -namespace ocs2 { - -VectorFunctionLinearApproximation qrConstraintProjection(const VectorFunctionLinearApproximation& constraint) { - // Constraint Projectors are based on the QR decomposition - const auto numConstraints = constraint.dfdu.rows(); - const auto numInputs = constraint.dfdu.cols(); - const Eigen::HouseholderQR QRof_DT(constraint.dfdu.transpose()); - - const auto RT = QRof_DT.matrixQR().topRows(numConstraints).triangularView().transpose(); - const matrix_t RTinvC = RT.solve(constraint.dfdx); // inv(R^T) * C - const matrix_t RTinve = RT.solve(constraint.f); // inv(R^T) * e - - const matrix_t Q = QRof_DT.householderQ(); - const auto Q1 = Q.leftCols(numConstraints); - - VectorFunctionLinearApproximation projectionTerms; - projectionTerms.dfdu = Q.rightCols(numInputs - numConstraints); - projectionTerms.dfdx.noalias() = -Q1 * RTinvC; - projectionTerms.f.noalias() = -Q1 * RTinve; - - return projectionTerms; -} - -VectorFunctionLinearApproximation luConstraintProjection(const VectorFunctionLinearApproximation& constraint) { - // Constraint Projectors are based on the LU decomposition - const Eigen::FullPivLU lu(constraint.dfdu); - - VectorFunctionLinearApproximation projectionTerms; - projectionTerms.dfdu = lu.kernel(); - projectionTerms.dfdx.noalias() = -lu.solve(constraint.dfdx); - projectionTerms.f.noalias() = -lu.solve(constraint.f); - - return projectionTerms; -} - -} // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp b/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp deleted file mode 100644 index 9b999f07f..000000000 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingTranscription.cpp +++ /dev/null @@ -1,186 +0,0 @@ -/****************************************************************************** -Copyright (c) 2020, Farbod Farshidian. All rights reserved. - -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. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -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 "ocs2_sqp/MultipleShootingTranscription.h" - -#include -#include - -#include "ocs2_sqp/ConstraintProjection.h" - -namespace ocs2 { -namespace multiple_shooting { - -Transcription setupIntermediateNode(const OptimalControlProblem& optimalControlProblem, - DynamicsSensitivityDiscretizer& sensitivityDiscretizer, bool projectStateInputEqualityConstraints, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { - // Results and short-hand notation - Transcription transcription; - auto& dynamics = transcription.dynamics; - auto& performance = transcription.performance; - auto& cost = transcription.cost; - auto& constraints = transcription.constraints; - auto& projection = transcription.constraintsProjection; - - // Dynamics - // Discretization returns x_{k+1} = A_{k} * dx_{k} + B_{k} * du_{k} + b_{k} - dynamics = sensitivityDiscretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); - dynamics.f -= x_next; // make it dx_{k+1} = ... - performance.dynamicsViolationSSE = dt * dynamics.f.squaredNorm(); - - // Precomputation for other terms - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint + Request::Approximation; - optimalControlProblem.preComputationPtr->request(request, t, x, u); - - // Costs: Approximate the integral with forward euler - cost = approximateCost(optimalControlProblem, t, x, u); - cost *= dt; - performance.cost = cost.f; - - // Constraints - if (!optimalControlProblem.equalityConstraintPtr->empty()) { - // C_{k} * dx_{k} + D_{k} * du_{k} + e_{k} = 0 - constraints = optimalControlProblem.equalityConstraintPtr->getLinearApproximation(t, x, u, *optimalControlProblem.preComputationPtr); - if (constraints.f.size() > 0) { - performance.equalityConstraintsSSE = dt * constraints.f.squaredNorm(); - if (projectStateInputEqualityConstraints) { // Handle equality constraints using projection. - // Projection stored instead of constraint, // TODO: benchmark between lu and qr method. LU seems slightly faster. - projection = luConstraintProjection(constraints); - constraints = VectorFunctionLinearApproximation(); - - // Adapt dynamics and cost - changeOfInputVariables(dynamics, projection.dfdu, projection.dfdx, projection.f); - changeOfInputVariables(cost, projection.dfdu, projection.dfdx, projection.f); - } - } - } - - return transcription; -} - -PerformanceIndex computeIntermediatePerformance(const OptimalControlProblem& optimalControlProblem, DynamicsDiscretizer& discretizer, - scalar_t t, scalar_t dt, const vector_t& x, const vector_t& x_next, const vector_t& u) { - PerformanceIndex performance; - - // Dynamics - vector_t dynamicsGap = discretizer(*optimalControlProblem.dynamicsPtr, t, x, u, dt); - dynamicsGap -= x_next; - performance.dynamicsViolationSSE = dt * dynamicsGap.squaredNorm(); - - // Precomputation for other terms - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Constraint; - optimalControlProblem.preComputationPtr->request(request, t, x, u); - - // Costs - performance.cost = dt * computeCost(optimalControlProblem, t, x, u); - - // Constraints - if (!optimalControlProblem.equalityConstraintPtr->empty()) { - const vector_t constraints = optimalControlProblem.equalityConstraintPtr->getValue(t, x, u, *optimalControlProblem.preComputationPtr); - if (constraints.size() > 0) { - performance.equalityConstraintsSSE = dt * constraints.squaredNorm(); - } - } - - return performance; -} - -TerminalTranscription setupTerminalNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { - // Results and short-hand notation - TerminalTranscription transcription; - auto& performance = transcription.performance; - auto& cost = transcription.cost; - auto& constraints = transcription.constraints; - - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Approximation; - optimalControlProblem.preComputationPtr->requestFinal(request, t, x); - - cost = approximateFinalCost(optimalControlProblem, t, x); - performance.cost = cost.f; - - constraints = VectorFunctionLinearApproximation::Zero(0, x.size()); - - return transcription; -} - -PerformanceIndex computeTerminalPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x) { - PerformanceIndex performance; - - constexpr auto request = Request::Cost + Request::SoftConstraint; - optimalControlProblem.preComputationPtr->requestFinal(request, t, x); - - performance.cost = computeFinalCost(optimalControlProblem, t, x); - - return performance; -} - -EventTranscription setupEventNode(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next) { - // Results and short-hand notation - EventTranscription transcription; - auto& performance = transcription.performance; - auto& dynamics = transcription.dynamics; - auto& cost = transcription.cost; - auto& constraints = transcription.constraints; - - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics + Request::Approximation; - optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); - - // Dynamics - // jump map returns // x_{k+1} = A_{k} * dx_{k} + b_{k} - dynamics = optimalControlProblem.dynamicsPtr->jumpMapLinearApproximation(t, x); - dynamics.f -= x_next; // make it dx_{k+1} = ... - dynamics.dfdu.setZero(x.size(), 0); // Overwrite derivative that shouldn't exist. - performance.dynamicsViolationSSE = dynamics.f.squaredNorm(); - - cost = approximateEventCost(optimalControlProblem, t, x); - performance.cost = cost.f; - - constraints = VectorFunctionLinearApproximation::Zero(0, x.size()); - return transcription; -} - -PerformanceIndex computeEventPerformance(const OptimalControlProblem& optimalControlProblem, scalar_t t, const vector_t& x, - const vector_t& x_next) { - PerformanceIndex performance; - - constexpr auto request = Request::Cost + Request::SoftConstraint + Request::Dynamics; - optimalControlProblem.preComputationPtr->requestPreJump(request, t, x); - - // Dynamics - const vector_t dynamicsGap = optimalControlProblem.dynamicsPtr->computeJumpMap(t, x) - x_next; - performance.dynamicsViolationSSE = dynamicsGap.squaredNorm(); - - performance.cost = computeEventCost(optimalControlProblem, t, x); - - return performance; -} - -} // namespace multiple_shooting -} // namespace ocs2 diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSettings.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp similarity index 94% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingSettings.cpp rename to ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp index be0b91a45..e19de6ad9 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSettings.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSettings.cpp @@ -27,7 +27,7 @@ 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 "ocs2_sqp/MultipleShootingSettings.h" +#include "ocs2_sqp/SqpSettings.h" #include #include @@ -35,7 +35,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include namespace ocs2 { -namespace multiple_shooting { +namespace sqp { Settings loadSettings(const std::string& filename, const std::string& fieldName, bool verbose) { boost::property_tree::ptree pt; @@ -44,7 +44,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, Settings settings; if (verbose) { - std::cerr << "\n #### Multiple Shooting Settings:"; + std::cerr << "\n #### Multiple-Shooting SQP Settings:"; std::cerr << "\n #### =============================================================================\n"; } @@ -66,6 +66,7 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, loadData::loadPtreeValue(pt, settings.inequalityConstraintMu, fieldName + ".inequalityConstraintMu", verbose); loadData::loadPtreeValue(pt, settings.inequalityConstraintDelta, fieldName + ".inequalityConstraintDelta", verbose); loadData::loadPtreeValue(pt, settings.projectStateInputEqualityConstraints, fieldName + ".projectStateInputEqualityConstraints", verbose); + loadData::loadPtreeValue(pt, settings.extractProjectionMultiplier, fieldName + ".extractProjectionMultiplier", verbose); loadData::loadPtreeValue(pt, settings.printSolverStatus, fieldName + ".printSolverStatus", verbose); loadData::loadPtreeValue(pt, settings.printSolverStatistics, fieldName + ".printSolverStatistics", verbose); loadData::loadPtreeValue(pt, settings.printLinesearch, fieldName + ".printLinesearch", verbose); @@ -82,5 +83,5 @@ Settings loadSettings(const std::string& filename, const std::string& fieldName, return settings; } -} // namespace multiple_shooting +} // namespace sqp } // namespace ocs2 \ No newline at end of file diff --git a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp similarity index 55% rename from ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp rename to ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp index a389a4c73..e5d35db2a 100644 --- a/ocs2_sqp/ocs2_sqp/src/MultipleShootingSolver.cpp +++ b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp @@ -27,51 +27,60 @@ 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 "ocs2_sqp/MultipleShootingSolver.h" +#include "ocs2_sqp/SqpSolver.h" -#include +#include #include #include -#include - -#include -#include -#include - -#include "ocs2_sqp/MultipleShootingInitialization.h" -#include "ocs2_sqp/MultipleShootingTranscription.h" +#include +#include +#include +#include +#include +#include +#include namespace ocs2 { -MultipleShootingSolver::MultipleShootingSolver(Settings settings, const OptimalControlProblem& optimalControlProblem, - const Initializer& initializer) - : SolverBase(), - settings_(std::move(settings)), - hpipmInterface_(hpipm_interface::OcpSize(), settings.hpipmSettings), +namespace { +sqp::Settings rectifySettings(const OptimalControlProblem& ocp, sqp::Settings&& settings) { + // True does not make sense if there are no constraints. + if (ocp.equalityConstraintPtr->empty()) { + settings.projectStateInputEqualityConstraints = false; + } + return settings; +} +} // anonymous namespace + +SqpSolver::SqpSolver(sqp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer) + : settings_(rectifySettings(optimalControlProblem, std::move(settings))), + hpipmInterface_(OcpSize(), settings_.hpipmSettings), threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority), logger_(settings_.logSize) { Eigen::setNbThreads(1); // No multithreading within Eigen. Eigen::initParallel(); // Dynamics discretization - discretizer_ = selectDynamicsDiscretization(settings.integratorType); - sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings.integratorType); + discretizer_ = selectDynamicsDiscretization(settings_.integratorType); + sensitivityDiscretizer_ = selectDynamicsSensitivityDiscretization(settings_.integratorType); // Clone objects to have one for each worker - for (int w = 0; w < settings.nThreads; w++) { + for (int w = 0; w < settings_.nThreads; w++) { ocpDefinitions_.push_back(optimalControlProblem); } // Operating points initializerPtr_.reset(initializer.clone()); - if (optimalControlProblem.equalityConstraintPtr->empty()) { - settings_.projectStateInputEqualityConstraints = false; // True does not make sense if there are no constraints. - } + // Linesearch + filterLinesearch_.g_max = settings_.g_max; + filterLinesearch_.g_min = settings_.g_min; + filterLinesearch_.gamma_c = settings_.gamma_c; + filterLinesearch_.armijoFactor = settings_.armijoFactor; } -MultipleShootingSolver::~MultipleShootingSolver() { +SqpSolver::~SqpSolver() { if (settings_.printSolverStatistics) { std::cerr << getBenchmarkingInformation() << std::endl; } @@ -98,14 +107,13 @@ MultipleShootingSolver::~MultipleShootingSolver() { } } -void MultipleShootingSolver::reset() { +void SqpSolver::reset() { // Clear solution primalSolution_ = PrimalSolution(); valueFunction_.clear(); performanceIndeces_.clear(); // reset timers - numProblems_ = 0; totalNumIterations_ = 0; logger_ = multiple_shooting::Logger(settings_.logSize); linearQuadraticApproximationTimer_.reset(); @@ -114,7 +122,7 @@ void MultipleShootingSolver::reset() { computeControllerTimer_.reset(); } -std::string MultipleShootingSolver::getBenchmarkingInformation() const { +std::string SqpSolver::getBenchmarkingInformation() const { const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds(); const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds(); const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds(); @@ -140,17 +148,17 @@ std::string MultipleShootingSolver::getBenchmarkingInformation() const { return infoStream.str(); } -const std::vector& MultipleShootingSolver::getIterationsLog() const { +const std::vector& SqpSolver::getIterationsLog() const { if (performanceIndeces_.empty()) { - throw std::runtime_error("[MultipleShootingSolver]: No performance log yet, no problem solved yet?"); + throw std::runtime_error("[SqpSolver]: No performance log yet, no problem solved yet?"); } else { return performanceIndeces_; } } -ScalarFunctionQuadraticApproximation MultipleShootingSolver::getValueFunction(scalar_t time, const vector_t& state) const { +ScalarFunctionQuadraticApproximation SqpSolver::getValueFunction(scalar_t time, const vector_t& state) const { if (valueFunction_.empty()) { - throw std::runtime_error("[MultipleShootingSolver] Value function is empty! Is createValueFunction true and did the solver run?"); + throw std::runtime_error("[SqpSolver] Value function is empty! Is createValueFunction true and did the solver run?"); } else { // Interpolation const auto indexAlpha = LinearInterpolation::timeSegment(time, primalSolution_.timeTrajectory_); @@ -169,7 +177,7 @@ ScalarFunctionQuadraticApproximation MultipleShootingSolver::getValueFunction(sc } } -void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { +void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; std::cerr << "\n+++++++++++++ SQP solver is initialized ++++++++++++++"; @@ -180,28 +188,34 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat const auto& eventTimes = this->getReferenceManager().getModeSchedule().eventTimes; const auto timeDiscretization = timeDiscretizationWithEvents(initTime, finalTime, settings_.dt, eventTimes); - // Initialize the state and input - vector_array_t x, u; - initializeStateInputTrajectories(initState, timeDiscretization, x, u); - // Initialize references for (auto& ocpDefinition : ocpDefinitions_) { const auto& targetTrajectories = this->getReferenceManager().getTargetTrajectories(); ocpDefinition.targetTrajectoriesPtr = &targetTrajectories; } + // Trajectory spread of primalSolution_ + if (!primalSolution_.timeTrajectory_.empty()) { + std::ignore = trajectorySpread(primalSolution_.modeSchedule_, this->getReferenceManager().getModeSchedule(), primalSolution_); + } + + // Initialize the state and input + vector_array_t x, u; + multiple_shooting::initializeStateInputTrajectories(initState, timeDiscretization, primalSolution_, *initializerPtr_, x, u); + // Bookkeeping performanceIndeces_.clear(); + std::vector metrics; int iter = 0; - multiple_shooting::Convergence convergence = multiple_shooting::Convergence::FALSE; - while (convergence == multiple_shooting::Convergence::FALSE) { + sqp::Convergence convergence = sqp::Convergence::FALSE; + while (convergence == sqp::Convergence::FALSE) { if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\nSQP iteration: " << iter << "\n"; } // Make QP approximation linearQuadraticApproximationTimer_.startTimer(); - const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u); + const auto baselinePerformance = setupQuadraticSubproblem(timeDiscretization, initState, x, u, metrics); linearQuadraticApproximationTimer_.endTimer(); // Solve QP @@ -213,7 +227,7 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat // Apply step linesearchTimer_.startTimer(); - const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u); + const auto stepInfo = takeStep(baselinePerformance, timeDiscretization, initState, deltaSolution, x, u, metrics); performanceIndeces_.push_back(stepInfo.performanceAfterStep); linesearchTimer_.endTimer(); @@ -242,11 +256,10 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat } computeControllerTimer_.startTimer(); - setPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u)); + problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics)); computeControllerTimer_.endTimer(); - ++numProblems_; - if (settings_.printSolverStatus || settings_.printLinesearch) { std::cerr << "\nConvergence : " << toString(convergence) << "\n"; std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++"; @@ -255,59 +268,11 @@ void MultipleShootingSolver::runImpl(scalar_t initTime, const vector_t& initStat } } -void MultipleShootingSolver::runParallel(std::function taskFunction) { +void SqpSolver::runParallel(std::function taskFunction) { threadPool_.runParallel(std::move(taskFunction), settings_.nThreads); } -void MultipleShootingSolver::initializeStateInputTrajectories(const vector_t& initState, - const std::vector& timeDiscretization, - vector_array_t& stateTrajectory, vector_array_t& inputTrajectory) { - const int N = static_cast(timeDiscretization.size()) - 1; // // size of the input trajectory - stateTrajectory.clear(); - stateTrajectory.reserve(N + 1); - inputTrajectory.clear(); - inputTrajectory.reserve(N); - - // Determine till when to use the previous solution - scalar_t interpolateStateTill = timeDiscretization.front().time; - scalar_t interpolateInputTill = timeDiscretization.front().time; - if (primalSolution_.timeTrajectory_.size() >= 2) { - interpolateStateTill = primalSolution_.timeTrajectory_.back(); - interpolateInputTill = primalSolution_.timeTrajectory_[primalSolution_.timeTrajectory_.size() - 2]; - } - - // Initial state - const scalar_t initTime = getIntervalStart(timeDiscretization[0]); - if (initTime < interpolateStateTill) { - stateTrajectory.push_back( - LinearInterpolation::interpolate(initTime, primalSolution_.timeTrajectory_, primalSolution_.stateTrajectory_)); - } else { - stateTrajectory.push_back(initState); - } - - for (int i = 0; i < N; i++) { - if (timeDiscretization[i].event == AnnotatedTime::Event::PreEvent) { - // Event Node - inputTrajectory.push_back(vector_t()); // no input at event node - stateTrajectory.push_back(multiple_shooting::initializeEventNode(timeDiscretization[i].time, stateTrajectory.back())); - } else { - // Intermediate node - const scalar_t time = getIntervalStart(timeDiscretization[i]); - const scalar_t nextTime = getIntervalEnd(timeDiscretization[i + 1]); - vector_t input, nextState; - if (time > interpolateInputTill || nextTime > interpolateStateTill) { // Using initializer - std::tie(input, nextState) = - multiple_shooting::initializeIntermediateNode(*initializerPtr_, time, nextTime, stateTrajectory.back()); - } else { // interpolate previous solution - std::tie(input, nextState) = multiple_shooting::initializeIntermediateNode(primalSolution_, time, nextTime, stateTrajectory.back()); - } - inputTrajectory.push_back(std::move(input)); - stateTrajectory.push_back(std::move(nextState)); - } - } -} - -MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolution(const vector_t& delta_x0) { +SqpSolver::OcpSubproblemSolution SqpSolver::getOCPSolution(const vector_t& delta_x0) { // Solve the QP OcpSubproblemSolution solution; auto& deltaXSol = solution.deltaXSol; @@ -315,44 +280,30 @@ MultipleShootingSolver::OcpSubproblemSolution MultipleShootingSolver::getOCPSolu hpipm_status status; const bool hasStateInputConstraints = !ocpDefinitions_.front().equalityConstraintPtr->empty(); if (hasStateInputConstraints && !settings_.projectStateInputEqualityConstraints) { - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, &constraints_)); - status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, &constraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); + hpipmInterface_.resize(extractSizesFromProblem(dynamics_, cost_, &stateInputEqConstraints_)); + status = + hpipmInterface_.solve(delta_x0, dynamics_, cost_, &stateInputEqConstraints_, deltaXSol, deltaUSol, settings_.printSolverStatus); } else { // without constraints, or when using projection, we have an unconstrained QP. - hpipmInterface_.resize(hpipm_interface::extractSizesFromProblem(dynamics_, cost_, nullptr)); + hpipmInterface_.resize(extractSizesFromProblem(dynamics_, cost_, nullptr)); status = hpipmInterface_.solve(delta_x0, dynamics_, cost_, nullptr, deltaXSol, deltaUSol, settings_.printSolverStatus); } if (status != hpipm_status::SUCCESS) { - throw std::runtime_error("[MultipleShootingSolver] Failed to solve QP"); + throw std::runtime_error("[SqpSolver] Failed to solve QP"); } - // To determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] - solution.armijoDescentMetric = 0.0; - for (int i = 0; i < cost_.size(); i++) { - if (cost_[i].dfdx.size() > 0) { - solution.armijoDescentMetric += cost_[i].dfdx.dot(deltaXSol[i]); - } - if (cost_[i].dfdu.size() > 0) { - solution.armijoDescentMetric += cost_[i].dfdu.dot(deltaUSol[i]); - } - } + // to determine if the solution is a descent direction for the cost: compute gradient(cost)' * [dx; du] + solution.armijoDescentMetric = armijoDescentMetric(cost_, deltaXSol, deltaUSol); // remap the tilde delta u to real delta u if (settings_.projectStateInputEqualityConstraints) { - vector_t tmp; // 1 temporary for re-use. - for (int i = 0; i < deltaUSol.size(); i++) { - if (constraintsProjection_[i].f.size() > 0) { - tmp.noalias() = constraintsProjection_[i].dfdu * deltaUSol[i]; - deltaUSol[i] = tmp + constraintsProjection_[i].f; - deltaUSol[i].noalias() += constraintsProjection_[i].dfdx * deltaXSol[i]; - } - } + multiple_shooting::remapProjectedInput(constraintsProjection_, deltaXSol, deltaUSol); } return solution; } -void MultipleShootingSolver::extractValueFunction(const std::vector& time, const vector_array_t& x) { +void SqpSolver::extractValueFunction(const std::vector& time, const vector_array_t& x) { if (settings_.createValueFunction) { valueFunction_ = hpipmInterface_.getRiccatiCostToGo(dynamics_[0], cost_[0]); // Correct for linearization state @@ -362,107 +313,73 @@ void MultipleShootingSolver::extractValueFunction(const std::vector& time, vector_array_t&& x, vector_array_t&& u) { - // Clear old solution - primalSolution_.clear(); - - // Correct for missing inputs at PreEvents - for (int i = 0; i < time.size(); ++i) { - if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { - u[i] = u[i - 1]; - } - } - - // Compute feedback, before x and u are moved to primal solution - vector_array_t uff; - matrix_array_t controllerGain; +PrimalSolution SqpSolver::toPrimalSolution(const std::vector& time, vector_array_t&& x, vector_array_t&& u) { if (settings_.useFeedbackPolicy) { - // see doc/LQR_full.pdf for detailed derivation for feedback terms - uff = u; // Copy and adapt in loop - controllerGain.reserve(time.size()); + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); matrix_array_t KMatrices = hpipmInterface_.getRiccatiFeedback(dynamics_[0], cost_[0]); - for (int i = 0; (i + 1) < time.size(); i++) { - if (time[i].event == AnnotatedTime::Event::PreEvent && i > 0) { - uff[i] = uff[i - 1]; - controllerGain.push_back(controllerGain.back()); - } else { - // Linear controller has convention u = uff + K * x; - // We computed u = u'(t) + K (x - x'(t)); - // >> uff = u'(t) - K x'(t) - if (constraintsProjection_[i].f.size() > 0) { - controllerGain.push_back(std::move(constraintsProjection_[i].dfdx)); // Steal! Don't use after this. - controllerGain.back().noalias() += constraintsProjection_[i].dfdu * KMatrices[i]; - } else { - controllerGain.push_back(std::move(KMatrices[i])); - } - uff[i].noalias() -= controllerGain.back() * x[i]; - } + if (settings_.projectStateInputEqualityConstraints) { + multiple_shooting::remapProjectedGain(constraintsProjection_, KMatrices); } - // Copy last one to get correct length - uff.push_back(uff.back()); - controllerGain.push_back(controllerGain.back()); - } + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u), std::move(KMatrices)); - // Construct nominal time, state and input trajectories - primalSolution_.stateTrajectory_ = std::move(x); - u.push_back(u.back()); // Repeat last input to make equal length vectors - primalSolution_.inputTrajectory_ = std::move(u); - primalSolution_.timeTrajectory_.reserve(time.size()); - for (size_t i = 0; i < time.size(); i++) { - primalSolution_.timeTrajectory_.push_back(time[i].time); - if (time[i].event == AnnotatedTime::Event::PreEvent) { - primalSolution_.postEventIndices_.push_back(i + 1); - } - } - primalSolution_.modeSchedule_ = this->getReferenceManager().getModeSchedule(); - - // Assign controller - if (settings_.useFeedbackPolicy) { - primalSolution_.controllerPtr_.reset(new LinearController(primalSolution_.timeTrajectory_, std::move(uff), std::move(controllerGain))); } else { - primalSolution_.controllerPtr_.reset(new FeedforwardController(primalSolution_.timeTrajectory_, primalSolution_.inputTrajectory_)); + ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule(); + return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u)); } } -PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vector& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { +PerformanceIndex SqpSolver::setupQuadraticSubproblem(const std::vector& time, const vector_t& initState, + const vector_array_t& x, const vector_array_t& u, std::vector& metrics) { // Problem horizon const int N = static_cast(time.size()) - 1; std::vector performance(settings_.nThreads, PerformanceIndex()); - dynamics_.resize(N); cost_.resize(N + 1); - constraints_.resize(N + 1); + dynamics_.resize(N); + stateInputEqConstraints_.resize(N + 1); // +1 because of HpipmInterface size check + stateIneqConstraints_.resize(N + 1); + stateInputIneqConstraints_.resize(N); constraintsProjection_.resize(N); + projectionMultiplierCoefficients_.resize(N); + metrics.resize(N + 1); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; PerformanceIndex workerPerformance; // Accumulate performance in local variable - const bool projection = settings_.projectStateInputEqualityConstraints; int i = timeIndex++; while (i < N) { if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node auto result = multiple_shooting::setupEventNode(ocpDefinition, time[i].time, x[i], x[i + 1]); - workerPerformance += result.performance; - dynamics_[i] = std::move(result.dynamics); + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result); cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); - constraintsProjection_[i] = VectorFunctionLinearApproximation::Zero(0, x[i].size(), 0); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i].resize(0, x[i].size()); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); + stateInputIneqConstraints_[i].resize(0, x[i].size()); + constraintsProjection_[i].resize(0, x[i].size()); + projectionMultiplierCoefficients_[i] = multiple_shooting::ProjectionMultiplierCoefficients(); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - auto result = - multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, projection, ti, dt, x[i], x[i + 1], u[i]); - workerPerformance += result.performance; - dynamics_[i] = std::move(result.dynamics); + auto result = multiple_shooting::setupIntermediateNode(ocpDefinition, sensitivityDiscretizer_, ti, dt, x[i], x[i + 1], u[i]); + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result, dt); + if (settings_.projectStateInputEqualityConstraints) { + multiple_shooting::projectTranscription(result, settings_.extractProjectionMultiplier); + } cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); + dynamics_[i] = std::move(result.dynamics); + stateInputEqConstraints_[i] = std::move(result.stateInputEqConstraints); + stateIneqConstraints_[i] = std::move(result.stateIneqConstraints); + stateInputIneqConstraints_[i] = std::move(result.stateInputIneqConstraints); constraintsProjection_[i] = std::move(result.constraintsProjection); + projectionMultiplierCoefficients_[i] = std::move(result.projectionMultiplierCoefficients); } i = timeIndex++; @@ -471,9 +388,11 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec if (i == N) { // Only one worker will execute this const scalar_t tN = getIntervalStart(time[N]); auto result = multiple_shooting::setupTerminalNode(ocpDefinition, tN, x[N]); - workerPerformance += result.performance; + metrics[i] = multiple_shooting::computeMetrics(result); + workerPerformance += multiple_shooting::computePerformanceIndex(result); cost_[i] = std::move(result.cost); - constraints_[i] = std::move(result.constraints); + stateInputEqConstraints_[i].resize(0, x[i].size()); + stateIneqConstraints_[i] = std::move(result.ineqConstraints); } // Accumulate! Same worker might run multiple tasks @@ -481,37 +400,42 @@ PerformanceIndex MultipleShootingSolver::setupQuadraticSubproblem(const std::vec }; runParallel(std::move(parallelTask)); - // Account for init state in performance - performance.front().dynamicsViolationSSE += (initState - x.front()).squaredNorm(); + // Account for initial state in performance + const vector_t initDynamicsViolation = initState - x.front(); + metrics.front().dynamicsViolation += initDynamicsViolation; + performance.front().dynamicsViolationSSE += initDynamicsViolation.squaredNorm(); // Sum performance of the threads PerformanceIndex totalPerformance = std::accumulate(std::next(performance.begin()), performance.end(), performance.front()); totalPerformance.merit = totalPerformance.cost + totalPerformance.equalityLagrangian + totalPerformance.inequalityLagrangian; + return totalPerformance; } -PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector& time, const vector_t& initState, - const vector_array_t& x, const vector_array_t& u) { - // Problem horizon +PerformanceIndex SqpSolver::computePerformance(const std::vector& time, const vector_t& initState, const vector_array_t& x, + const vector_array_t& u, std::vector& metrics) { + // Problem size const int N = static_cast(time.size()) - 1; + metrics.resize(N + 1); std::vector performance(settings_.nThreads, PerformanceIndex()); std::atomic_int timeIndex{0}; auto parallelTask = [&](int workerId) { // Get worker specific resources OptimalControlProblem& ocpDefinition = ocpDefinitions_[workerId]; - PerformanceIndex workerPerformance; // Accumulate performance in local variable int i = timeIndex++; while (i < N) { if (time[i].event == AnnotatedTime::Event::PreEvent) { // Event node - workerPerformance += multiple_shooting::computeEventPerformance(ocpDefinition, time[i].time, x[i], x[i + 1]); + metrics[i] = multiple_shooting::computeEventMetrics(ocpDefinition, time[i].time, x[i], x[i + 1]); + performance[workerId] += toPerformanceIndex(metrics[i]); } else { // Normal, intermediate node const scalar_t ti = getIntervalStart(time[i]); const scalar_t dt = getIntervalDuration(time[i], time[i + 1]); - workerPerformance += multiple_shooting::computeIntermediatePerformance(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + metrics[i] = multiple_shooting::computeIntermediateMetrics(ocpDefinition, discretizer_, ti, dt, x[i], x[i + 1], u[i]); + performance[workerId] += toPerformanceIndex(metrics[i], dt); } i = timeIndex++; @@ -519,16 +443,16 @@ PerformanceIndex MultipleShootingSolver::computePerformance(const std::vector& timeDiscretization, - const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, - vector_array_t& x, vector_array_t& u) { - using StepType = multiple_shooting::StepInfo::StepType; +sqp::StepInfo SqpSolver::takeStep(const PerformanceIndex& baseline, const std::vector& timeDiscretization, + const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x, + vector_array_t& u, std::vector& metrics) { + using StepType = FilterLinesearch::StepType; /* * Filter linesearch based on: @@ -566,56 +477,34 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn } // Baseline costs - const scalar_t baselineConstraintViolation = totalConstraintViolation(baseline); + const scalar_t baselineConstraintViolation = FilterLinesearch::totalConstraintViolation(baseline); // Update norm const auto& dx = subproblemSolution.deltaXSol; const auto& du = subproblemSolution.deltaUSol; - const scalar_t deltaUnorm = trajectoryNorm(du); - const scalar_t deltaXnorm = trajectoryNorm(dx); - - // Prepare step info - multiple_shooting::StepInfo stepInfo; + const auto deltaUnorm = multiple_shooting::trajectoryNorm(du); + const auto deltaXnorm = multiple_shooting::trajectoryNorm(dx); scalar_t alpha = 1.0; vector_array_t xNew(x.size()); vector_array_t uNew(u.size()); + std::vector metricsNew(metrics.size()); do { // Compute step - for (int i = 0; i < u.size(); i++) { - if (du[i].size() > 0) { // account for absence of inputs at events. - uNew[i] = u[i] + alpha * du[i]; - } - } - for (int i = 0; i < x.size(); i++) { - xNew[i] = x[i] + alpha * dx[i]; - } + multiple_shooting::incrementTrajectory(u, du, alpha, uNew); + multiple_shooting::incrementTrajectory(x, dx, alpha, xNew); // Compute cost and constraints - const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew); - const scalar_t newConstraintViolation = totalConstraintViolation(performanceNew); + const PerformanceIndex performanceNew = computePerformance(timeDiscretization, initState, xNew, uNew, metricsNew); // Step acceptance and record step type - const bool stepAccepted = [&]() { - if (newConstraintViolation > settings_.g_max) { - // High constraint violation. Only accept decrease in constraints. - stepInfo.stepType = StepType::CONSTRAINT; - return newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); - } else if (newConstraintViolation < settings_.g_min && baselineConstraintViolation < settings_.g_min && - subproblemSolution.armijoDescentMetric < 0.0) { - // With low violation and having a descent direction, require the armijo condition. - stepInfo.stepType = StepType::COST; - return performanceNew.merit < (baseline.merit + settings_.armijoFactor * alpha * subproblemSolution.armijoDescentMetric); - } else { - // Medium violation: either merit or constraints decrease (with small gamma_c mixing of old constraints) - stepInfo.stepType = StepType::DUAL; - return performanceNew.merit < (baseline.merit - settings_.gamma_c * baselineConstraintViolation) || - newConstraintViolation < ((1.0 - settings_.gamma_c) * baselineConstraintViolation); - } - }(); + bool stepAccepted; + StepType stepType; + std::tie(stepAccepted, stepType) = + filterLinesearch_.acceptStep(baseline, performanceNew, alpha * subproblemSolution.armijoDescentMetric); if (settings_.printLinesearch) { - std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepInfo.stepType) + std::cerr << "Step size: " << alpha << ", Step Type: " << toString(stepType) << (stepAccepted ? std::string{" (Accepted)"} : std::string{" (Rejected)"}) << "\n"; std::cerr << "|dx| = " << alpha * deltaXnorm << "\t|du| = " << alpha * deltaUnorm << "\n"; std::cerr << performanceNew << "\n"; @@ -624,13 +513,18 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn if (stepAccepted) { // Return if step accepted x = std::move(xNew); u = std::move(uNew); + metrics = std::move(metricsNew); + // Prepare step info + sqp::StepInfo stepInfo; stepInfo.stepSize = alpha; + stepInfo.stepType = stepType; stepInfo.dx_norm = alpha * deltaXnorm; stepInfo.du_norm = alpha * deltaUnorm; stepInfo.performanceAfterStep = performanceNew; - stepInfo.totalConstraintViolationAfterStep = newConstraintViolation; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(performanceNew); return stepInfo; + } else { // Try smaller step alpha *= settings_.alpha_decay; @@ -646,12 +540,13 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn } while (alpha >= settings_.alpha_min); // Alpha_min reached -> Don't take a step + sqp::StepInfo stepInfo; stepInfo.stepSize = 0.0; stepInfo.stepType = StepType::ZERO; stepInfo.dx_norm = 0.0; stepInfo.du_norm = 0.0; stepInfo.performanceAfterStep = baseline; - stepInfo.totalConstraintViolationAfterStep = baselineConstraintViolation; + stepInfo.totalConstraintViolationAfterStep = FilterLinesearch::totalConstraintViolation(baseline); if (settings_.printLinesearch) { std::cerr << "[Linesearch terminated] Step size: " << stepInfo.stepSize << ", Step Type: " << toString(stepInfo.stepType) << "\n"; @@ -660,9 +555,8 @@ multiple_shooting::StepInfo MultipleShootingSolver::takeStep(const PerformanceIn return stepInfo; } -multiple_shooting::Convergence MultipleShootingSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, - const multiple_shooting::StepInfo& stepInfo) const { - using Convergence = multiple_shooting::Convergence; +sqp::Convergence SqpSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const sqp::StepInfo& stepInfo) const { + using Convergence = sqp::Convergence; if ((iteration + 1) >= settings_.sqpIteration) { // Converged because the next iteration would exceed the specified number of iterations return Convergence::ITERATIONS; @@ -670,7 +564,7 @@ multiple_shooting::Convergence MultipleShootingSolver::checkConvergence(int iter // Converged because step size is below the specified minimum return Convergence::STEPSIZE; } else if (std::abs(stepInfo.performanceAfterStep.merit - baseline.merit) < settings_.costTol && - totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { + FilterLinesearch::totalConstraintViolation(stepInfo.performanceAfterStep) < settings_.g_min) { // Converged because the change in merit is below the specified tolerance while the constraint violation is below the minimum return Convergence::METRICS; } else if (stepInfo.dx_norm < settings_.deltaTol && stepInfo.du_norm < settings_.deltaTol) { diff --git a/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp b/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp index 05e1d9897..83024150f 100644 --- a/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testCircularKinematics.cpp @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "ocs2_sqp/MultipleShootingSolver.h" +#include "ocs2_sqp/SqpSolver.h" #include @@ -37,13 +37,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. TEST(test_circular_kinematics, solve_projected_EqConstraints) { // optimal control problem - ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/sqp_test_generated"); + ocs2::OptimalControlProblem problem = ocs2::createCircularKinematicsProblem("/tmp/ocs2/sqp_test_generated"); // Initializer ocs2::DefaultInitializer zeroInitializer(2); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.01; settings.sqpIteration = 20; settings.projectStateInputEqualityConstraints = true; @@ -59,7 +59,7 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints) { const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 // Solve - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); // Inspect solution @@ -97,7 +97,7 @@ TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { ocs2::DefaultInitializer zeroInitializer(2); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.01; settings.sqpIteration = 20; settings.projectStateInputEqualityConstraints = false; // <- false to turn off projection of state-input equalities @@ -112,7 +112,7 @@ TEST(test_circular_kinematics, solve_EqConstraints_inQPSubproblem) { const ocs2::vector_t initState = (ocs2::vector_t(2) << 1.0, 0.0).finished(); // radius 1.0 // Solve - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.run(startTime, initState, finalTime); // Inspect solution diff --git a/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp b/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp index 5ad47d6cd..67d63f6e9 100644 --- a/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testSwitchedProblem.cpp @@ -29,15 +29,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "ocs2_sqp/MultipleShootingSolver.h" -#include "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_sqp/SqpSolver.h" #include #include #include #include - +#include #include + #include namespace ocs2 { @@ -116,18 +116,17 @@ std::pair> solveWithEventTime(scal // Reference Manager const ocs2::ModeSchedule modeSchedule({eventTime}, {0, 1}); const ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Random(n)}, {ocs2::vector_t::Random(m)}); - std::shared_ptr referenceManagerPtr(new ocs2::ReferenceManager(targetTrajectories, modeSchedule)); + auto referenceManagerPtr = std::make_shared(targetTrajectories, modeSchedule); problem.targetTrajectoriesPtr = &targetTrajectories; // Constraint - problem.equalityConstraintPtr->add("switchedConstraint", - std::unique_ptr(new ocs2::SwitchedConstraint(referenceManagerPtr))); + problem.equalityConstraintPtr->add("switchedConstraint", std::make_unique(referenceManagerPtr)); ocs2::DefaultInitializer zeroInitializer(m); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.05; settings.sqpIteration = 20; settings.projectStateInputEqualityConstraints = true; @@ -141,7 +140,7 @@ std::pair> solveWithEventTime(scal const ocs2::vector_t initState = ocs2::vector_t::Random(n); // Set up solver - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); // Solve diff --git a/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp b/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp index c68ffe0d9..e9cd6d372 100644 --- a/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testUnconstrained.cpp @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "ocs2_sqp/MultipleShootingSolver.h" +#include "ocs2_sqp/SqpSolver.h" #include @@ -56,7 +56,7 @@ std::pair> solveWithFeedbackSettin // Reference Managaer ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Ones(n)}, {ocs2::vector_t::Ones(m)}); - std::shared_ptr referenceManagerPtr(new ReferenceManager(targetTrajectories)); + auto referenceManagerPtr = std::make_shared(targetTrajectories); problem.targetTrajectoriesPtr = &referenceManagerPtr->getTargetTrajectories(); @@ -67,7 +67,7 @@ std::pair> solveWithFeedbackSettin ocs2::DefaultInitializer zeroInitializer(m); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.05; settings.sqpIteration = 10; settings.projectStateInputEqualityConstraints = true; @@ -83,7 +83,7 @@ std::pair> solveWithFeedbackSettin const ocs2::vector_t initState = ocs2::vector_t::Ones(n); // Construct solver - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); // Solve diff --git a/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp b/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp index d9e6fe2a5..56e00dc8f 100644 --- a/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp +++ b/ocs2_sqp/ocs2_sqp/test/testValuefunction.cpp @@ -29,11 +29,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "ocs2_sqp/MultipleShootingSolver.h" -#include "ocs2_sqp/TimeDiscretization.h" +#include "ocs2_sqp/SqpSolver.h" #include +#include #include #include @@ -44,7 +44,7 @@ TEST(test_valuefunction, linear_quadratic_problem) { constexpr int Nsample = 10; constexpr ocs2::scalar_t tol = 1e-9; const ocs2::scalar_t startTime = 0.0; - const ocs2::scalar_t eventTime = 1.0/3.0; + const ocs2::scalar_t eventTime = 1.0 / 3.0; const ocs2::scalar_t finalTime = 1.0; ocs2::OptimalControlProblem problem; @@ -62,17 +62,17 @@ TEST(test_valuefunction, linear_quadratic_problem) { // Reference Manager const ocs2::ModeSchedule modeSchedule({eventTime}, {0, 1}); const ocs2::TargetTrajectories targetTrajectories({0.0}, {ocs2::vector_t::Random(n)}, {ocs2::vector_t::Random(m)}); - std::shared_ptr referenceManagerPtr(new ocs2::ReferenceManager(targetTrajectories, modeSchedule)); + auto referenceManagerPtr = std::make_shared(targetTrajectories, modeSchedule); problem.targetTrajectoriesPtr = &targetTrajectories; // Constraint - problem.equalityConstraintPtr->add("constraint", ocs2::getOcs2Constraints(ocs2::getRandomConstraints(n, m, nc))); + problem.equalityConstraintPtr->add("constraint", ocs2::getOcs2Constraints(ocs2::getRandomConstraints(n, m, nc))); ocs2::DefaultInitializer zeroInitializer(m); // Solver settings - ocs2::multiple_shooting::Settings settings; + ocs2::sqp::Settings settings; settings.dt = 0.05; settings.sqpIteration = 1; settings.projectStateInputEqualityConstraints = true; @@ -83,14 +83,14 @@ TEST(test_valuefunction, linear_quadratic_problem) { settings.createValueFunction = true; // Set up solver - ocs2::MultipleShootingSolver solver(settings, problem, zeroInitializer); + ocs2::SqpSolver solver(settings, problem, zeroInitializer); solver.setReferenceManager(referenceManagerPtr); // Get value function const ocs2::vector_t zeroState = ocs2::vector_t::Random(n); solver.reset(); solver.run(startTime, zeroState, finalTime); - const auto costToGo = solver.getValueFunction(startTime, zeroState); + const auto costToGo = solver.getValueFunction(startTime, zeroState); const ocs2::scalar_t zeroCost = solver.getPerformanceIndeces().cost; // Solve for random states and check consistency with value function @@ -103,4 +103,4 @@ TEST(test_valuefunction, linear_quadratic_problem) { EXPECT_NEAR(sampleCost, zeroCost + costToGo.dfdx.dot(dx) + 0.5 * dx.dot(costToGo.dfdxx * dx), tol); } -} \ No newline at end of file +} diff --git a/ocs2_test_tools/ocs2_qp_solver/test/testOcs2QpSolver.cpp b/ocs2_test_tools/ocs2_qp_solver/test/testOcs2QpSolver.cpp index c47d63abe..c68c7c51f 100644 --- a/ocs2_test_tools/ocs2_qp_solver/test/testOcs2QpSolver.cpp +++ b/ocs2_test_tools/ocs2_qp_solver/test/testOcs2QpSolver.cpp @@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include @@ -129,7 +130,7 @@ TEST_F(Ocs2QpSolverTest, satisfiesConstraints) { const auto& u0 = constrainedSolution.inputTrajectory[0]; preComputation.request(ocs2::Request::Constraint, t0, x0, u0); - const auto g0 = unconstrainedProblem.equalityConstraintPtr->getValue(t0, x0, u0, preComputation); + const auto g0 = ocs2::toVector(unconstrainedProblem.equalityConstraintPtr->getValue(t0, x0, u0, preComputation)); ASSERT_TRUE(g0.isZero(precision)); for (int k = 1; k < N; ++k) { @@ -138,10 +139,10 @@ TEST_F(Ocs2QpSolverTest, satisfiesConstraints) { const auto& u = constrainedSolution.inputTrajectory[k]; preComputation.request(ocs2::Request::Constraint, t, x, u); - const auto g = unconstrainedProblem.equalityConstraintPtr->getValue(t, x, u, preComputation); + const auto g = ocs2::toVector(unconstrainedProblem.equalityConstraintPtr->getValue(t, x, u, preComputation)); ASSERT_TRUE(g.isZero(precision)); - const auto h = unconstrainedProblem.stateEqualityConstraintPtr->getValue(t, x, preComputation); + const auto h = ocs2::toVector(unconstrainedProblem.stateEqualityConstraintPtr->getValue(t, x, preComputation)); ASSERT_TRUE(h.isZero(precision)); } @@ -149,7 +150,7 @@ TEST_F(Ocs2QpSolverTest, satisfiesConstraints) { const auto& xf = constrainedSolution.stateTrajectory[N]; preComputation.requestFinal(ocs2::Request::Constraint, tf, xf); - const auto gf = unconstrainedProblem.finalEqualityConstraintPtr->getValue(tf, xf, preComputation); + const auto gf = ocs2::toVector(unconstrainedProblem.finalEqualityConstraintPtr->getValue(tf, xf, preComputation)); ASSERT_TRUE(gf.isZero(precision)); }