diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e9ea2114..738af981 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -13,6 +13,7 @@ set(SOURCES GenQPSolver.cpp QPContactConstr.cpp QLDQPSolver.cpp + QPCoincidenceConstr.cpp ) set(HEADERS Tasks/Tasks.h @@ -25,6 +26,7 @@ set(HEADERS Tasks/GenQPSolver.h Tasks/Bounds.h Tasks/QPContactConstr.h + Tasks/QPCoincidenceConstr.h ) set(PRIVATE_HEADERS utils.h GenQPUtils.h QLDQPSolver.h) diff --git a/src/QPCoincidenceConstr.cpp b/src/QPCoincidenceConstr.cpp new file mode 100644 index 00000000..e9467860 --- /dev/null +++ b/src/QPCoincidenceConstr.cpp @@ -0,0 +1,193 @@ +#include "Tasks/QPCoincidenceConstr.h" + +// RBDyn +#include +#include +#include + +// Tasks +#include "utils.h" + +namespace tasks +{ +namespace qp +{ + +const Eigen::MatrixXd & CoincidenceConstr::AEq() const +{ + return A_; +} + +const Eigen::VectorXd & CoincidenceConstr::bEq() const +{ + return b_; +} + +int CoincidenceConstr::maxEq() const +{ + return int(A_.rows()); +} + +void CoincidenceConstr::setJointSelector(const Eigen::VectorXd & selector) +{ + jointSelector_ = selector; +} + +// =================== FixedCoincidenceConstr =================== + +FixedCoincidenceConstr::FixedCoincidenceConstr(int robotIndex, + const std::string & body1Name, + const std::string & body2Name, + const Eigen::Vector3d & point1, + const Eigen::Vector3d & point2, + const Eigen::VectorXd & jointSelector) +: CoincidenceConstr(robotIndex, body1Name, body2Name, jointSelector), point1_(point1), point2_(point2) +{ + A_.resize(0, 0); + b_.resize(0); +} + +std::string FixedCoincidenceConstr::nameEq() const +{ + return "FixedCoincidenceConstr"; +} + +std::string FixedCoincidenceConstr::descEq(const std::vector & mbs, int i) +{ + return "FixedCoincidenceConstr between " + body1Name_ + " and " + body2Name_ + "."; +} + +void FixedCoincidenceConstr::updateNrVars(const std::vector & mbs, const SolverData & data) +{ + const rbd::MultiBody & mb = mbs[robotIndex_]; + body1Index_ = mb.bodyIndexByName(body1Name_); + body2Index_ = mb.bodyIndexByName(body2Name_); + if(body1Index_ == -1 || body2Index_ == -1) + { + // Error: Invalid body indices in FixedCoincidenceConstr::update + return; + } + + A_.setZero(6, data.nrVars()); + b_.setZero(6); +} + +void FixedCoincidenceConstr::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + using namespace Eigen; + const rbd::MultiBody & mb = mbs[robotIndex_]; + const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_]; + if(body1Index_ == -1 || body2Index_ == -1) + { + // Error: Invalid body indices in FixedCoincidenceConstr::update + return; + } + + int alphaDBegin = data.alphaDBegin(robotIndex_); + int nrDof = mb.nrDof(); + + rbd::Jacobian jac1(mb, body1Name_); + Eigen::MatrixXd jacMat1 = jac1.jacobian(mb, mbc); + Eigen::MatrixXd fulljacobian1(6, nrDof); + jac1.fullJacobian(mb, jacMat1.block(0, 0, 6, nrDof), fulljacobian1); + + rbd::Jacobian jac2(mb, body2Name_); + Eigen::MatrixXd jacMat2 = jac2.jacobian(mb, mbc); + Eigen::MatrixXd fulljacobian2(6, nrDof); + jac2.fullJacobian(mb, jacMat2.block(0, 0, 6, nrDof), fulljacobian2); + + A_.block(0, alphaDBegin, 6, nrDof) = (fulljacobian1 - fulljacobian2) * jointSelector_.asDiagonal(); + + const std::vector & normalAccB = data.normalAccB(robotIndex_); + Vector6d normalAcc1 = jac1.normalAcceleration(mb, mbc, normalAccB).vector(); + Vector6d normalAcc2 = jac2.normalAcceleration(mb, mbc, normalAccB).vector(); + + double Kp = 1; + double Kd = 2; + Eigen::VectorXd errorp = mbc.bodyPosW[body1Index_].translation() - mbc.bodyPosW[body2Index_].translation(); + Eigen::VectorXd errord = mbc.bodyVelW[body1Index_].linear() - mbc.bodyVelW[body2Index_].linear(); + + b_.segment(0, 6) = -(normalAcc1.tail(6) - normalAcc2.tail(6)) - Kp * errorp - Kd * errord; +} + +// =============== RotationalCoincidenceConstr =============== + +RotationalCoincidenceConstr::RotationalCoincidenceConstr(int robotIndex, + const std::string & body1Name, + const std::string & body2Name, + const Eigen::VectorXd & jointSelector) +: CoincidenceConstr(robotIndex, body1Name, body2Name, jointSelector) +{ + A_.resize(0, 0); + b_.resize(0); +} + +std::string RotationalCoincidenceConstr::nameEq() const +{ + return "RotationalCoincidenceConstr"; +} + +std::string RotationalCoincidenceConstr::descEq(const std::vector & mbs, int i) +{ + return "RotationalCoincidenceConstr between " + body1Name_ + " and " + body2Name_ + "."; +} + +void RotationalCoincidenceConstr::updateNrVars(const std::vector & mbs, const SolverData & data) +{ + const rbd::MultiBody & mb = mbs[robotIndex_]; + body1Index_ = mb.bodyIndexByName(body1Name_); + body2Index_ = mb.bodyIndexByName(body2Name_); + if(body1Index_ == -1 || body2Index_ == -1) + { + // Error: Invalid body indices in RotationalCoincidenceConstr::update + return; + } + + A_.setZero(3, data.nrVars()); + b_.setZero(3); +} + +void RotationalCoincidenceConstr::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + using namespace Eigen; + const rbd::MultiBody & mb = mbs[robotIndex_]; + const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_]; + if(body1Index_ == -1 || body2Index_ == -1) + { + // Error: Invalid body indices in RotationalCoincidenceConstr::update + return; + } + + int alphaDBegin = data.alphaDBegin(robotIndex_); + int nrDof = mb.nrDof(); + + rbd::Jacobian jac1(mb, body1Name_); + Eigen::MatrixXd jacMat1 = jac1.jacobian(mb, mbc); + Eigen::MatrixXd fulljacobian1(3, nrDof); + jac1.fullJacobian(mb, jacMat1.block(3, 0, 3, nrDof), fulljacobian1); + + rbd::Jacobian jac2(mb, body2Name_); + Eigen::MatrixXd jacMat2 = jac2.jacobian(mb, mbc); + Eigen::MatrixXd fulljacobian2(3, nrDof); + jac2.fullJacobian(mb, jacMat2.block(3, 0, 3, nrDof), fulljacobian2); + + A_.block(0, alphaDBegin, 3, nrDof) = (fulljacobian1 - fulljacobian2) * jointSelector_.asDiagonal(); + + const std::vector & normalAccB = data.normalAccB(robotIndex_); + Vector6d normalAcc1 = jac1.normalAcceleration(mb, mbc, normalAccB).vector(); + Vector6d normalAcc2 = jac2.normalAcceleration(mb, mbc, normalAccB).vector(); + + double Kp = 1; + double Kd = 2; + Eigen::VectorXd errorp = mbc.bodyPosW[body1Index_].translation() - mbc.bodyPosW[body2Index_].translation(); + Eigen::VectorXd errord = mbc.bodyVelW[body1Index_].linear() - mbc.bodyVelW[body2Index_].linear(); + + b_.segment(0, 3) = -(normalAcc1.tail(3) - normalAcc2.tail(3)) - Kp * errorp - Kd * errord; +} + +} // namespace qp +} // namespace tasks diff --git a/src/Tasks/QPCoincidenceConstr.h b/src/Tasks/QPCoincidenceConstr.h new file mode 100644 index 00000000..f92bd497 --- /dev/null +++ b/src/Tasks/QPCoincidenceConstr.h @@ -0,0 +1,154 @@ +/* + * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#pragma once + +// includes +// std +#include + +// Eigen +#include + +// Tasks +#include "QPSolver.h" + +namespace tasks +{ + +namespace qp +{ + +/** + * Base class for coincidence equality constraints. + * + * This constraint enforces acceleration-level coincidence between two bodies + * of the same robot using their Jacobians and normal accelerations. + * + * A proportional-derivative (PD) stabilization term is added in the update step + * to reduce position and velocity errors between the bodies. + * + * Derived classes define whether the coincidence applies to translation, + * rotation, or both. + */ +class TASKS_DLLAPI CoincidenceConstr : public ConstraintFunction +{ +public: + /** + * @param robotIndex Constrained robot Index in mbs. + * @param body1Name Name of the first body. + * @param body2Name Name of the second body. + * @param jointSelector Joint selection vector. + */ + CoincidenceConstr(int robotIndex, + const std::string & body1Name, + const std::string & body2Name, + const Eigen::VectorXd & jointSelector) + : robotIndex_(robotIndex), body1Name_(body1Name), body2Name_(body2Name), body1Index_(-1), body2Index_(-1), + jointSelector_(jointSelector) + { + } + + // Equality constraint + virtual int maxEq() const override; + + virtual const Eigen::MatrixXd & AEq() const override; + + virtual const Eigen::VectorXd & bEq() const override; + + /** + * Set the joint selector. + * @param selector Joint selection vector. + */ + void setJointSelector(const Eigen::VectorXd & selector); + +protected: + Eigen::MatrixXd A_; + Eigen::VectorXd b_; + int robotIndex_; + std::string body1Name_, body2Name_; + int body1Index_, body2Index_; + Eigen::VectorXd jointSelector_; +}; + +/** + * Enforce translational coincidence between two bodies. + * + * This constraint enforces equality of linear accelerations between two bodies + * (6D formulation), using the difference of their full Jacobians. + * + * A PD stabilization term is added based on the difference of body world + * positions and linear velocities. + */ +class TASKS_DLLAPI FixedCoincidenceConstr : public CoincidenceConstr +{ +public: + /** + * @param robotIndex Constrained robot Index in mbs. + * @param body1Name Name of the first body. + * @param body2Name Name of the second body. + * @param point1 Unused (kept for API compatibility). + * @param point2 Unused (kept for API compatibility). + * @param jointSelector Joint selection vector. + */ + FixedCoincidenceConstr(int robotIndex = 0, + const std::string & body1Name = "", + const std::string & body2Name = "", + const Eigen::Vector3d & point1 = Eigen::Vector3d::Zero(), + const Eigen::Vector3d & point2 = Eigen::Vector3d::Zero(), + const Eigen::VectorXd & jointSelector = Eigen::VectorXd()); + + // Equality constraint + virtual std::string nameEq() const override; + + virtual std::string descEq(const std::vector & mbs, int line) override; + + virtual void updateNrVars(const std::vector & mbs, const SolverData & data) override; + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) override; + +private: + Eigen::Vector3d point1_, point2_; +}; + +/** + * Enforce rotational coincidence between two bodies. + * + * This constraint enforces equality of angular accelerations between two bodies + * (3D formulation), using the angular part of their Jacobians. + * + * A PD stabilization term is added based on the difference of body world + * orientations and angular velocities. + */ +class TASKS_DLLAPI RotationalCoincidenceConstr : public CoincidenceConstr +{ +public: + /** + * @param robotIndex Constrained robot Index in mbs. + * @param body1Name Name of the first body. + * @param body2Name Name of the second body. + * @param jointSelector Joint selection vector. + */ + RotationalCoincidenceConstr(int robotIndex = 0, + const std::string & body1Name = "", + const std::string & body2Name = "", + const Eigen::VectorXd & jointSelector = Eigen::VectorXd()); + + // Equality constraint + virtual std::string nameEq() const override; + + virtual std::string descEq(const std::vector & mbs, int line) override; + + virtual void updateNrVars(const std::vector & mbs, const SolverData & data) override; + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) override; +}; + +} // namespace qp + +} // namespace tasks