diff --git a/binding/python/include/qp_wrapper.hpp b/binding/python/include/qp_wrapper.hpp index 8bddbdcf7..79fb80877 100644 --- a/binding/python/include/qp_wrapper.hpp +++ b/binding/python/include/qp_wrapper.hpp @@ -19,5 +19,10 @@ JointsSelector* UnactiveJoints2Ptr(const std::vector& mbs, int r return new JointsSelector(JointsSelector::UnactiveJoints(mbs, robotIndex, hl, unactiveJointsNames)); } +std::shared_ptr FD2ShPtr(rbd::ForwardDynamics & fd) +{ + return std::shared_ptr(&fd, [](rbd::ForwardDynamics *){}); +} + } } diff --git a/binding/python/tasks/qp/c_qp.pxd b/binding/python/tasks/qp/c_qp.pxd index e55330ebe..6d621b6a6 100644 --- a/binding/python/tasks/qp/c_qp.pxd +++ b/binding/python/tasks/qp/c_qp.pxd @@ -7,6 +7,7 @@ from sva.c_sva cimport * from rbdyn.c_rbdyn cimport * from sch.c_sch cimport * cimport tasks.c_tasks as c_tasks +from libcpp.memory cimport shared_ptr from libcpp.string cimport string from libcpp.vector cimport vector from libcpp cimport bool @@ -494,7 +495,7 @@ cdef extern from "" namespace "tasks::qp": cdef extern from "" namespace "tasks::qp": cdef cppclass MotionPolyConstr(ConstraintFunction[GenInequality], GenInequality, Constraint): - MotionPolyConstr(const vector[MultiBody]&, int, const c_tasks.PolyTorqueBound&) + MotionPolyConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.PolyTorqueBound&) # Motion default void computeTorque(const VectorXd&, const VectorXd&) VectorXd torque() const @@ -504,8 +505,8 @@ cdef extern from "" namespace "tasks::qp": void removeFromSolver(QPSolver &) cdef cppclass MotionConstr(ConstraintFunction[GenInequality], GenInequality, Constraint): - MotionConstr(const vector[MultiBody]&, int, const c_tasks.TorqueBound&) - MotionConstr(const vector[MultiBody]&, int, const c_tasks.TorqueBound&, const c_tasks.TorqueDBound&, double) + MotionConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.TorqueBound&) + MotionConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.TorqueBound&, const c_tasks.TorqueDBound&, double) # Motion default void computeTorque(const VectorXd&, const VectorXd&) VectorXd torque() const @@ -515,8 +516,8 @@ cdef extern from "" namespace "tasks::qp": void removeFromSolver(QPSolver &) cdef cppclass MotionSpringConstr(ConstraintFunction[GenInequality], GenInequality, Constraint): - MotionSpringConstr(const vector[MultiBody]&, int, const c_tasks.TorqueBound&, const vector[SpringJoint]&) - MotionSpringConstr(const vector[MultiBody]&, int, const c_tasks.TorqueBound&, const c_tasks.TorqueDBound&, double, const vector[SpringJoint]&) + MotionSpringConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.TorqueBound&, const vector[SpringJoint]&) + MotionSpringConstr(const vector[MultiBody]&, int, const shared_ptr[ForwardDynamics], const c_tasks.TorqueBound&, const c_tasks.TorqueDBound&, double, const vector[SpringJoint]&) # Motion default void computeTorque(const VectorXd&, const VectorXd&) VectorXd torque() const diff --git a/binding/python/tasks/qp/c_qp_private.pxd b/binding/python/tasks/qp/c_qp_private.pxd index 998d967bc..f650fb723 100644 --- a/binding/python/tasks/qp/c_qp_private.pxd +++ b/binding/python/tasks/qp/c_qp_private.pxd @@ -10,3 +10,4 @@ from libcpp.vector cimport vector cdef extern from "qp_wrapper.hpp" namespace "tasks::qp": JointsSelector* ActiveJoints2Ptr(const vector[MultiBody]&, int, HighLevelTask*, const vector[string]) JointsSelector* UnactiveJoints2Ptr(const vector[MultiBody]&, int, HighLevelTask*, const vector[string]) + shared_ptr[ForwardDynamics] FD2ShPtr(ForwardDynamics & fd) diff --git a/binding/python/tasks/qp/qp.pxd b/binding/python/tasks/qp/qp.pxd index 934820247..44dbfe0e5 100644 --- a/binding/python/tasks/qp/qp.pxd +++ b/binding/python/tasks/qp/qp.pxd @@ -3,6 +3,7 @@ # cimport c_qp +cimport rbdyn.rbdyn as rbdyn from libcpp.vector cimport vector from libcpp cimport bool as cppbool @@ -154,18 +155,21 @@ cdef class GripperTorqueTask(Task): cdef class MotionConstr(GenInequality): cdef c_qp.MotionConstr * impl cdef cppbool __own_impl + cdef rbdyn.ForwardDynamics fd_instance cdef MotionConstr MotionConstrFromPtr(c_qp.MotionConstr *) cdef class MotionPolyConstr(GenInequality): cdef c_qp.MotionPolyConstr * impl cdef cppbool __own_impl + cdef rbdyn.ForwardDynamics fd_instance cdef MotionPolyConstr MotionPolyConstrFromPtr(c_qp.MotionPolyConstr *) cdef class MotionSpringConstr(GenInequality): cdef c_qp.MotionSpringConstr * impl cdef cppbool __own_impl + cdef rbdyn.ForwardDynamics fd_instance cdef MotionSpringConstr MotionSpringConstrFromPtr(c_qp.MotionSpringConstr *) diff --git a/binding/python/tasks/qp/qp.pyx b/binding/python/tasks/qp/qp.pyx index 3f1129329..94101d212 100644 --- a/binding/python/tasks/qp/qp.pyx +++ b/binding/python/tasks/qp/qp.pyx @@ -1614,13 +1614,15 @@ cdef class MotionConstr(GenInequality): def __dealloc__(self): if self.__own_impl: del self.impl - def __cinit__(self, MultiBodyVector mbs, int robotIndex, tasks.TorqueBound tb, tasks.TorqueDBound tdb = None, dt = None, skip_alloc = False): + def __cinit__(self, MultiBodyVector mbs, int robotIndex, ForwardDynamics fd, tasks.TorqueBound tb, tasks.TorqueDBound tdb = None, dt = None, skip_alloc = False): self.__own_impl = True if not skip_alloc: + # Keep FD alive + self.fd_instance = fd if tdb is None: - self.impl = new c_qp.MotionConstr(deref(mbs.v), robotIndex, tb.impl) + self.impl = new c_qp.MotionConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl) else: - self.impl = new c_qp.MotionConstr(deref(mbs.v), robotIndex, tb.impl, tdb.impl, dt) + self.impl = new c_qp.MotionConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl, tdb.impl, dt) self.cf_base = self.impl self.genineq_base = self.impl self.constraint_base = self.impl @@ -1656,10 +1658,11 @@ cdef class MotionPolyConstr(GenInequality): def __dealloc__(self): if self.__own_impl: del self.impl - def __cinit__(self, MultiBodyVector mbs, int robotIndex, tasks.PolyTorqueBound tb, skip_alloc = False): + def __cinit__(self, MultiBodyVector mbs, int robotIndex, ForwardDynamics fd, tasks.PolyTorqueBound tb, skip_alloc = False): self.__own_impl = True if not skip_alloc: - self.impl = new c_qp.MotionPolyConstr(deref(mbs.v), robotIndex, tb.impl) + self.fd_instance = fd + self.impl = new c_qp.MotionPolyConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl) self.cf_base = self.impl self.genineq_base = self.impl self.constraint_base = self.impl @@ -1695,13 +1698,14 @@ cdef class MotionSpringConstr(GenInequality): def __dealloc__(self): if self.__own_impl: del self.impl - def __cinit__(self, MultiBodyVector mbs, int robotIndex, tasks.TorqueBound tb, sjs, tasks.TorqueDBound tdb = None, dt = None, skip_alloc = False): + def __cinit__(self, MultiBodyVector mbs, int robotIndex, ForwardDynamics fd, tasks.TorqueBound tb, sjs, tasks.TorqueDBound tdb = None, dt = None, skip_alloc = False): self.__own_impl = True if not skip_alloc: + self.fd_instance = fd if tdb is None: - self.impl = new c_qp.MotionSpringConstr(deref(mbs.v), robotIndex, tb.impl, SpringJointVector(sjs).v) + self.impl = new c_qp.MotionSpringConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl, SpringJointVector(sjs).v) else: - self.impl = new c_qp.MotionSpringConstr(deref(mbs.v), robotIndex, tb.impl, tdb.impl, dt, SpringJointVector(sjs).v) + self.impl = new c_qp.MotionSpringConstr(deref(mbs.v), robotIndex, c_qp_private.FD2ShPtr(fd.impl), tb.impl, tdb.impl, dt, SpringJointVector(sjs).v) self.cf_base = self.impl self.genineq_base = self.impl self.constraint_base = self.impl diff --git a/binding/python/tests/TestQPMultiRobot.py b/binding/python/tests/TestQPMultiRobot.py index 7a7cdbf1b..63edb163d 100644 --- a/binding/python/tests/TestQPMultiRobot.py +++ b/binding/python/tests/TestQPMultiRobot.py @@ -152,6 +152,9 @@ def test(self): mb1, mbc1Init = arms.makeZXZArm() rbdyn.forwardKinematics(mb1, mbc1Init) rbdyn.forwardVelocity(mb1, mbc1Init) + self.fd1 = rbdyn.ForwardDynamics(mb1) + self.fd1.computeH(mb1, mbc1Init) + self.fd1.computeC(mb1, mbc1Init) mb2, mbc2Init = arms.makeZXZArm(False) if not LEGACY: @@ -164,6 +167,9 @@ def test(self): mbc2Init.q[0] = [mb2InitOri.w(), mb2InitOri.x(), mb2InitOri.y(), mb2InitOri.z(), mb2InitPos.x(), mb2InitPos.y() + 1, mb2InitPos.z()] rbdyn.forwardKinematics(mb2, mbc2Init) rbdyn.forwardVelocity(mb2, mbc2Init) + self.fd2 = rbdyn.ForwardDynamics(mb2) + self.fd2.computeH(mb2, mbc2Init) + self.fd2.computeC(mb2, mbc2Init) if not LEGACY: X_0_b1 = sva.PTransformd(mbc1Init.bodyPosW[-1]) @@ -228,8 +234,8 @@ def test(self): torqueMax1 = [[], [Inf], [Inf], [Inf]] torqueMin2 = [[0,0,0,0,0,0], [-Inf], [-Inf], [-Inf]] torqueMax2 = [[0,0,0,0,0,0], [Inf], [Inf], [Inf]] - motion1 = tasks.qp.MotionConstr(mbs, 0, tasks.TorqueBound(torqueMin1, torqueMax1)) - motion2 = tasks.qp.MotionConstr(mbs, 1, tasks.TorqueBound(torqueMin2, torqueMax2)) + motion1 = tasks.qp.MotionConstr(mbs, 0, self.fd1, tasks.TorqueBound(torqueMin1, torqueMax1)) + motion2 = tasks.qp.MotionConstr(mbs, 1, self.fd2, tasks.TorqueBound(torqueMin2, torqueMax2)) plCstr = tasks.qp.PositiveLambda() motion1.addToSolver(solver) @@ -264,6 +270,10 @@ def test(self): rbdyn.eulerIntegration(mbs[i], mbcs[i], 0.001) rbdyn.forwardKinematics(mbs[i], mbcs[i]) rbdyn.forwardVelocity(mbs[i], mbcs[i]) + self.fd1.computeH(mbs[0], mbcs[0]) + self.fd1.computeC(mbs[0], mbcs[0]) + self.fd2.computeH(mbs[1], mbcs[1]) + self.fd2.computeC(mbs[1], mbcs[1]) # Check that the link hold if not LEGACY: diff --git a/binding/python/tests/TestQPSolver.py b/binding/python/tests/TestQPSolver.py index fe4a442dd..adf6b46b4 100644 --- a/binding/python/tests/TestQPSolver.py +++ b/binding/python/tests/TestQPSolver.py @@ -171,6 +171,9 @@ def setUp(self): rbdyn.forwardVelocity(mb, self.mbcInit) rbdyn.forwardKinematics(mbEnv, mbcEnv) rbdyn.forwardVelocity(mbEnv, mbcEnv) + self.fd = rbdyn.ForwardDynamics(mb) + self.fd.computeH(mb, self.mbcInit) + self.fd.computeC(mb, self.mbcInit) if not LEGACY: self.mbcs = rbdyn.MultiBodyConfigVector([self.mbcInit, mbcEnv]) @@ -201,6 +204,8 @@ def run_solver(self): rbdyn.eulerIntegration(self.mbs[0], self.mbcs[0], 0.001) rbdyn.forwardKinematics(self.mbs[0], self.mbcs[0]) rbdyn.forwardVelocity(self.mbs[0], self.mbcs[0]) + self.fd.computeH(self.mbs[0], self.mbcs[0]) + self.fd.computeC(self.mbs[0], self.mbcs[0]) def check_equality_constr(self, ConstrClass, *args): self.contVec = [tasks.qp.UnilateralContact(0, 1, "b3", "b0", [eigen.Vector3d.Zero()], eigen.Matrix3d.Identity(), sva.PTransformd.Identity(), 3, math.tan(math.pi/4))] @@ -255,7 +260,7 @@ def test_motion_constr(self): Inf = float("inf") torqueMin = [[], [-Inf], [-Inf], [-Inf]] torqueMax = [[], [Inf], [Inf], [Inf]] - motionCstr = tasks.qp.MotionConstr(self.mbs, 0, tasks.TorqueBound(torqueMin, torqueMax)) + motionCstr = tasks.qp.MotionConstr(self.mbs, 0, self.fd, tasks.TorqueBound(torqueMin, torqueMax)) plCstr = tasks.qp.PositiveLambda() motionCstr.addToSolver(self.solver) @@ -301,7 +306,7 @@ def test_motion_constr_w_contact(self): Inf = float("inf") torqueMin = [[], [-Inf], [-Inf], [-Inf]] torqueMax = [[], [Inf], [Inf], [Inf]] - motionCstr = tasks.qp.MotionConstr(self.mbs, 0, tasks.TorqueBound(torqueMin, torqueMax)) + motionCstr = tasks.qp.MotionConstr(self.mbs, 0, self.fd, tasks.TorqueBound(torqueMin, torqueMax)) plCstr = tasks.qp.PositiveLambda() motionCstr.addToSolver(self.solver) @@ -473,6 +478,9 @@ def setUp(self): rbdyn.forwardKinematics(mb, self.mbcInit) rbdyn.forwardVelocity(mb, self.mbcInit) + self.fd = rbdyn.ForwardDynamics(mb) + self.fd.computeH(mb, self.mbcInit) + self.fd.computeC(mb, self.mbcInit) if not LEGACY: self.mbcs = rbdyn.MultiBodyConfigVector([self.mbcInit]) @@ -499,7 +507,7 @@ def tearDown(self): def test_motion_constr(self): lBound = [[], [-30], [-30], [-30]] uBound = [[], [30], [30], [30]] - motionCstr = tasks.qp.MotionConstr(self.mbs, 0, tasks.TorqueBound(lBound, uBound)) + motionCstr = tasks.qp.MotionConstr(self.mbs, 0, self.fd, tasks.TorqueBound(lBound, uBound)) self.solver.addGenInequalityConstraint(motionCstr) self.assertEqual(self.solver.nrGenInequalityConstraints(), 1) @@ -522,6 +530,8 @@ def test_motion_constr(self): rbdyn.eulerIntegration(self.mbs[0], self.mbcs[0], 0.001) rbdyn.forwardKinematics(self.mbs[0], self.mbcs[0]) rbdyn.forwardVelocity(self.mbs[0], self.mbcs[0]) + self.fd.computeH(self.mbs[0], self.mbcs[0]) + self.fd.computeC(self.mbs[0], self.mbcs[0]) motionCstr.computeTorque(self.solver.alphaDVec(), self.solver.lambdaVec()) motionCstr.torque(self.mbs, self.mbcs) if not LEGACY: @@ -579,7 +589,7 @@ def test_motion_poly_constr(self): null = eigen.VectorXd() lBoundPoly = [[null], [lpoly], [lpoly], [lpoly]] uBoundPoly = [[null], [upoly], [upoly], [upoly]] - motionPolyCstr = tasks.qp.MotionPolyConstr(self.mbs, 0, tasks.PolyTorqueBound(lBoundPoly, uBoundPoly)) + motionPolyCstr = tasks.qp.MotionPolyConstr(self.mbs, 0, self.fd, tasks.PolyTorqueBound(lBoundPoly, uBoundPoly)) motionPolyCstr.addToSolver(self.solver) self.assertEqual(self.solver.nrGenInequalityConstraints(), 1) @@ -803,6 +813,9 @@ def test(self): rbdyn.forwardVelocity(mb, mbcInit) rbdyn.forwardKinematics(mbEnv, mbcEnv) rbdyn.forwardVelocity(mbEnv, mbcEnv) + self.fd = rbdyn.ForwardDynamics(mb) + self.fd.computeH(mb, mbcInit) + self.fd.computeC(mb, mbcInit) if not LEGACY: mbs = rbdyn.MultiBodyVector([mb, mbEnv]) @@ -816,7 +829,7 @@ def test(self): Inf = float("inf") torqueMin = [[0, 0, 0, 0, 0, 0], [-Inf], [-Inf], [-Inf]] torqueMax = [[0, 0, 0, 0, 0, 0], [Inf], [Inf], [Inf]] - motionCstr = tasks.qp.MotionConstr(mbs, 0, tasks.TorqueBound(torqueMin, torqueMax)) + motionCstr = tasks.qp.MotionConstr(mbs, 0, self.fd, tasks.TorqueBound(torqueMin, torqueMax)) plCstr = tasks.qp.PositiveLambda() contCstrAcc = tasks.qp.ContactAccConstr() @@ -858,6 +871,8 @@ def test(self): rbdyn.eulerIntegration(mbs[0], mbcs[0], 0.001) rbdyn.forwardKinematics(mbs[0], mbcs[0]) rbdyn.forwardVelocity(mbs[0], mbcs[0]) + self.fd.computeH(mbs[0], mbcs[0]) + self.fd.computeC(mbs[0], mbcs[0]) plCstr.removeFromSolver(solver) contCstrAcc.removeFromSolver(solver) diff --git a/src/GenQPUtils.h b/src/GenQPUtils.h index c026889b8..9da4d0b3d 100644 --- a/src/GenQPUtils.h +++ b/src/GenQPUtils.h @@ -21,7 +21,7 @@ namespace qp { // Value add to the diagonal to ensure positive matrix -static const double DIAG_CONSTANT = 1e-4; + static const double DIAG_CONSTANT = 1e-4; /** * Fill the \f$ Q \f$ matrix and the \f$ c \f$ vector based on the @@ -112,6 +112,8 @@ inline int fillEq(const std::vector & eq, Eigen::VectorXd & AL, Eigen::VectorXd & AU) { + // std::cout << "Rafa, in GenQPUtils::fillEq, nrALines = "; + for(std::size_t i = 0; i < eq.size(); ++i) { // ineq constraint can return a matrix with more line @@ -120,13 +122,19 @@ inline int fillEq(const std::vector & eq, const Eigen::MatrixXd & Ai = eq[i]->AEq(); const Eigen::VectorXd & bi = eq[i]->bEq(); + // std::cout << "(" << eq[i]->nameEq() << ") "; // Added by Rafa + A.block(nrALines, 0, nrConstr, nrVars) = Ai.block(0, 0, nrConstr, nrVars); AL.segment(nrALines, nrConstr) = bi.head(nrConstr); AU.segment(nrALines, nrConstr) = bi.head(nrConstr); nrALines += nrConstr; + + // std::cout << nrALines << " "; // Added by Rafa } + // std::cout << std::endl; // Added by Rafa + return nrALines; } @@ -141,6 +149,8 @@ inline int fillInEq(const std::vector & inEq, Eigen::VectorXd & AL, Eigen::VectorXd & AU) { + // std::cout << "Rafa, in GenQPUtils::fillInEq, nrALines = "; + for(std::size_t i = 0; i < inEq.size(); ++i) { // ineq constraint can return a matrix with more line @@ -149,13 +159,19 @@ inline int fillInEq(const std::vector & inEq, const Eigen::MatrixXd & Ai = inEq[i]->AInEq(); const Eigen::VectorXd & bi = inEq[i]->bInEq(); + // std::cout << "(" << inEq[i]->nameInEq() << ") "; // Added by Rafa + A.block(nrALines, 0, nrConstr, nrVars) = Ai.block(0, 0, nrConstr, nrVars); AL.segment(nrALines, nrConstr).fill(-std::numeric_limits::infinity()); AU.segment(nrALines, nrConstr) = bi.head(nrConstr); nrALines += nrConstr; + + // std::cout << nrALines << " "; // Added by Rafa } + // std::cout << std::endl; // Added by Rafa + return nrALines; } @@ -170,6 +186,8 @@ inline int fillGenInEq(const std::vector & genInEq, Eigen::VectorXd & AL, Eigen::VectorXd & AU) { + // std::cout << "Rafa, in GenQPUtils::fillGenInEq, nrALines = "; + for(std::size_t i = 0; i < genInEq.size(); ++i) { // ineq constraint can return a matrix with more line @@ -179,13 +197,19 @@ inline int fillGenInEq(const std::vector & genInEq, const Eigen::VectorXd & ALi = genInEq[i]->LowerGenInEq(); const Eigen::VectorXd & AUi = genInEq[i]->UpperGenInEq(); + // std::cout << "(" << genInEq[i]->nameGenInEq() << ") "; // Added by Rafa + A.block(nrALines, 0, nrConstr, nrVars) = Ai.block(0, 0, nrConstr, nrVars); AL.segment(nrALines, nrConstr) = ALi.head(nrConstr); AU.segment(nrALines, nrConstr) = AUi.head(nrConstr); nrALines += nrConstr; - } + // std::cout << nrALines << " "; // Added by Rafa + } + + // std::cout << std::endl; // Added by Rafa + return nrALines; } diff --git a/src/LSSOLQPSolver.cpp b/src/LSSOLQPSolver.cpp index a4c92785c..9af864d70 100644 --- a/src/LSSOLQPSolver.cpp +++ b/src/LSSOLQPSolver.cpp @@ -31,6 +31,7 @@ LSSOLQPSolver::LSSOLQPSolver() void LSSOLQPSolver::updateSize(int nrVars, int nrEq, int nrInEq, int nrGenInEq) { int maxALines = nrEq + nrInEq + nrGenInEq; + // std::cout << "Rafa, in LSSOLQPSolver::updateSize, maxALines = " << maxALines << std::endl; AFull_.resize(maxALines, nrVars); AL_.resize(maxALines); AU_.resize(maxALines); @@ -82,6 +83,12 @@ void LSSOLQPSolver::updateMatrix(const std::vector & tasks, nrALines_ = fillInEq(inEqConstr, nrVars, nrALines_, AFull_, AL_, AU_); nrALines_ = fillGenInEq(genInEqConstr, nrVars, nrALines_, AFull_, AL_, AU_); + // std::cout << "Rafa, in LSSOLQPSolver::updateMatrix, nrALines_ = " << nrALines_ << std::endl; + + // The following check was added temporarily by Rafa + //if (nrALines_ >= 40) + // *(int*)0 = 0; + fillBound(boundConstr, XLFull_, XUFull_); fillQC(tasks, nrVars, QFull_, CFull_); @@ -104,6 +111,8 @@ bool LSSOLQPSolver::solve() } else { + // std::cout << "Rafa, in LSSOLQPSolver::solve, AFull_.rows() = " << AFull_.rows() << std::endl; + success = lssol_.solve(XLFull_, XUFull_, static_cast(QFull_), CFull_, AFull_.block(0, 0, nrALines_, AFull_.cols()), AL_.segment(0, nrALines_), AU_.segment(0, nrALines_)); diff --git a/src/LSSOLQPSolver.h b/src/LSSOLQPSolver.h index a60e2b395..207115704 100644 --- a/src/LSSOLQPSolver.h +++ b/src/LSSOLQPSolver.h @@ -42,7 +42,8 @@ class TASKS_DLLAPI LSSOLQPSolver : public GenQPSolver std::ostream & out) const override; std::string name() const override; -private: +public: // Changed by Rafa as a test +//private: Eigen::LSSOL_QP lssol_; Eigen::MatrixXd A_; diff --git a/src/QPContactConstr.cpp b/src/QPContactConstr.cpp index 340a98b62..9b9c94cd1 100644 --- a/src/QPContactConstr.cpp +++ b/src/QPContactConstr.cpp @@ -387,6 +387,217 @@ std::string ContactPosConstr::nameEq() const return "ContactPosConstr"; } +/** + * TorqueFbTermContactPDConstr + */ + + +TorqueFbTermContactPDConstr::TorqueFbTermContactPDConstr(Eigen::Vector6d stiffness, + Eigen::Vector6d damping, + int mainRobotIndex, + const std::shared_ptr fbTerm): + ContactConstr(), + stiffness_default_(stiffness), + damping_default_(damping), + mainRobotIndex_(mainRobotIndex), + fbTerm_(fbTerm) +{} + + +void TorqueFbTermContactPDConstr::setPDgainsForContact(const ContactId& cId, const Eigen::Vector6d& stiff, + const Eigen::Vector6d& damp) +{ + if (contPD_.find(cId) == contPD_.end()) + contPD_.insert(std::make_pair(cId, PDgains(stiff, damp))); + else + contPD_.at(cId) = PDgains(stiff, damp); +} + + +void TorqueFbTermContactPDConstr::update(const std::vector& mbs, + const std::vector& mbcs, + const SolverData& data) +{ + using namespace Eigen; + + A_.block(0, 0, nrEq_, totalAlphaD_).setZero(); + b_.head(nrEq_).setZero(); + + int index = 0; + + for(std::size_t i = 0; i < cont_.size(); ++i) + { + ContactData& cd = cont_[i]; + int rows = int(cd.dof.rows()); + + Vector6d stiffness, damping; + + if (contPD_.find(cd.contactId) != contPD_.end()) + { + stiffness = contPD_.at(cd.contactId).stiffness; + damping = contPD_.at(cd.contactId).damping; + } + else + { + stiffness = stiffness_default_; + damping = damping_default_; + } + + for(std::size_t j = 0; j < cd.contacts.size(); ++j) + { + ContactSideData& csd = cd.contacts[j]; + const rbd::MultiBody& mb = mbs[csd.robotIndex]; + const rbd::MultiBodyConfig& mbc = mbcs[csd.robotIndex]; + + // AEq = J_i + sva::PTransformd X_0_p = csd.X_b_p*mbc.bodyPosW[csd.bodyIndex]; + const MatrixXd& jacMat = csd.jac.jacobian(mb, mbc, X_0_p); + dofJac_.block(0, 0, rows, csd.jac.dof()).noalias() = + csd.sign*cd.dof*jacMat; + csd.jac.fullJacobian(mb, dofJac_.block(0, 0, rows, csd.jac.dof()), + fullJac_); + A_.block(index, csd.alphaDBegin, rows, mb.nrDof()).noalias() += + fullJac_.block(0, 0, rows, mb.nrDof()); + + // BEq = dVSurf_obj - JD_i*alpha - J_i*gammaD + Vector6d normalAcc = csd.jac.normalAcceleration( + mb, mbc, data.normalAccB(csd.robotIndex), csd.X_b_p, + sva::MotionVecd(Vector6d::Zero())).vector(); + Vector6d velocity = csd.jac.velocity(mb, mbc, csd.X_b_p).vector(); + b_.segment(index, rows).noalias() -= + csd.sign*cd.dof*(normalAcc + damping.asDiagonal() * velocity); + if (csd.robotIndex == mainRobotIndex_) + { + b_.segment(index, rows).noalias() -= + csd.sign * cd.dof * fullJac_.block(0, 0, rows, mb.nrDof()) * fbTerm_->gammaD(); + } + } + + sva::PTransformd X_0_b1cf = + cd.X_b1_cf*mbcs[cd.contactId.r1Index].bodyPosW[cd.b1Index]; + sva::PTransformd X_0_b2cf = + cd.X_b1_cf*cd.X_b1_b2.inv()*mbcs[cd.contactId.r2Index].bodyPosW[cd.b2Index]; + + sva::PTransformd X_b1cf_b2cf = X_0_b2cf*X_0_b1cf.inv(); + Eigen::Vector6d error; + error.head<3>() = sva::rotationVelocity(X_b1cf_b2cf.rotation()); + error.tail<3>() = X_b1cf_b2cf.translation(); + b_.segment(index, rows) += cd.dof * stiffness.asDiagonal() * error; + + index += rows; + } +} + + +std::string TorqueFbTermContactPDConstr::nameEq() const +{ + return "TorqueFbTermContactPDConstr"; +} + + +/** + * TorqueFbTermContactHybridConstr + */ + + +TorqueFbTermContactHybridConstr::TorqueFbTermContactHybridConstr(Eigen::Vector6d proportional, + Eigen::Vector6d derivative, + Eigen::Vector6d damping, + int mainRobotIndex, + const std::shared_ptr fbTerm): + ContactConstr(), + proportional_default_(proportional), + derivative_default_(derivative), + damping_default_(damping), + mainRobotIndex_(mainRobotIndex), + fbTerm_(fbTerm) +{} + + +void TorqueFbTermContactHybridConstr::setGainsForContact(const ContactId& cId, const Eigen::Vector6d& prop, + const Eigen::Vector6d& deriv, const Eigen::Vector6d& damp) +{ + if (contGains_.find(cId) == contGains_.end()) + contGains_.insert(std::make_pair(cId, Gains(prop, deriv, damp))); + else + contGains_.at(cId) = Gains(prop, deriv, damp); +} + + +void TorqueFbTermContactHybridConstr::update(const std::vector& mbs, + const std::vector& mbcs, + const SolverData& data) +{ + using namespace Eigen; + + A_.block(0, 0, nrEq_, totalAlphaD_).setZero(); + b_.head(nrEq_).setZero(); + + int index = 0; + + for(std::size_t i = 0; i < cont_.size(); ++i) + { + ContactData& cd = cont_[i]; + int rows = int(cd.dof.rows()); + + Vector6d proportional, derivative, damping; + + if (contGains_.find(cd.contactId) != contGains_.end()) + { + proportional = contGains_.at(cd.contactId).proportional; + derivative = contGains_.at(cd.contactId).derivative; + damping = contGains_.at(cd.contactId).damping; + } + else + { + proportional = proportional_default_; + derivative = derivative_default_; + damping = damping_default_; + } + + for(std::size_t j = 0; j < cd.contacts.size(); ++j) + { + ContactSideData& csd = cd.contacts[j]; + const rbd::MultiBody& mb = mbs[csd.robotIndex]; + const rbd::MultiBodyConfig& mbc = mbcs[csd.robotIndex]; + + // AEq = J_i + sva::PTransformd X_0_p = csd.X_b_p*mbc.bodyPosW[csd.bodyIndex]; + const MatrixXd& jacMat = csd.jac.jacobian(mb, mbc, X_0_p); + dofJac_.block(0, 0, rows, csd.jac.dof()).noalias() = + csd.sign*cd.dof*jacMat; + csd.jac.fullJacobian(mb, dofJac_.block(0, 0, rows, csd.jac.dof()), + fullJac_); + A_.block(index, csd.alphaDBegin, rows, mb.nrDof()).noalias() += + fullJac_.block(0, 0, rows, mb.nrDof()); + + // BEq = dVSurf_obj - JD_i*alpha - J_i*gammaD + Vector6d normalAcc = csd.jac.normalAcceleration( + mb, mbc, data.normalAccB(csd.robotIndex), csd.X_b_p, + sva::MotionVecd(Vector6d::Zero())).vector(); + Vector6d velocity = csd.jac.velocity(mb, mbc, csd.X_b_p).vector(); + b_.segment(index, rows).noalias() -= + csd.sign*cd.dof*(normalAcc + damping.asDiagonal() * velocity); + if (csd.robotIndex == mainRobotIndex_) + { + b_.segment(index, rows).noalias() -= + csd.sign * cd.dof * fullJac_.block(0, 0, rows, mb.nrDof()) * fbTerm_->gammaD(); + } + } + + // Pending to add the admittance component + + index += rows; + } +} + + +std::string TorqueFbTermContactHybridConstr::nameEq() const +{ + return "TorqueFbTermContactHybridConstr"; +} + + } // namespace qp } // namespace tasks diff --git a/src/QPContacts.cpp b/src/QPContacts.cpp index 060a66d00..bf65dd118 100644 --- a/src/QPContacts.cpp +++ b/src/QPContacts.cpp @@ -15,6 +15,8 @@ // boost #include +#include // Added by Rafa + namespace tasks { @@ -328,8 +330,18 @@ Eigen::Vector3d BilateralContact::force(const Eigen::VectorXd & lambda, for(std::size_t i = 0; i < cones[point].generators.size(); ++i) { F += cones[point].generators[i] * lambda(i); + + if (std::isnan(F[0])) + { + std::cout << "Rafa, in BilateralContact::force, cones[point].generators[i] = " << cones[point].generators[i].transpose() << std::endl; + std::cout << "Rafa, in BilateralContact::force, lambda = " << lambda.transpose() << std::endl; + std::cout << "Rafa, in BilateralContact::force, lambda(i) = " << lambda(i) << std::endl; + //*(int*)0 = 0; + } + + // std::cout << "Rafa, in BilateralContact::force, at i = " << i << ", F += cones[point].generators[i]*lambda(i) = " << F.transpose() << std::endl; } - + return F; } diff --git a/src/QPMotionConstr.cpp b/src/QPMotionConstr.cpp index 0d313d3ab..1d77339c1 100644 --- a/src/QPMotionConstr.cpp +++ b/src/QPMotionConstr.cpp @@ -24,7 +24,7 @@ namespace qp { /** - * PositiveLambda + * PositiveLambda */ PositiveLambda::PositiveLambda() : lambdaBegin_(-1), XL_(), XU_(), cont_() {} @@ -90,7 +90,7 @@ const Eigen::VectorXd & PositiveLambda::Upper() const } /** - * MotionConstrCommon + * MotionConstrCommon */ MotionConstrCommon::ContactData::ContactData(const rbd::MultiBody & mb, @@ -111,9 +111,9 @@ MotionConstrCommon::ContactData::ContactData(const rbd::MultiBody & mb, } } -MotionConstrCommon::MotionConstrCommon(const std::vector & mbs, int robotIndex) -: robotIndex_(robotIndex), alphaDBegin_(-1), nrDof_(mbs[robotIndex_].nrDof()), lambdaBegin_(-1), fd_(mbs[robotIndex_]), - fullJacLambda_(), jacTrans_(6, nrDof_), jacLambda_(), cont_(), curTorque_(nrDof_), A_(), AL_(nrDof_), AU_(nrDof_) +MotionConstrCommon::MotionConstrCommon(const std::vector& mbs, int robotIndex, const std::shared_ptr fd) +: robotIndex_(robotIndex), alphaDBegin_(-1), nrDof_(mbs[robotIndex_].nrDof()), lambdaBegin_(-1), fd_(fd), + fullJacLambda_(), jacTrans_(6, nrDof_), jacLambda_(), cont_(), curTorque_(nrDof_), A_(), AL_(nrDof_), AU_(nrDof_) { assert(std::size_t(robotIndex_) < mbs.size() && robotIndex_ >= 0); // This is technically incorrect but practically not a huge deal, see #66 @@ -122,8 +122,8 @@ MotionConstrCommon::MotionConstrCommon(const std::vector & mbs, void MotionConstrCommon::computeTorque(const Eigen::VectorXd & alphaD, const Eigen::VectorXd & lambda) { - curTorque_ = fd_.H() * alphaD.segment(alphaDBegin_, nrDof_); - curTorque_ += fd_.C(); + curTorque_ = fd_->H() * alphaD.segment(alphaDBegin_, nrDof_); + curTorque_ += fd_->C(); curTorque_ += A_.block(0, lambdaBegin_, nrDof_, A_.cols() - lambdaBegin_) * lambda; } @@ -186,13 +186,12 @@ void MotionConstrCommon::computeMatrix(const std::vector & mbs, const rbd::MultiBody & mb = mbs[robotIndex_]; const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_]; - fd_.computeH(mb, mbc); - fd_.computeC(mb, mbc); - // tauMin -C <= H*alphaD - J^t G lambda <= tauMax - C // fill inertia matrix part - A_.block(0, alphaDBegin_, nrDof_, nrDof_) = fd_.H(); + A_.block(0, alphaDBegin_, nrDof_, nrDof_) = fd_->H(); + + // std::cout << "Rafa, in MotionConstrCommon::computeMatrix, fd_->H() is " << fd_->H().rows() << " x " << fd_->H().cols() << std::endl; for(std::size_t i = 0; i < cont_.size(); ++i) { @@ -220,8 +219,8 @@ void MotionConstrCommon::computeMatrix(const std::vector & mbs, } // BEq = -C - AL_ = -fd_.C(); - AU_ = -fd_.C(); + AL_ = -fd_->C(); + AU_ = -fd_->C(); } int MotionConstrCommon::maxGenInEq() const @@ -256,21 +255,25 @@ std::string MotionConstrCommon::descGenInEq(const std::vector & } /** - * MotionConstr + * MotionConstr */ -MotionConstr::MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb) -: MotionConstr(mbs, robotIndex, tb, {}, 0.001) +MotionConstr::MotionConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, + const TorqueBound & tb) +: MotionConstr(mbs, robotIndex, fd, tb, {}, 0.001) { } -MotionConstr::MotionConstr(const std::vector & mbs, - int robotIndex, +MotionConstr::MotionConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt) -: MotionConstrCommon(mbs, robotIndex), torqueL_(mbs[robotIndex].nrDof()), torqueU_(mbs[robotIndex].nrDof()), - torqueDtL_(mbs[robotIndex].nrDof()), torqueDtU_(mbs[robotIndex].nrDof()), tmpL_(nrDof_), tmpU_(nrDof_) +: MotionConstrCommon(mbs, robotIndex, fd), computedTorque_(mbs[robotIndex].nrDof()), + torqueL_(mbs[robotIndex].nrDof()), torqueU_(mbs[robotIndex].nrDof()), + torqueDtL_(mbs[robotIndex].nrDof()), torqueDtU_(mbs[robotIndex].nrDof()), + tmpL_(nrDof_), tmpU_(nrDof_) { rbd::paramToVector(tb.lTorqueBound, torqueL_); rbd::paramToVector(tb.uTorqueBound, torqueU_); @@ -282,6 +285,12 @@ MotionConstr::MotionConstr(const std::vector & mbs, torqueDtU_ *= dt; } +void MotionConstr::computeTorque(const Eigen::VectorXd& alphaD, const Eigen::VectorXd& lambda) +{ + MotionConstrCommon::computeTorque(alphaD, lambda); + computedTorque_ = curTorque_; +} + void MotionConstr::update(const std::vector & mbs, const std::vector & mbcs, const SolverData & /* data */) @@ -306,30 +315,30 @@ Eigen::MatrixXd MotionConstr::contactMatrix() const return A_.block(0, nrDof_, A_.rows(), A_.cols() - nrDof_); } -const rbd::ForwardDynamics MotionConstr::fd() const +const std::shared_ptr MotionConstr::fd() const { return fd_; } /** - * MotionSpringConstr + * MotionSpringConstr */ -MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, - int robotIndex, +MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const std::vector & springs) -: MotionSpringConstr(mbs, robotIndex, tb, {}, 0.001, springs) +: MotionSpringConstr(mbs, robotIndex, fd, tb, {}, 0.001, springs) { } -MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, - int robotIndex, +MotionSpringConstr::MotionSpringConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt, const std::vector & springs) -: MotionConstr(mbs, robotIndex, tb, tdb, dt), springs_() +: MotionConstr(mbs, robotIndex, fd, tb, tdb, dt), springs_() { const rbd::MultiBody & mb = mbs[robotIndex_]; for(const SpringJoint & sj : springs) @@ -368,11 +377,13 @@ void MotionSpringConstr::update(const std::vector & mbs, } /** - * MotionPolyConstr + * MotionPolyConstr */ -MotionPolyConstr::MotionPolyConstr(const std::vector & mbs, int robotIndex, const PolyTorqueBound & ptb) -: MotionConstrCommon(mbs, robotIndex), torqueL_(), torqueU_(), jointIndex_() +MotionPolyConstr::MotionPolyConstr(const std::vector& mbs, int robotIndex, + const std::shared_ptr fd, + const PolyTorqueBound& ptb) +: MotionConstrCommon(mbs, robotIndex, fd), torqueL_(), torqueU_(), jointIndex_() { const rbd::MultiBody & mb = mbs[robotIndex_]; @@ -405,6 +416,105 @@ void MotionPolyConstr::update(const std::vector & mbs, } } +/** + * MotionFrictionConstr + */ + +MotionFrictionConstr::MotionFrictionConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, + const std::shared_ptr friction, + const TorqueBound & tb) +: MotionConstr(mbs, robotIndex, fd, tb), friction_(friction), + frictionTorque_(mbs[robotIndex].nrDof()) +{ +} + +void MotionFrictionConstr::computeTorque(const Eigen::VectorXd& alphaD, const Eigen::VectorXd& lambda) +{ + MotionConstr::computeTorque(alphaD, lambda); + frictionTorque_ = friction_->friction(); + curTorque_ += frictionTorque_; +} + +void MotionFrictionConstr::update(const std::vector& mbs, + const std::vector& mbcs, + const SolverData& data) +{ + MotionConstr::update(mbs, mbcs, data); + AL_ -= friction_->friction(); + AU_ -= friction_->friction(); +} + +std::string MotionFrictionConstr::nameGenInEq() const +{ + return "MotionFrictionConstr"; +} + +/** + * TorqueFeedbackTermMotionConstr + */ + +TorqueFbTermMotionConstr::TorqueFbTermMotionConstr(const std::vector& mbs, int robotIndex, + const std::shared_ptr fd, + const std::shared_ptr fbTerm, + const TorqueBound& tb) +: MotionConstr(mbs, robotIndex, fd, tb), fbTerm_(fbTerm) +{ +} + +void TorqueFbTermMotionConstr::computeTorque(const Eigen::VectorXd& alphaD, const Eigen::VectorXd& lambda) +{ + MotionConstr::computeTorque(alphaD, lambda); + curTorque_ += fbTerm_->P(); +} + +void TorqueFbTermMotionConstr::update(const std::vector& mbs, + const std::vector& mbcs, + const SolverData& data) +{ + MotionConstr::update(mbs, mbcs, data); + AL_ -= fbTerm_->P(); + AU_ -= fbTerm_->P(); +} + +std::string TorqueFbTermMotionConstr::nameGenInEq() const +{ + return "TorqueFbTermMotionConstr"; +} + +/** + * TorqueFeedbackTermMotionFrictionConstr + */ + +TorqueFbTermMotionFrictionConstr::TorqueFbTermMotionFrictionConstr(const std::vector& mbs, int robotIndex, + const std::shared_ptr fd, + const std::shared_ptr friction, + const std::shared_ptr fbTerm, + const TorqueBound& tb) +: MotionFrictionConstr(mbs, robotIndex, fd, friction, tb), fbTerm_(fbTerm) +{ +} + +void TorqueFbTermMotionFrictionConstr::computeTorque(const Eigen::VectorXd& alphaD, const Eigen::VectorXd& lambda) +{ + MotionFrictionConstr::computeTorque(alphaD, lambda); + curTorque_ += fbTerm_->P(); +} + +void TorqueFbTermMotionFrictionConstr::update(const std::vector& mbs, + const std::vector& mbcs, + const SolverData& data) +{ + MotionFrictionConstr::update(mbs, mbcs, data); + AL_ -= fbTerm_->P(); + AU_ -= fbTerm_->P(); +} + +std::string TorqueFbTermMotionFrictionConstr::nameGenInEq() const +{ + return "TorqueFbTermMotionFrictionConstr"; +} + } // namespace qp } // namespace tasks diff --git a/src/QPSolver.cpp b/src/QPSolver.cpp index 35791b580..7d9c9d97c 100644 --- a/src/QPSolver.cpp +++ b/src/QPSolver.cpp @@ -19,6 +19,10 @@ // Tasks #include "Tasks/GenQPSolver.h" +#include "Tasks/QPMotionConstr.h" + +// included by Rafa as a test +#include "LSSOLQPSolver.h" namespace tasks { @@ -57,6 +61,22 @@ bool QPSolver::solveNoMbcUpdate(const std::vector & mbs, const s bool success = solver_->solve(); solverTimer_.stop(); + data_.lambdaVecPrev_ = lambdaVec(); + + //std::cout << "Rafa, in QPSolver::solveNoMbcUpdate, lambdaVec() = " + // << lambdaVec().transpose() << std::endl; + + //volatile bool check = data_.lambdaVecPrev_.isZero(0); + //volatile int dummy = 0; + + //if (check) { + // dummy = 1; + // std::cout << "Rafa, in QPSolver::solveNoMbcUpdate, lambdaVec() = 0" << std::endl; + //} + + // if (data_.lambdaVecPrev_.isZero(0)) + // *(int*)0 = 0; + if(!success) { solver_->errorMsg(mbs, tasks_, eqConstr_, inEqConstr_, genInEqConstr_, boundConstr_, std::cerr) << std::endl; @@ -83,6 +103,8 @@ void QPSolver::updateConstrSize() maxInEqLines_ = std::accumulate(inEqConstr_.begin(), inEqConstr_.end(), 0, accumMaxLines); maxGenInEqLines_ = std::accumulate(genInEqConstr_.begin(), genInEqConstr_.end(), 0, accumMaxLines); + std::cout << "Rafa, in tasks::qp::QPSolver::updateConstrSize, maxEqLines_ = " << maxEqLines_ << ", maxInEqLines_ = " << maxInEqLines_ << ", maxGenInEqLines_ = " << maxGenInEqLines_ << std::endl; + solver_->updateSize(data_.nrVars_, maxEqLines_, maxInEqLines_, maxGenInEqLines_); } @@ -210,6 +232,29 @@ void QPSolver::updateNrVars(const std::vector & mbs) const updateConstrsNrVars(mbs); } +bool QPSolver::hasConstraint(const Constraint* co) +{ + return std::find(constr_.begin(), constr_.end(), co) != constr_.end(); +} + +/* +std::shared_ptr QPSolver::getMotionConstr() +{ + std::shared_ptr motionConstr = NULL; + + for(Constraint* c: constr_) + { + std::shared_ptr c_prime = std::make_shared().reset(c); + motionConstr = std::dynamic_pointer_cast(c_prime); + + if (motionConstr) + { + break; + } + } +} +*/ + void QPSolver::addEqualityConstraint(Equality * co) { eqConstr_.push_back(co); @@ -383,6 +428,47 @@ Eigen::VectorXd QPSolver::alphaDVec(int rIndex) const Eigen::VectorXd QPSolver::lambdaVec() const { + // Rafa added this + bool nantest = false; + bool zerotest = false; + //for (int i = 0; i < solver_->result().size(); i++) { + // nantest |= std::isnan(solver_->result()[i]); + //} + + nantest = (isnan(solver_->result().array())).count() > 0; + //zerotest = solver_->result().segment(data_.lambdaBegin(), data_.totalLambda_).isZero(0); + + // std::cout << "Rafa, in QPSolver::lambdaVec, solver_->result() = " << solver_->result().transpose() << std::endl; + + if (nantest || zerotest) + { + if (zerotest) + std::cout << "Rafa, in QPSolver::lambdaVec, zerotest" << std::endl; + + std::cout << "Rafa, in QPSolver::lambdaVec, solver_->result() = " << solver_->result().transpose() << std::endl; + + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->AFull_.size() = " << static_cast(solver_.get())->AFull_.size() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->AL_.size() = " << static_cast(solver_.get())->AL_.size() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->AU_.size() = " << static_cast(solver_.get())->AU_.size() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->XLFull_.size() = " << static_cast(solver_.get())->XLFull_.size() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->XUFull_.size() = " << static_cast(solver_.get())->XUFull_.size() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->QFull_.size() = " << static_cast(solver_.get())->QFull_.size() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->CFull_.size() = " << static_cast(solver_.get())->CFull_.size() << std::endl; + + /* + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->AFull_ = " << static_cast(solver_.get())->AFull_ << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->AL_ = " << static_cast(solver_.get())->AL_.transpose() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->AU_ = " << static_cast(solver_.get())->AU_.transpose() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->XLFull_ = " << static_cast(solver_.get())->XLFull_.transpose() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->XUFull_ = " << static_cast(solver_.get())->XUFull_.transpose() << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->QFull_ = " << static_cast(solver_.get())->QFull_ << std::endl; + std::cout << "Rafa, in QPSolver::lambdaVec, static_cast(solver_.get())->CFull_ = " << static_cast(solver_.get())->CFull_.transpose() << std::endl; + */ + + if (nantest) + *(int*)0 = 0; + } + return solver_->result().segment(data_.lambdaBegin(), data_.totalLambda_); } @@ -453,6 +539,69 @@ void QPSolver::postUpdate(const std::vector & /* mbs */, } } +/** + * PassivityPIDTerm_QPSolver + */ + +PassivityPIDTerm_QPSolver::PassivityPIDTerm_QPSolver() +: QPSolver() +{ +} + +bool PassivityPIDTerm_QPSolver::solve(const std::vector & mbs, + std::vector & mbcs_real, + std::vector & mbcs_calc) +{ + bool success = solveNoMbcUpdate(mbs, mbcs_real, mbcs_calc); + + postUpdate(mbs, mbcs_real, success); + postUpdate(mbs, mbcs_calc, success); + + return success; +} + +bool PassivityPIDTerm_QPSolver::solveNoMbcUpdate(const std::vector & mbs, + const std::vector & mbcs_real, + const std::vector & mbcs_calc) +{ + solverAndBuildTimer_.start(); + preUpdate(mbs, mbcs_real, mbcs_calc); + + solverTimer_.start(); + bool success = solver_->solve(); + solverTimer_.stop(); + + data_.lambdaVecPrev_ = lambdaVec(); + + if(!success) + { + solver_->errorMsg(mbs, tasks_, eqConstr_, inEqConstr_, genInEqConstr_, boundConstr_, std::cerr) << std::endl; + } + solverAndBuildTimer_.stop(); + + return success; +} + +void PassivityPIDTerm_QPSolver::preUpdate(const std::vector& mbs, + const std::vector& mbcs_real, + const std::vector& mbcs_calc) +{ + data_.computeNormalAccB(mbs, mbcs_real); + for(std::size_t i = 0; i < constr_.size(); ++i) + { + constr_[i]->update(mbs, mbcs_real, data_); + } + + data_.computeNormalAccB(mbs, mbcs_calc); + for(std::size_t i = 0; i < tasks_.size(); ++i) + { + tasks_[i]->update(mbs, mbcs_calc, data_); + } + + solver_->updateMatrix(tasks_, eqConstr_, inEqConstr_, genInEqConstr_, boundConstr_); +} + + } // namespace qp } // namespace tasks diff --git a/src/QPSolverData.cpp b/src/QPSolverData.cpp index 4d335ddf1..fbc57ed82 100644 --- a/src/QPSolverData.cpp +++ b/src/QPSolverData.cpp @@ -18,7 +18,7 @@ namespace qp SolverData::SolverData() : alphaD_(), alphaDBegin_(), lambda_(), totalAlphaD_(0), totalLambda_(0), nrUniLambda_(0), nrBiLambda_(0), nrVars_(0), - uniCont_(), biCont_(), allCont_(), mobileRobotIndex_(), normalAccB_() + uniCont_(), biCont_(), allCont_(), mobileRobotIndex_(), normalAccB_(), lambdaVecPrev_() { } @@ -49,6 +49,7 @@ void SolverData::computeNormalAccB(const std::vector & mbs, } } + } // namespace qp } // namespace tasks diff --git a/src/QPTasks.cpp b/src/QPTasks.cpp index f8f562ea4..5b4a260b1 100644 --- a/src/QPTasks.cpp +++ b/src/QPTasks.cpp @@ -10,6 +10,7 @@ #include #include #include +#include ///added for Rafael code, so do not revert // Eigen #include @@ -28,7 +29,7 @@ namespace qp { /** - * SetPointTaskCommon + * SetPointTaskCommon */ SetPointTaskCommon::SetPointTaskCommon(const std::vector & mbs, @@ -85,7 +86,7 @@ const Eigen::VectorXd & SetPointTaskCommon::C() const } /** - * SetPointTask + * SetPointTask */ SetPointTask::SetPointTask(const std::vector & mbs, @@ -129,7 +130,7 @@ void SetPointTask::update(const std::vector & mbs, } /** - * TrackingTask + * TrackingTask */ TrackingTask::TrackingTask(const std::vector & mbs, @@ -191,7 +192,7 @@ void TrackingTask::update(const std::vector & mbs, } /** - * TrajectoryTask + * TrajectoryTask */ TrajectoryTask::TrajectoryTask(const std::vector & mbs, @@ -301,7 +302,7 @@ void TrajectoryTask::update(const std::vector & mbs, } /** - * PIDTask + * PIDTask */ PIDTask::PIDTask(const std::vector & mbs, @@ -388,7 +389,7 @@ void PIDTask::update(const std::vector & mbs, } /** - * TargetObjectiveTask + * TargetObjectiveTask */ TargetObjectiveTask::TargetObjectiveTask(const std::vector & mbs, @@ -511,7 +512,7 @@ const Eigen::VectorXd & TargetObjectiveTask::C() const } /** - * JointsSelector + * JointsSelector */ JointsSelector JointsSelector::ActiveJoints(const std::vector & mbs, @@ -641,62 +642,64 @@ const Eigen::VectorXd & JointsSelector::normalAcc() } /** Torque Task **/ -TorqueTask::TorqueTask(const std::vector & mbs, int robotIndex, const TorqueBound & tb, double weight) -: TorqueTask(mbs, robotIndex, tb, TorqueDBound{}, 0, weight) +TorqueTask::TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, + const TorqueBound & tb, double weight) +: TorqueTask(mbs, robotIndex, fd, tb, TorqueDBound{}, 0, weight) { } -TorqueTask::TorqueTask(const std::vector & mbs, - int robotIndex, +TorqueTask::TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const Eigen::VectorXd & jointSelect, double weight) -: TorqueTask(mbs, robotIndex, tb, TorqueDBound{}, 0, jointSelect, weight) +: TorqueTask(mbs, robotIndex, fd, tb, TorqueDBound{}, 0, jointSelect, weight) { } -TorqueTask::TorqueTask(const std::vector & mbs, - int robotIndex, +TorqueTask::TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const std::string & efName, double weight) -: TorqueTask(mbs, robotIndex, tb, TorqueDBound{}, 0, efName, weight) +: TorqueTask(mbs, robotIndex, fd, tb, TorqueDBound{}, 0, efName, weight) { } -TorqueTask::TorqueTask(const std::vector & mbs, - int robotIndex, +TorqueTask::TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt, double weight) -: Task(weight), robotIndex_(robotIndex), alphaDBegin_(-1), lambdaBegin_(-1), motionConstr(mbs, robotIndex, tb, tdb, dt), +: Task(weight), robotIndex_(robotIndex), alphaDBegin_(-1), lambdaBegin_(-1), motionConstr(mbs, robotIndex, fd, tb, tdb, dt), jointSelector_(mbs[robotIndex].nrDof()), Q_(mbs[robotIndex].nrDof(), mbs[robotIndex].nrDof()), C_(mbs[robotIndex].nrDof()) { jointSelector_.setOnes(); } -TorqueTask::TorqueTask(const std::vector & mbs, - int robotIndex, +TorqueTask::TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt, const Eigen::VectorXd & jointSelect, double weight) -: Task(weight), robotIndex_(robotIndex), alphaDBegin_(-1), lambdaBegin_(-1), motionConstr(mbs, robotIndex, tb, tdb, dt), +: Task(weight), robotIndex_(robotIndex), alphaDBegin_(-1), lambdaBegin_(-1), motionConstr(mbs, robotIndex, fd, tb, tdb, dt), jointSelector_(jointSelect), Q_(mbs[robotIndex].nrDof(), mbs[robotIndex].nrDof()), C_(mbs[robotIndex].nrDof()) { } -TorqueTask::TorqueTask(const std::vector & mbs, - int robotIndex, +TorqueTask::TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt, const std::string & efName, double weight) -: Task(weight), robotIndex_(robotIndex), alphaDBegin_(-1), lambdaBegin_(-1), motionConstr(mbs, robotIndex, tb, tdb, dt), +: Task(weight), robotIndex_(robotIndex), alphaDBegin_(-1), lambdaBegin_(-1), motionConstr(mbs, robotIndex, fd, tb, tdb, dt), jointSelector_(mbs[robotIndex].nrDof()), Q_(mbs[robotIndex].nrDof(), mbs[robotIndex].nrDof()), C_(mbs[robotIndex].nrDof()) { @@ -727,12 +730,12 @@ void TorqueTask::update(const std::vector & mbs, { motionConstr.update(mbs, mbcs, data); Q_.noalias() = motionConstr.matrix().transpose() * jointSelector_.asDiagonal() * motionConstr.matrix(); - C_.noalias() = motionConstr.fd().C().transpose() * jointSelector_.asDiagonal() * motionConstr.matrix(); + C_.noalias() = motionConstr.fd()->C().transpose() * jointSelector_.asDiagonal() * motionConstr.matrix(); // C_.setZero(); } /** - * PostureTask + * PostureTask */ PostureTask::PostureTask(const std::vector & mbs, @@ -845,7 +848,7 @@ const Eigen::VectorXd & PostureTask::eval() const } /** - * PositionTask + * PositionTask */ PositionTask::PositionTask(const std::vector & mbs, @@ -890,7 +893,7 @@ const Eigen::VectorXd & PositionTask::normalAcc() } /** - * OrientationTask + * OrientationTask */ OrientationTask::OrientationTask(const std::vector & mbs, @@ -942,7 +945,7 @@ const Eigen::VectorXd & OrientationTask::normalAcc() } /** - * SurfaceTransformTask + * SurfaceTransformTask */ SurfaceTransformTask::SurfaceTransformTask(const std::vector & mbs, @@ -962,7 +965,7 @@ void SurfaceTransformTask::update(const std::vector & mbs, } /** - * TransformTask + * TransformTask */ TransformTask::TransformTask(const std::vector & mbs, @@ -994,7 +997,7 @@ void TransformTask::update(const std::vector & mbs, } /** - * SurfaceOrientationTask + * SurfaceOrientationTask */ SurfaceOrientationTask::SurfaceOrientationTask(const std::vector & mbs, @@ -1048,7 +1051,7 @@ const Eigen::VectorXd & SurfaceOrientationTask::normalAcc() } /** - * GazeTask + * GazeTask */ GazeTask::GazeTask(const std::vector & mbs, @@ -1105,7 +1108,7 @@ const Eigen::VectorXd & GazeTask::normalAcc() } /** - * PositionBasedVisServoTask + * PositionBasedVisServoTask */ PositionBasedVisServoTask::PositionBasedVisServoTask(const std::vector & mbs, @@ -1150,7 +1153,7 @@ const Eigen::VectorXd & PositionBasedVisServoTask::normalAcc() } /** - * CoMTask + * CoMTask */ CoMTask::CoMTask(const std::vector & mbs, int rI, const Eigen::Vector3d & com) @@ -1203,9 +1206,9 @@ const Eigen::VectorXd & CoMTask::normalAcc() { return ct_.normalAcc(); } - + /** - * MultiCoMTask + * MultiCoMTask */ MultiCoMTask::MultiCoMTask(const std::vector & mbs, @@ -1318,7 +1321,7 @@ void MultiCoMTask::init(const std::vector & mbs) } /** - * MultiRobotTransformTask + * MultiRobotTransformTask */ MultiRobotTransformTask::MultiRobotTransformTask(const std::vector & mbs, @@ -1448,7 +1451,7 @@ const Eigen::VectorXd & MultiRobotTransformTask::speed() const } /** - * MomentumTask + * MomentumTask */ MomentumTask::MomentumTask(const std::vector & mbs, int rI, const sva::ForceVecd & mom) @@ -1489,7 +1492,52 @@ const Eigen::VectorXd & MomentumTask::normalAcc() } /** - * ContactTask + * CentroidalAngularMomentumTask + */ + +CentroidalAngularMomentumTask::CentroidalAngularMomentumTask(const std::vector& mbs, int robotIndex, + double gain, const Eigen::Vector3d angMomentum, double weight) +: Task(weight), robotIndex_(robotIndex), angMomentum_(angMomentum), gain_(gain), alphaDBegin_(-1), + dimWeight_(Eigen::Vector3d::Ones()), centroidalMomentumMatrix_(mbs[robotIndex]), + Q_(mbs[robotIndex].nrDof(), mbs[robotIndex].nrDof()), C_(mbs[robotIndex].nrDof()), + jacMat_(3, mbs[robotIndex].nrDof()), preQ_(3, mbs[robotIndex].nrDof()), + CSum_(Eigen::Vector3d::Zero()), normalAcc_(Eigen::Vector3d::Zero()) +{ +} + +void CentroidalAngularMomentumTask::updateNrVars(const std::vector& /* mbs */, + const SolverData& data) +{ + alphaDBegin_ = data.alphaDBegin(robotIndex_); +} + +void CentroidalAngularMomentumTask::update(const std::vector& mbs, + const std::vector& mbcs, + const SolverData& data) +{ + const rbd::MultiBody & mb = mbs[robotIndex_]; + const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_]; + + Eigen::Vector3d com = rbd::computeCoM(mb, mbc); + Eigen::Vector3d dcom = rbd::computeCoMVelocity(mb, mbc); + + centroidalMomentumMatrix_.computeMatrix(mb, mbc, com); + normalAcc_ = centroidalMomentumMatrix_.normalMomentumDot(mb, mbc, com, dcom).couple(); + + CSum_ = Eigen::Vector3d::Zero(); + if (gain_) + CSum_ += gain_ * (angMomentum_ - rbd::computeCentroidalMomentum(mb, mbc, com).couple()); + CSum_ -= normalAcc_; + + jacMat_ = centroidalMomentumMatrix_.matrix().topRows<3>(); + preQ_.noalias() = dimWeight_.asDiagonal() * jacMat_; + + Q_.noalias() = jacMat_.transpose() * preQ_; + C_.noalias() = -jacMat_.transpose() * dimWeight_.asDiagonal() * CSum_; +} + +/** + * ContactTask */ void ContactTask::error(const Eigen::Vector3d & error) @@ -1561,7 +1609,7 @@ const Eigen::VectorXd & ContactTask::C() const } /** - * GripperTorqueTask + * GripperTorqueTask */ void GripperTorqueTask::updateNrVars(const std::vector & /* mbs */, const SolverData & data) @@ -1632,7 +1680,7 @@ const Eigen::VectorXd & GripperTorqueTask::C() const } /** - * LinVelocityTask + * LinVelocityTask */ LinVelocityTask::LinVelocityTask(const std::vector & mbs, @@ -1677,7 +1725,7 @@ const Eigen::VectorXd & LinVelocityTask::normalAcc() } /** - * OrientationTrackingTask + * OrientationTrackingTask */ OrientationTrackingTask::OrientationTrackingTask(const std::vector & mbs, @@ -1729,7 +1777,7 @@ const Eigen::VectorXd & OrientationTrackingTask::normalAcc() } /** - * RelativeDistTask + * RelativeDistTask */ RelativeDistTask::RelativeDistTask(const std::vector & mbs, @@ -1776,7 +1824,7 @@ const Eigen::VectorXd & RelativeDistTask::normalAcc() } /** - * VectorOrientationTask + * VectorOrientationTask */ VectorOrientationTask::VectorOrientationTask(const std::vector & mbs, @@ -1820,6 +1868,1145 @@ const Eigen::VectorXd & VectorOrientationTask::normalAcc() return vot_.normalAcc(); } +/** + * WrenchTask + */ + +WrenchTask::WrenchTask(const std::vector & mbs, int robotIndex, const std::string & bodyName, + const Eigen::Vector3d & bodyPoint, double weight) +: Task(weight), robotIndex_(robotIndex), bodyIndex_(mbs[robotIndex].bodyIndexByName(bodyName)), lambdaBegin_(-1), + dimWeight_(Eigen::Vector6d::Ones()), bodyPoint_(bodyPoint), wrench_(Eigen::Vector6d::Zero()), local_(true), + preC_(Eigen::Vector6d::Zero()) +{} + +void WrenchTask::wrench(const std::vector & mbcs, const sva::ForceVecd & wrench) +{ + if (local_) + { + Eigen::Matrix3d RBody = mbcs[robotIndex_].bodyPosW[bodyIndex_].rotation().transpose(); + + wrench_ = sva::ForceVecd(RBody * wrench.couple(), RBody * wrench.force()); + } + else + { + wrench_ = wrench; + } +} + +void WrenchTask::dimWeight(const std::vector & mbcs, const Eigen::Vector6d & dim) +{ + if (local_) + { + Eigen::Matrix3d RBody = mbcs[robotIndex_].bodyPosW[bodyIndex_].rotation().transpose(); + dimWeight_.head(3).noalias() = RBody * dim.head(3); + dimWeight_.tail(3).noalias() = RBody * dim.tail(3); + } + else + { + dimWeight_ = dim; + } +} + +void WrenchTask::updateNrVars(const std::vector & /* mbs */, + const SolverData & data) +{ + lambdaBegin_ = data.lambdaBegin(); + int nrLambda = data.totalLambda(); + + W_.setZero(6, nrLambda); + + Q_.setZero(nrLambda, nrLambda); + C_.setZero(nrLambda); + + preQ_.setZero(6, nrLambda); +} + +void WrenchTask::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + int index = 0; + + for (const BilateralContact & contact : data.allContacts()) + { + int r1BodyIndex = mbs[contact.contactId.r1Index].bodyIndexByName(contact.contactId.r1BodyName); + + // std::cout << "Rafa, in WrenchTask::update, contact.contactId.r1BodyName = " << contact.contactId.r1BodyName << std::endl; + + if (contact.contactId.r1Index == robotIndex_ && r1BodyIndex == bodyIndex_) + { + for (size_t i = 0; i < contact.r1Cones.size(); i++) + { + const FrictionCone & cone = contact.r1Cones[i]; + + for (const Eigen::Vector3d & gen : cone.generators) + { + W_.col(index).head<3>().noalias() = (mbcs[robotIndex_].bodyPosW[bodyIndex_].rotation().transpose() * (contact.r1Points[i] - bodyPoint_)).cross(gen); + W_.col(index).tail<3>() = gen; + index++; + } + } + } + else + { + index += contact.nrLambda(); + } + } + + // std::cout << "Rafa, in WrenchTask::update, wrench_.vector() = " << wrench_.vector().transpose() << std::endl; + // std::cout << "Rafa, in WrenchTask::update, W_ = " << std::endl << W_ << std::endl; + + preC_.noalias() = dimWeight_.asDiagonal() * wrench_.vector(); + C_.noalias() = -W_.transpose() * preC_; + + preQ_.noalias() = dimWeight_.asDiagonal() * W_; + Q_.noalias() = W_.transpose() * preQ_; +} + +/** + * LocalCoPTask + */ + +LocalCoPTask::LocalCoPTask(const std::vector & mbs, int robotIndex, + const std::string & bodyName, const Eigen::Vector3d & localCoP, + double weight) +: Task(weight), robotIndex_(robotIndex), bodyIndex_(mbs[robotIndex].bodyIndexByName(bodyName)), + lambdaBegin_(-1), dimWeight_(Eigen::Vector3d::Ones()), localCoP_(localCoP) +{} + +void LocalCoPTask::dimWeight(const std::vector & mbcs, const Eigen::Vector3d & dim) +{ + Eigen::Matrix3d RBody = mbcs[robotIndex_].bodyPosW[bodyIndex_].rotation().transpose(); + dimWeight_.noalias() = RBody * dim; +} + +void LocalCoPTask::updateNrVars(const std::vector & /* mbs */, + const SolverData & data) +{ + lambdaBegin_ = data.lambdaBegin(); + int nrLambda = data.totalLambda(); + + W_.setZero(3, nrLambda); + + Q_.setZero(nrLambda, nrLambda); + C_.setZero(nrLambda); + + preQ_.setZero(3, nrLambda); +} + +void LocalCoPTask::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + int index = 0; + + for (const BilateralContact & contact : data.allContacts()) + { + int r1BodyIndex = mbs[contact.contactId.r1Index].bodyIndexByName(contact.contactId.r1BodyName); + + if (contact.contactId.r1Index == robotIndex_ && r1BodyIndex == bodyIndex_) + { + for (size_t i = 0; i < contact.r1Cones.size(); i++) + { + const FrictionCone & cone = contact.r1Cones[i]; + + for (const Eigen::Vector3d & gen : cone.generators) + { + W_.col(index).noalias() = (mbcs[robotIndex_].bodyPosW[bodyIndex_].rotation().transpose() * (contact.r1Points[i] - localCoP_)).cross(gen); + index++; + } + } + } + else + { + index += contact.nrLambda(); + } + } + + // std::cout << "Rafa, in LocalCoPTask::update, W_ = " << std::endl << W_ << std::endl; + + preQ_.noalias() = dimWeight_.asDiagonal() * W_; + Q_.noalias() = W_.transpose() * preQ_; +} + +/** + * AdmittanceTaskCommon + */ + +AdmittanceTaskCommon::AdmittanceTaskCommon(const std::vector & mbs, int robotIndex, + const std::string & bodyName, + const Eigen::Vector3d & bodyPoint, + double timeStep, double gainForceP, double gainForceD, + double gainCoupleP, double gainCoupleD, double weight) +: Task(weight), robotIndex_(robotIndex), bodyIndex_(mbs[robotIndex].bodyIndexByName(bodyName)), + alphaDBegin_(-1), contactBodies_(), contactBodiesPrev_(), dt_(timeStep), + gainForceP_(gainForceP), gainForceD_(gainForceD), + gainCoupleP_(gainCoupleP), gainCoupleD_(gainCoupleD), + jac_(mbs[robotIndex], bodyName, bodyPoint), jacMat_(6, mbs[robotIndex].nrDof()), + error_(Eigen::Vector6d::Zero()), normalAcc_(Eigen::Vector6d::Zero()), + dimWeight_(Eigen::Vector6d::Ones()), local_(true), + Q_(mbs[robotIndex].nrDof(), mbs[robotIndex].nrDof()), C_(mbs[robotIndex].nrDof()), + preQ_(6, mbs[robotIndex].nrDof()), preC_(6) +{ +} + +void AdmittanceTaskCommon::dimWeight(const std::vector & mbcs, + const Eigen::Vector6d & dim) +{ + if (local_) + { + Eigen::Matrix3d RBody = mbcs[robotIndex_].bodyPosW[bodyIndex_].rotation().transpose(); + + dimWeight_.head(3).noalias() = RBody * dim.head(3); + dimWeight_.tail(3).noalias() = RBody * dim.tail(3); + } + else + { + dimWeight_ = dim; + } +} + +void AdmittanceTaskCommon::updateNrVars(const std::vector & /* mbs */, + const SolverData & data) +{ + alphaDBegin_ = data.alphaDBegin(robotIndex_); + + contactBodies_.clear(); + + int eachBeginIndex = 0; + for (const BilateralContact & contact : data.allContacts()) + { + if (contact.contactId.r1Index == robotIndex_) + { + int nrEachLambda = 0; + for (size_t i = 0; i < contact.r1Cones.size(); i++) + nrEachLambda += contact.r1Cones[i].generators.size(); + + BodyLambda contactBody; + contactBody.body = contact.contactId.r1BodyName; + contactBody.begin = eachBeginIndex; + contactBody.size = nrEachLambda; + + contactBodies_.push_back(contactBody); + + eachBeginIndex += nrEachLambda; + } + } + + if (contactBodiesPrev_.size() == 0) + contactBodiesPrev_ = contactBodies_; +} + +sva::ForceVecd AdmittanceTaskCommon::computeWrench(const rbd::MultiBodyConfig & mbc, + const tasks::qp::BilateralContact & contact, + Eigen::VectorXd lambdaVec, int pos) +{ + sva::ForceVecd wrench(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + + for (size_t i = 0; i < contact.r1Points.size(); i++) + { + Eigen::VectorXd lambda = Eigen::VectorXd::Zero(contact.nrLambda(i)); + + if (lambdaVec.size() > 0 && pos < lambdaVec.size()) + lambda = lambdaVec.segment(pos, contact.nrLambda(i)); + + Eigen::Matrix3d RBody = mbc.bodyPosW[bodyIndex_].rotation().transpose(); + + wrench.force() += contact.force(lambda, i, contact.r1Cones); + wrench.couple() += (RBody * contact.r1Points[i]).cross(contact.force(lambda, i, contact.r1Cones)); + + pos += contact.nrLambda(i); + } + + return wrench; +} + +/** + * AdmittanceTask + */ + +AdmittanceTask::AdmittanceTask(const std::vector & mbs, int robotIndex, + const std::string & bodyName, + const Eigen::Vector3d & bodyPoint, + double timeStep, double gainForceP, double gainForceD, + double gainCoupleP, double gainCoupleD, double weight) +: AdmittanceTaskCommon(mbs, robotIndex, bodyName, bodyPoint, timeStep, + gainForceP, gainForceD, gainCoupleP, gainCoupleD, weight), + measuredWrench_(sva::ForceVecd::Zero()), measuredWrenchPrev_(sva::ForceVecd::Zero()), + calculatedWrench_(sva::ForceVecd::Zero()), calculatedWrenchPrev_(sva::ForceVecd::Zero()) +{ +} + +void AdmittanceTask::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + const rbd::MultiBody& mb = mbs[robotIndex_]; + const rbd::MultiBodyConfig& mbc = mbcs[robotIndex_]; + const std::vector& normalAccB = data.normalAccB(robotIndex_); + + normalAcc_.head(3) = jac_.normalAcceleration(mb, mbc, normalAccB).angular(); + normalAcc_.tail(3) = jac_.normalAcceleration(mb, mbc, normalAccB).linear(); + + jac_.fullJacobian(mb, jac_.jacobian(mb, mbc), jacMat_); + + Eigen::VectorXd lambdaVecPrev = Eigen::VectorXd::Zero(data.totalLambda()); + + // Assuming that contacts are not set and released simultaneously + if (contactBodies_.size() == contactBodiesPrev_.size()) + + lambdaVecPrev = data.lambdaVecPrev(); + + else { + + for (size_t i = 0; i < contactBodies_.size(); i++) { + + size_t j; + for (j = 0; j < contactBodiesPrev_.size(); j++) + if (contactBodies_[i].body == contactBodiesPrev_[j].body) + break; + + if (j < contactBodiesPrev_.size()) + lambdaVecPrev.segment(contactBodies_[i].begin, contactBodies_[i].size) = data.lambdaVecPrev().segment(contactBodiesPrev_[j].begin, contactBodiesPrev_[j].size); + } + } + + contactBodiesPrev_ = contactBodies_; + + calculatedWrench_ = sva::ForceVecd::Zero(); + + for (size_t ci = 0; ci < data.allContacts().size(); ci++) + { + const tasks::qp::BilateralContact& contact = data.allContacts()[ci]; + + int r1BodyIndex = mbs[contact.contactId.r1Index].bodyIndexByName(contact.contactId.r1BodyName); + + if (contact.contactId.r1Index == robotIndex_ && r1BodyIndex == bodyIndex_) + //calculatedWrench_ += computeWrench(mbc, contact, data.lambdaVecPrev(), + // data.lambdaBegin(ci) - data.lambdaBegin()); + calculatedWrench_ += computeWrench(mbc, contact, lambdaVecPrev, + data.lambdaBegin(ci) - data.lambdaBegin()); + } + + sva::ForceVecd calculatedWrenchDot = (calculatedWrench_ - calculatedWrenchPrev_) / dt_; + calculatedWrenchPrev_ = calculatedWrench_; + + sva::ForceVecd measuredWrenchDot = (measuredWrench_ - measuredWrenchPrev_) / dt_; + measuredWrenchPrev_ = measuredWrench_; + + Eigen::Matrix6d gainP = Eigen::Matrix6d::Zero(); + gainP.block(0, 0, 3, 3) = gainCoupleP_ * Eigen::Matrix3d::Identity(); + gainP.block(3, 3, 3, 3) = gainForceP_ * Eigen::Matrix3d::Identity(); + + Eigen::Matrix6d gainD = Eigen::Matrix6d::Zero(); + gainD.block(0, 0, 3, 3) = gainCoupleD_ * Eigen::Matrix3d::Identity(); + gainD.block(3, 3, 3, 3) = gainForceD_ * Eigen::Matrix3d::Identity(); + + error_.noalias() = -gainP * (calculatedWrench_.vector() - measuredWrench_.vector()); + error_.noalias() += -gainD * (calculatedWrenchDot.vector() - measuredWrenchDot.vector()); + // The minus sign multiplying the gains is set to get the wrench applied to the environment (important) + + error_.noalias() -= normalAcc_; + + preC_.noalias() = dimWeight_.asDiagonal() * error_; + C_.noalias() = -jacMat_.transpose() * preC_; + + preQ_.noalias() = dimWeight_.asDiagonal() * jacMat_; + Q_.noalias() = jacMat_.transpose() * preQ_; +} + +/** + * NullSpaceAdmittanceTask + */ + +NullSpaceAdmittanceTask::NullSpaceAdmittanceTask(const std::vector & mbs, int robotIndex, + const std::string & bodyName, + const Eigen::Vector3d & bodyPoint, + double timeStep, double gainForceP, double gainForceD, + double gainCoupleP, double gainCoupleD, double weight) +: AdmittanceTaskCommon(mbs, robotIndex, bodyName, bodyPoint, timeStep, + gainForceP, gainForceD, gainCoupleP, gainCoupleD, weight), + nrBodies_(-1), + calculatedBodyWrench_(sva::ForceVecd::Zero()), + measuredBodyWrenchPrev_(sva::ForceVecd::Zero()), + calculatedBodyWrenchPrev_(sva::ForceVecd::Zero()), + fdistRatio_(Eigen::Vector3d::Zero()), + projForceErr_(Eigen::Vector3d::Zero()) +{ +} + +void NullSpaceAdmittanceTask::measuredWrench(const std::string & bodyName, const sva::ForceVecd & wrench) +{ + std::map::iterator measuredWrench = measuredWrenches_.find(bodyName); + if (measuredWrench != measuredWrenches_.end()) + measuredWrenches_.at(bodyName) = wrench; +} + +void NullSpaceAdmittanceTask::measuredWrenches(const std::map & wrenches) +{ + /* + for (const std::pair & wrench : wrenches) + { + std::map::iterator measuredWrench = measuredWrenches_.find(wrench.first); + if (measuredWrench != measuredWrenches_.end()) + measuredWrenches_.at(wrench.first) = wrench.second; + } + */ + measuredWrenches_ = wrenches; +} + +void NullSpaceAdmittanceTask::updateNrVars(const std::vector & mbs, + const SolverData& data) +{ + AdmittanceTaskCommon::updateNrVars(mbs, data); + + // bodies_.clear(); + std::map measuredWrenches_tmp; + std::map calculatedForces_tmp; + + for (const BilateralContact & contact : data.allContacts()) + { + if (contact.contactId.r1Index == robotIndex_) + { + const std::string & r1BodyName = contact.contactId.r1BodyName; + // bodies_.insert(r1BodyName); + + /* + std::map::iterator measuredWrench = measuredWrenches_.find(r1BodyName); + if (measuredWrench != measuredWrenches_.end()) + { + measuredWrenches_tmp[r1BodyName] = measuredWrenches_[r1BodyName]; + } + else + { + measuredWrenches_tmp[r1BodyName] = sva::ForceVecd::Zero(); + } + */ + + std::map::iterator calculatedForce = calculatedForces_.find(r1BodyName); + if (calculatedForce != calculatedForces_.end()) + { + calculatedForces_tmp[r1BodyName] = calculatedForces_[r1BodyName]; + } + else + { + calculatedForces_tmp[r1BodyName] = Eigen::Vector3d::Zero(); + } + } + } + + //measuredWrenches_ = measuredWrenches_tmp; + calculatedForces_ = calculatedForces_tmp; +} + +void NullSpaceAdmittanceTask::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + const rbd::MultiBody& mb = mbs[robotIndex_]; + const rbd::MultiBodyConfig& mbc = mbcs[robotIndex_]; + const std::vector& normalAccB = data.normalAccB(robotIndex_); + + normalAcc_.head(3) = jac_.normalAcceleration(mb, mbc, normalAccB).angular(); + normalAcc_.tail(3) = jac_.normalAcceleration(mb, mbc, normalAccB).linear(); + + jac_.fullJacobian(mb, jac_.jacobian(mb, mbc), jacMat_); + + Eigen::VectorXd lambdaVecPrev = Eigen::VectorXd::Zero(data.totalLambda()); + + // Assuming that contacts are not set and released simultaneously + if (contactBodies_.size() == contactBodiesPrev_.size()) + + lambdaVecPrev = data.lambdaVecPrev(); + + else { + + for (size_t i = 0; i < contactBodies_.size(); i++) { + + size_t j; + for (j = 0; j < contactBodiesPrev_.size(); j++) + if (contactBodies_[i].body == contactBodiesPrev_[j].body) + break; + + if (j < contactBodiesPrev_.size()) + lambdaVecPrev.segment(contactBodies_[i].begin, contactBodies_[i].size) = data.lambdaVecPrev().segment(contactBodiesPrev_[j].begin, contactBodiesPrev_[j].size); + } + } + + contactBodiesPrev_ = contactBodies_; + + calculatedBodyWrench_ = sva::ForceVecd::Zero(); + + for (const std::pair & calculatedForce : calculatedForces_) + calculatedForces_.at(calculatedForce.first) = Eigen::Vector3d::Zero(); + + for (size_t ci = 0; ci < data.allContacts().size(); ci++) + { + const tasks::qp::BilateralContact& contact = data.allContacts()[ci]; + + if (contact.contactId.r1Index == robotIndex_) + { + const std::string & r1BodyName = contact.contactId.r1BodyName; + int r1BodyIndex = mbs[robotIndex_].bodyIndexByName(r1BodyName); + + // sva::ForceVecd wrench = computeWrench(mbc, contact, data.lambdaVecPrev(), + // data.lambdaBegin(ci) - data.lambdaBegin()); + + sva::ForceVecd wrench = computeWrench(mbc, contact, lambdaVecPrev, + data.lambdaBegin(ci) - data.lambdaBegin()); + + if (r1BodyIndex == bodyIndex_) + calculatedBodyWrench_ += wrench; + + calculatedForces_.at(r1BodyName) += wrench.force(); + } + } + + const std::string & bodyName = mb.body(bodyIndex_).name(); + + projForceErr_.setZero(); + + // Rafa added this temporal debugging code: + /* + for (const std::pair measWrench : measuredWrenches_) + std::cout << "Rafa, in NullSpaceAdmittanceTask::update, measWrench.first = " + << measWrench.first << std::endl; + for (const std::pair calcForce : calculatedForces_) + std::cout << "Rafa, in NullSpaceAdmittanceTask::update, calcForce.first = " + << calcForce.first << std::endl; + */ + + for (BodyLambda contactBody : contactBodies_) + //for (const std::string & eachBody : bodies_) + { + //std::cout << "Rafa, in NullSpaceAdmittanceTask::update, eachBody = " + // << eachBody << std::endl; + + // Eigen::Vector3d forceErr = calculatedForces_.at(eachBody) - measuredWrenches_.at(eachBody).force(); + Eigen::Vector3d forceErr = calculatedForces_.at(contactBody.body) - measuredWrenches_.at(contactBody.body).force(); + + // if (eachBody == bodyName) + if (contactBody.body == bodyName) + projForceErr_.noalias() += forceErr; + + projForceErr_.noalias() -= fdistRatio_.asDiagonal() * forceErr; + } + + sva::ForceVecd calculatedBodyWrenchDot = (calculatedBodyWrench_ - calculatedBodyWrenchPrev_) / dt_; + calculatedBodyWrenchPrev_ = calculatedBodyWrench_; + + //std::cout << "Rafa, in NullSpaceAdmittanceTask::update, calculatedBodyWrench_.force() = " + // << calculatedBodyWrench_.force().transpose() << std::endl; + + sva::ForceVecd measuredBodyWrenchDot = (measuredWrenches_.at(bodyName) - measuredBodyWrenchPrev_) / dt_; + measuredBodyWrenchPrev_ = measuredWrenches_.at(bodyName); + + Eigen::Matrix6d gainD = Eigen::Matrix6d::Zero(); + gainD.block(0, 0, 3, 3) = gainCoupleD_ * Eigen::Matrix3d::Identity(); + gainD.block(3, 3, 3, 3) = gainForceD_ * Eigen::Matrix3d::Identity(); + + error_.head<3>().noalias() = -gainCoupleP_ * Eigen::Matrix3d::Identity() * (calculatedBodyWrench_.couple() - measuredWrenches_.at(bodyName).couple()); + error_.tail<3>().noalias() = -gainForceP_ * Eigen::Matrix3d::Identity() * projForceErr_; + + error_.noalias() += -gainD * (calculatedBodyWrenchDot.vector() - measuredBodyWrenchDot.vector()); + // The minus sign multiplying the gains is set to get the wrench applied to the environment (important) + + error_.noalias() -= normalAcc_; + + preC_.noalias() = dimWeight_.asDiagonal() * error_; + C_.noalias() = -jacMat_.transpose() * preC_; + + preQ_.noalias() = dimWeight_.asDiagonal() * jacMat_; + Q_.noalias() = jacMat_.transpose() * preQ_; +} + +/** + * ForceDistributionTaskCommon + */ + +ForceDistributionTaskCommon::ForceDistributionTaskCommon(const std::vector & mbs, + int robotIndex, double weight) +: Task(weight), robotIndex_(robotIndex), lambdaBegin_(-1), nrBodies_(-1), + contactBodies_(), contactBodiesPrev_() +{ +} + +void ForceDistributionTaskCommon::fdistRatios(const std::map & ratios) +{ + /* + for (const std::pair & ratio : ratios) + { + fdistRatios_.at(ratio.first) = ratio.second; + } + */ + fdistRatios_ = ratios; +} + +Eigen::Vector3d ForceDistributionTaskCommon::fdistRatio(const std::string & bodyName) const +{ + std::map::const_iterator fdistRatio = fdistRatios_.find(bodyName); + if (fdistRatio != fdistRatios_.end()) + return fdistRatios_.at(bodyName); + else + return Eigen::Vector3d::Zero(); +} + +Eigen::Vector3d ForceDistributionTaskCommon::refForce(const std::string & bodyName) const +{ + std::map::const_iterator refForce = refForces_.find(bodyName); + if (refForce != refForces_.end()) { + return refForces_.at(bodyName); + } + else + return Eigen::Vector3d::Zero(); +} + +void ForceDistributionTaskCommon::updateNrVars(const std::vector & mbs, + const SolverData& data) +{ + lambdaBegin_ = data.lambdaBegin(); + + //std::map fdistRatios_tmp; + std::map refForces_tmp; + + for (const BilateralContact & contact : data.allContacts()) + { + if (contact.contactId.r1Index == robotIndex_) + { + const std::string & r1BodyName = contact.contactId.r1BodyName; + + /* + std::map::iterator fdistRatio = fdistRatios_.find(r1BodyName); + if (fdistRatio != fdistRatios_.end()) + { + fdistRatios_tmp[r1BodyName] = fdistRatios_[r1BodyName]; + } + else + { + fdistRatios_tmp[r1BodyName] = Eigen::Vector3d::Zero(); + } + */ + + std::map::iterator refForce = refForces_.find(r1BodyName); + if (refForce != refForces_.end()) + { + refForces_tmp[r1BodyName] = refForces_[r1BodyName]; + } + else + { + refForces_tmp[r1BodyName] = Eigen::Vector3d::Zero(); + } + } + } + + //fdistRatios_ = fdistRatios_tmp; + refForces_ = refForces_tmp; + + //nrBodies_ = fdistRatios_.size(); + //nrBodies_ = refForces_.size(); + + contactBodies_.clear(); + + int eachBeginIndex = 0; + for (const BilateralContact & contact : data.allContacts()) + { + if (contact.contactId.r1Index == robotIndex_) + { + int nrEachLambda = 0; + for (size_t i = 0; i < contact.r1Cones.size(); i++) + nrEachLambda += contact.r1Cones[i].generators.size(); + + BodyLambda contactBody; + contactBody.body = contact.contactId.r1BodyName; + contactBody.begin = eachBeginIndex; + contactBody.size = nrEachLambda; + + contactBodies_.push_back(contactBody); + + eachBeginIndex += nrEachLambda; + } + } + + if (contactBodiesPrev_.size() == 0) + contactBodiesPrev_ = contactBodies_; + + nrBodies_ = contactBodies_.size(); + + fdistRatioMat_.setZero(3 * nrBodies_, 3); +} + +void ForceDistributionTaskCommon::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + int row = 0; + int column = 0; + + std::map refForceBegin; + + for (const BilateralContact & contact : data.allContacts()) + { + if (contact.contactId.r1Index == robotIndex_) + { + fdistRatioMat_.block<3, 3>(row, 0) = fdistRatios_[contact.contactId.r1BodyName].asDiagonal(); + refForceBegin[contact.contactId.r1BodyName] = row; + + for (size_t i = 0; i < contact.r1Cones.size(); i++) + { + const FrictionCone & cone = contact.r1Cones[i]; + + for (const Eigen::Vector3d & gen : cone.generators) + { + W_.block<3, 1>(row, column) = gen; + column++; + } + } + row += 3; + } + else + { + column += contact.nrLambda(); + } + } + + Eigen::VectorXd lambdaVecPrev = Eigen::VectorXd::Zero(W_.cols()); + + // Assuming that contacts are not set and released simultaneously + if (contactBodies_.size() == contactBodiesPrev_.size()) + + lambdaVecPrev = data.lambdaVecPrev(); + + else { + + for (size_t i = 0; i < contactBodies_.size(); i++) { + + size_t j; + for (j = 0; j < contactBodiesPrev_.size(); j++) + if (contactBodies_[i].body == contactBodiesPrev_[j].body) + break; + + if (j < contactBodiesPrev_.size()) + lambdaVecPrev.segment(contactBodies_[i].begin, contactBodies_[i].size) = data.lambdaVecPrev().segment(contactBodiesPrev_[j].begin, contactBodiesPrev_[j].size); + } + } + + contactBodiesPrev_ = contactBodies_; + + //Eigen::VectorXd refForcesVec = W_ * data.lambdaVecPrev(); + Eigen::VectorXd refForcesVec = W_ * lambdaVecPrev; + + std::map::iterator refForce; + + for (refForce = refForces_.begin(); refForce != refForces_.end(); refForce++) + refForce->second = refForcesVec.segment<3>(refForceBegin.at(refForce->first)); +} + +/** + * ForceDistributionTaskOriginal + */ + +ForceDistributionTaskOriginal::ForceDistributionTaskOriginal(const std::vector & mbs, + int robotIndex, double weight) +: ForceDistributionTaskCommon(mbs, robotIndex, weight), alphaDBegin_(-1), gAcc_(9.81), + totalMass_(0), comJac_(mbs[robotIndex_]), comJacMat_(3, mbs[robotIndex_].nrDof()), + normalAcc_(Eigen::Vector3d::Zero()) +{ + const rbd::MultiBody & mb = mbs[robotIndex_]; + + for (int i = 0; i < mb.nrBodies(); i++) + { + totalMass_ += mb.body(i).inertia().mass(); + } +} + +void ForceDistributionTaskOriginal::updateNrVars(const std::vector & mbs, + const SolverData& data) +{ + ForceDistributionTaskCommon::updateNrVars(mbs, data); + + alphaDBegin_ = data.alphaDBegin(robotIndex_); + + int nrLambda = data.totalLambda(); + int nrVars = data.nrVars(); + + W_.setZero(3 * nrBodies_, nrLambda); + A_.setZero(3 * nrBodies_, nrVars); + + Q_.setZero(nrVars, nrVars); + C_.setZero(nrVars); + + CSum_.setZero(3 * nrBodies_); +} + +void ForceDistributionTaskOriginal::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + ForceDistributionTaskCommon::update(mbs, mbcs, data); + + const rbd::MultiBody & mb = mbs[robotIndex_]; + const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_]; + + comJacMat_ = comJac_.jacobian(mb, mbc); + + Eigen::Vector3d gVec(0, 0, -gAcc_); + normalAcc_ = comJac_.normalAcceleration(mb, mbc); + + Eigen::Vector3d preCSum = totalMass_ * (gVec - normalAcc_); + CSum_ = fdistRatioMat_ * preCSum; + + A_.block(0, alphaDBegin_, 3 * nrBodies_, mb.nrDof()).noalias() = totalMass_ * fdistRatioMat_ * comJacMat_; + A_.block(0, lambdaBegin_, 3 * nrBodies_, data.totalLambda()) = -W_; + + Q_.noalias() = A_.transpose() * A_; + C_.noalias() = -A_.transpose() * CSum_; +} + +/** + * ForceDistributionTaskOptimized + */ + +ForceDistributionTaskOptimized::ForceDistributionTaskOptimized(const std::vector & mbs, + int robotIndex, double weight) +: ForceDistributionTaskCommon(mbs, robotIndex, weight) +{ +} + +void ForceDistributionTaskOptimized::updateNrVars(const std::vector & mbs, + const SolverData& data) +{ + ForceDistributionTaskCommon::updateNrVars(mbs, data); + + int nrLambda = data.totalLambda(); + + W_.setZero(3 * nrBodies_, nrLambda); + + preA_.setZero(3 * nrBodies_, nrLambda); + A_.setZero(3 * nrBodies_, nrLambda); + + Q_.setZero(nrLambda, nrLambda); + C_.setZero(nrLambda); + + SumMat_.setZero(3, 3 * nrBodies_); +} + +void ForceDistributionTaskOptimized::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + ForceDistributionTaskCommon::update(mbs, mbcs, data); + + for (int col = 0; col < nrBodies_; col++) + SumMat_.block<3, 3>(0, 3 * col) = Eigen::MatrixXd::Identity(3, 3); + + preA_.noalias() = Eigen::MatrixXd::Identity(3 * nrBodies_, 3 * nrBodies_) - fdistRatioMat_ * SumMat_; + A_.noalias() = preA_ * W_; + + Q_.noalias() = A_.transpose() * A_; +} + +/** + * ZMPBasedCoMTask + */ + +ZMPBasedCoMTask::ZMPBasedCoMTask(const std::vector & mbs, int robotIndex, + const Eigen::Vector3d & com, const Eigen::Vector3d & zmp, + double weight) +: Task(weight), robotIndex_(robotIndex), com_(com), ddcom_(Eigen::Vector3d::Zero()), zmp_(zmp), + gAcc_(9.81), alphaDBegin_(0), dimWeight_(Eigen::Vector3d::Ones()), jac_(mbs[robotIndex_]), + Q_(mbs[robotIndex_].nrDof(), mbs[robotIndex_].nrDof()), C_(mbs[robotIndex_].nrDof()), + jacMat_(3, mbs[robotIndex_].nrDof()), preQ_(3, mbs[robotIndex_].nrDof()), + CSum_(Eigen::Vector3d::Zero()), normalAcc_(Eigen::Vector3d::Zero()) +{ +} + +void ZMPBasedCoMTask::updateNrVars(const std::vector& /* mbs */, + const SolverData& data) +{ + alphaDBegin_ = data.alphaDBegin(robotIndex_); +} + +void ZMPBasedCoMTask::update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) +{ + const rbd::MultiBody & mb = mbs[robotIndex_]; + const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_]; + + normalAcc_ = jac_.normalAcceleration(mb, mbc); + + Eigen::Vector3d com_hat = rbd::computeCoM(mb, mbc); + + /* + CSum_ << + (gAcc_ + ddcom_.z()) / (com_hat.z() - zmp_.z()) * (com_.x() - zmp_.x()), + (gAcc_ + ddcom_.z()) / (com_hat.z() - zmp_.z()) * (com_.y() - zmp_.y()), + ddcom_.z(); + */ + + CSum_ << + (gAcc_ + ddcom_.z()) / (com_.z() - zmp_.z()) * (com_.x() - zmp_.x()), + (gAcc_ + ddcom_.z()) / (com_.z() - zmp_.z()) * (com_.y() - zmp_.y()), + ddcom_.z(); + + CSum_ -= normalAcc_; + + jacMat_ = jac_.jacobian(mb, mbc); + + preQ_.noalias() = dimWeight_.asDiagonal() * jacMat_; + + Q_.noalias() = jacMat_.transpose() * preQ_; + C_.noalias() = -jacMat_.transpose() * dimWeight_.asDiagonal() * CSum_; +} + +/** + * ZMPTask + */ + +ZMPTask::ZMPTask(const std::vector & mbs, int robotIndex, + const Eigen::Vector3d & zmp, double weight) +: Task(weight), robotIndex_(robotIndex), lambdaBegin_(-1), nrBodies_(-1), + contactBodies_(), contactBodiesPrev_(), + zmp_(zmp), totalForce_(Eigen::Vector3d::Zero()), totalMomentZMP_(Eigen::Vector3d::Zero()), + dimWeight_(Eigen::Vector3d::Ones()) +{} + +void ZMPTask::updateNrVars(const std::vector & /* mbs */, const SolverData & data) +{ + lambdaBegin_ = data.lambdaBegin(); + int nrLambda = data.totalLambda(); + + contactBodies_.clear(); + + int eachBeginIndex = 0; + for (const BilateralContact & contact : data.allContacts()) + { + if (contact.contactId.r1Index == robotIndex_) + { + int nrEachLambda = 0; + for (size_t i = 0; i < contact.r1Cones.size(); i++) + nrEachLambda += contact.r1Cones[i].generators.size(); + + BodyLambda contactBody; + contactBody.body = contact.contactId.r1BodyName; + contactBody.begin = eachBeginIndex; + contactBody.size = nrEachLambda; + + contactBodies_.push_back(contactBody); + + eachBeginIndex += nrEachLambda; + } + } + + if (contactBodiesPrev_.size() == 0) + contactBodiesPrev_ = contactBodies_; + + /* + std::cout << "Rafa, in ZMPTask::updateNrVars, contactBodies = "; + for (BodyLambda contactBody : contactBodies_) + std::cout << contactBody.body << " " << contactBody.begin << " " << contactBody.size << " "; + std::cout << std::endl; // Rafa + */ + + nrBodies_ = contactBodies_.size(); + + W_.setZero(3 * nrBodies_, nrLambda); + pW_.setZero(3, nrLambda); + + Q_.setZero(nrLambda, nrLambda); + C_.setZero(nrLambda); + + preQ_.setZero(3, nrLambda); +} + +void ZMPTask::update(const std::vector & mbs, const std::vector & mbcs, const SolverData & data) +{ + int row = 0; + int column = 0; + + for (const BilateralContact & contact : data.allContacts()) + { + if (contact.contactId.r1Index == robotIndex_) + { + int r1BodyIndex = mbs[contact.contactId.r1Index].bodyIndexByName(contact.contactId.r1BodyName); + + for (size_t i = 0; i < contact.r1Cones.size(); i++) + { + const sva::PTransformd & bodyPosW = mbcs[robotIndex_].bodyPosW[r1BodyIndex]; + Eigen::Vector3d contactPosW = bodyPosW.translation() + bodyPosW.rotation().transpose() * contact.r1Points[i]; + + const FrictionCone & cone = contact.r1Cones[i]; + + for (const Eigen::Vector3d & gen : cone.generators) + { + /* + std::cout << "Rafa, in ZMPTask::update, for i = " << i << ", " + << "gen = " << gen.transpose() << std::endl; + */ + + W_.block<3, 1>(row, column) = gen; + pW_.col(column).noalias() = (contactPosW - zmp_).cross(gen); + column++; + } + } + row += 3; + } + else + { + column += contact.nrLambda(); + } + } + + Eigen::VectorXd lambdaVecPrev = Eigen::VectorXd::Zero(W_.cols()); + + // Assuming that contacts are not set and released simultaneously + if (contactBodies_.size() == contactBodiesPrev_.size()) + + lambdaVecPrev = data.lambdaVecPrev(); + + else { + + for (size_t i = 0; i < contactBodies_.size(); i++) { + + size_t j; + for (j = 0; j < contactBodiesPrev_.size(); j++) + if (contactBodies_[i].body == contactBodiesPrev_[j].body) + break; + + if (j < contactBodiesPrev_.size()) + lambdaVecPrev.segment(contactBodies_[i].begin, contactBodies_[i].size) = data.lambdaVecPrev().segment(contactBodiesPrev_[j].begin, contactBodiesPrev_[j].size); + + // std::cout << "Rafa, in ZMPTask::update, for i = " << i << ", lambdaVecPrev = " << lambdaVecPrev.transpose() << std::endl; + } + } + + contactBodiesPrev_ = contactBodies_; + + totalForce_ = Eigen::Vector3d::Zero(); + // Eigen::VectorXd refForcesVec = W_ * data.lambdaVecPrev(); + Eigen::VectorXd refForcesVec = W_ * lambdaVecPrev; + for (size_t i = 0; i < nrBodies_; i++) + totalForce_ += refForcesVec.segment<3>(3 * i); + + if (totalForce_.norm() == 0 || totalForce_.norm() > 2000) { + std::cout << "Rafa, in ZMPTask::update, W_ = " << std::endl << W_ << std::endl; + std::cout << "Rafa, in ZMPTask::update, data.lambdaVecPrev() = " << data.lambdaVecPrev().transpose() << std::endl; + std::cout << "Rafa, in ZMPTask::update, lambdaVecPrev = " << lambdaVecPrev.transpose() << std::endl; + std::cout << "Rafa, in ZMPTask::update, refForcesVec = " << refForcesVec.transpose() << std::endl; + } + + // totalMomentZMP_ = pW_ * data.lambdaVecPrev(); + totalMomentZMP_ = pW_ * lambdaVecPrev; + + preQ_.noalias() = dimWeight_.asDiagonal() * pW_; + Q_.noalias() = pW_.transpose() * preQ_; +} + +/** + * YawMomentCompensationTask - Not used + */ + +YawMomentCompensationTask::YawMomentCompensationTask(const std::vector & mbs, int robotIndex, double timeStep, + double gainKp, double gainKd, double gainKi, double gainKii, double weight) +: Task(weight), robotIndex_(robotIndex), dt_(timeStep), gainKp_(gainKp), gainKd_(gainKd), gainKi_(gainKi), gainKii_(gainKii), + alphaDBegin_(0), com_prev_(Eigen::Vector3d::Zero()), zmp_(Eigen::Vector3d::Zero()), zmp_prev_(Eigen::Vector3d::Zero()), + delta_tau_p_(0), delta_tau_p_prev_(0), Lpz_(0), iLpz_(0), centroidalMomentumMatrix_(mbs[robotIndex]), jacMat_(3, mbs[robotIndex].nrDof()), + Q_(mbs[robotIndex].nrDof(), mbs[robotIndex].nrDof()), C_(mbs[robotIndex].nrDof()) +{} + +void YawMomentCompensationTask::updateNrVars(const std::vector& /* mbs */, const SolverData& data) +{ + alphaDBegin_ = data.alphaDBegin(robotIndex_); +} + +void YawMomentCompensationTask::update(const std::vector & mbs, const std::vector & mbcs, const SolverData & data) +{ + const rbd::MultiBody & mb = mbs[robotIndex_]; + const rbd::MultiBodyConfig & mbc = mbcs[robotIndex_]; + + Eigen::Vector3d com = rbd::computeCoM(mb, mbc); + Eigen::Vector3d dcom = (com - com_prev_) / dt_; + com_prev_ = com; + + Eigen::Vector3d dzmp = (zmp_ - zmp_prev_) / dt_; + zmp_prev_ = zmp_; + + centroidalMomentumMatrix_.computeMatrixAndMatrixDot(mb, mbc, com, dcom); + + Eigen::MatrixXd zmpMomentumMatrix = centroidalMomentumMatrix_.matrix(); + zmpMomentumMatrix.topRows<3>().noalias() = skewMatrix(com - zmp_) * zmpMomentumMatrix.bottomRows<3>() + zmpMomentumMatrix.topRows<3>(); + + Eigen::MatrixXd zmpMomentumMatrixDot = centroidalMomentumMatrix_.matrixDot(); + zmpMomentumMatrixDot.topRows<3>().noalias() = skewMatrix(com - zmp_) * zmpMomentumMatrixDot.bottomRows<3>() + + skewMatrix(dcom - dzmp) * zmpMomentumMatrix.bottomRows<3>() + zmpMomentumMatrixDot.topRows<3>(); + + double delta_dtau_p = (delta_tau_p_ - delta_tau_p_prev_) / dt_; + delta_tau_p_prev_ = delta_tau_p_; + + double dLpz = gainKp_ * delta_tau_p_ + gainKd_ * delta_dtau_p - gainKi_ * Lpz_ - gainKii_ * iLpz_; + double error = dLpz - zmpMomentumMatrixDot.row(2) * rbd::dofToVector(mb, mbc.alpha); + + iLpz_ += Lpz_ * dt_ + dLpz * dt_ * dt_ / 2; + Lpz_ += dLpz * dt_; + + jacMat_ = zmpMomentumMatrix.row(2); + + C_.noalias() = -jacMat_.transpose() * error; + Q_.noalias() = jacMat_.transpose() * jacMat_; +} + +double YawMomentCompensationTask::computeTauP() +{ + /* + calculatedWrench_ = sva::ForceVecd(Eigen::Vector6d::Zero()); + + for (size_t ci = 0; ci < data.allContacts().size(); ci++) + { + const tasks::qp::BilateralContact& contact = data.allContacts()[ci]; + + int r1BodyIndex = mbs[contact.contactId.r1Index].bodyIndexByName(contact.contactId.r1BodyName); + + if (contact.contactId.r1Index == robotIndex_ && r1BodyIndex == bodyIndex_) + { + calculatedWrench_ += computeWrench(mbc, contact, data.lambdaVecPrev(), data.lambdaBegin(ci) - data.lambdaBegin()); + } + } + */ +} + +sva::ForceVecd YawMomentCompensationTask::computeWrench(const rbd::MultiBodyConfig & mbc, const tasks::qp::BilateralContact & contact, Eigen::VectorXd lambdaVec, int pos) +{ + sva::ForceVecd wrench(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero()); + /* + for (size_t i = 0; i < contact.r1Points.size(); i++) + { + Eigen::VectorXd lambda = Eigen::VectorXd::Zero(contact.nrLambda(i)); + + if (lambdaVec.size() > 0 && pos < lambdaVec.size()) + { + lambda = lambdaVec.segment(pos, contact.nrLambda(i)); + } + + Eigen::Matrix3d RBody = mbc.bodyPosW[bodyIndex_].rotation().transpose(); + + wrench.force() += contact.force(lambda, i, contact.r1Cones); + wrench.couple() += (RBody * contact.r1Points[i]).cross(contact.force(lambda, i, contact.r1Cones)); + + pos += contact.nrLambda(i); + } + */ + return wrench; +} + +Eigen::Matrix3d YawMomentCompensationTask::skewMatrix(const Eigen::Vector3d & v) +{ + Eigen::Matrix3d m; + m << 0., -v[2], v[1], + v[2], 0., -v[0], + -v[1], v[0], 0.; + return m; +} + } // namespace qp } // namespace tasks diff --git a/src/Tasks.cpp b/src/Tasks.cpp index 27799af13..3824ee0ef 100644 --- a/src/Tasks.cpp +++ b/src/Tasks.cpp @@ -14,6 +14,8 @@ #include #include +#include // Rafa added this + namespace tasks { @@ -1518,6 +1520,7 @@ const Eigen::MatrixXd & RelativeDistTask::jac() const return jacMat_; } + /** * VectorOrientationTask */ @@ -1608,4 +1611,5 @@ const Eigen::MatrixXd & VectorOrientationTask::jac() const return jacMat_; } + } // namespace tasks diff --git a/src/Tasks/QPContactConstr.h b/src/Tasks/QPContactConstr.h index 0f4c219f9..b3697f8ed 100644 --- a/src/Tasks/QPContactConstr.h +++ b/src/Tasks/QPContactConstr.h @@ -14,6 +14,7 @@ // RBDyn #include +#include // Tasks #include "QPSolver.h" @@ -250,6 +251,83 @@ class TASKS_DLLAPI ContactPosConstr : public ContactConstr double timeStep_; }; -} // namespace qp +/** + * Contact constraint by tracking a frame with a PD, + * and including the integral term. + * + */ +class TASKS_DLLAPI TorqueFbTermContactPDConstr : public ContactConstr +{ +public: + TorqueFbTermContactPDConstr(Eigen::Vector6d stiffness, Eigen::Vector6d damping, int mainRobotIndex, + const std::shared_ptr fbTerm); + + void setPDgainsForContact(const ContactId & cId, const Eigen::Vector6d & stiff, const Eigen::Vector6d & damp); + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual std::string nameEq() const override; + +private: + + struct PDgains + { + PDgains(const Eigen::Vector6d& stiff, const Eigen::Vector6d& damp) + : stiffness(stiff), damping(damp) + {} + + Eigen::Vector6d stiffness, damping; + }; + + std::map contPD_; + + Eigen::Vector6d stiffness_default_, damping_default_; + int mainRobotIndex_; + std::shared_ptr fbTerm_; +}; + + +/** + * Contact constraint by using admittance control and damping, + * and including the integral term. + * + */ +class TASKS_DLLAPI TorqueFbTermContactHybridConstr : public ContactConstr +{ +public: + TorqueFbTermContactHybridConstr(Eigen::Vector6d proportional, Eigen::Vector6d derivative, + Eigen::Vector6d damping, int mainRobotIndex, + const std::shared_ptr fbTerm); + + void setGainsForContact(const ContactId & cId, const Eigen::Vector6d & prop, + const Eigen::Vector6d & deriv, const Eigen::Vector6d & damp); + + virtual void update(const std::vector & mbs, + const std::vector& mbcs, + const SolverData& data); + + virtual std::string nameEq() const override; + +private: + + struct Gains + { + Gains(const Eigen::Vector6d& prop, const Eigen::Vector6d& deriv, const Eigen::Vector6d& damp) + : proportional(prop), derivative(deriv), damping(damp) + {} + + Eigen::Vector6d proportional, derivative, damping; + }; + + std::map contGains_; + + Eigen::Vector6d proportional_default_, derivative_default_, damping_default_; + int mainRobotIndex_; + std::shared_ptr fbTerm_; +}; + +} // qp } // namespace tasks diff --git a/src/Tasks/QPMotionConstr.h b/src/Tasks/QPMotionConstr.h index 03eee2b1e..9a270efab 100644 --- a/src/Tasks/QPMotionConstr.h +++ b/src/Tasks/QPMotionConstr.h @@ -14,6 +14,8 @@ // RBDyn #include #include +#include +#include // Tasks #include "QPSolver.h" @@ -65,9 +67,10 @@ class TASKS_DLLAPI PositiveLambda : public ConstraintFunction class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction { public: - MotionConstrCommon(const std::vector & mbs, int robotIndex); + MotionConstrCommon(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd); - void computeTorque(const Eigen::VectorXd & alphaD, const Eigen::VectorXd & lambda); + virtual void computeTorque(const Eigen::VectorXd & alphaD, const Eigen::VectorXd & lambda); const Eigen::VectorXd & torque() const; void torque(const std::vector & mbs, std::vector & mbcs) const; @@ -108,7 +111,8 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction protected: int robotIndex_, alphaDBegin_, nrDof_, lambdaBegin_; - rbd::ForwardDynamics fd_; + // rbd::ForwardDynamics fd_; + std::shared_ptr fd_; Eigen::MatrixXd fullJacLambda_, jacTrans_, jacLambda_; std::vector cont_; @@ -122,14 +126,18 @@ class TASKS_DLLAPI MotionConstrCommon : public ConstraintFunction class TASKS_DLLAPI MotionConstr : public MotionConstrCommon { public: - MotionConstr(const std::vector & mbs, int robotIndex, const TorqueBound & tb); + MotionConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, + const TorqueBound & tb); - MotionConstr(const std::vector & mbs, - int robotIndex, + MotionConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt); + void computeTorque(const Eigen::VectorXd& alphaD, const Eigen::VectorXd& lambda) override; + // Constraint virtual void update(const std::vector & mbs, const std::vector & mbcs, @@ -142,9 +150,15 @@ class TASKS_DLLAPI MotionConstr : public MotionConstrCommon // Contact torque Eigen::MatrixXd contactMatrix() const; // Access fd... - const rbd::ForwardDynamics fd() const; + const std::shared_ptr fd() const; + + const Eigen::VectorXd & computedTorque() const + { + return computedTorque_; + } protected: + Eigen::VectorXd computedTorque_; Eigen::VectorXd torqueL_, torqueU_; Eigen::VectorXd torqueDtL_, torqueDtU_; Eigen::VectorXd tmpL_, tmpU_; @@ -162,13 +176,13 @@ struct SpringJoint class TASKS_DLLAPI MotionSpringConstr : public MotionConstr { public: - MotionSpringConstr(const std::vector & mbs, - int robotIndex, + MotionSpringConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const std::vector & springs); - MotionSpringConstr(const std::vector & mbs, - int robotIndex, + MotionSpringConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt, @@ -200,7 +214,8 @@ class TASKS_DLLAPI MotionSpringConstr : public MotionConstr class TASKS_DLLAPI MotionPolyConstr : public MotionConstrCommon { public: - MotionPolyConstr(const std::vector & mbs, int robotIndex, const PolyTorqueBound & ptb); + MotionPolyConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const PolyTorqueBound & ptb); // Constraint virtual void update(const std::vector & mbs, @@ -212,6 +227,76 @@ class TASKS_DLLAPI MotionPolyConstr : public MotionConstrCommon std::vector jointIndex_; }; +class TASKS_DLLAPI MotionFrictionConstr : public MotionConstr +{ +public: + MotionFrictionConstr(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, + const std::shared_ptr friction, + const TorqueBound & tb); + + void computeTorque(const Eigen::VectorXd& alphaD, const Eigen::VectorXd& lambda) override; + + // Constraint + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + const Eigen::VectorXd& frictionTorque() const + { + return frictionTorque_; + } + + virtual std::string nameGenInEq() const override; + +private: + std::shared_ptr friction_; + Eigen::VectorXd frictionTorque_; +}; + +class TASKS_DLLAPI TorqueFbTermMotionConstr : public MotionConstr +{ +public: + TorqueFbTermMotionConstr(const std::vector& mbs, int robotIndex, + const std::shared_ptr fd, + const std::shared_ptr fbTerm, + const TorqueBound& tb); + + void computeTorque(const Eigen::VectorXd& alphaD, const Eigen::VectorXd& lambda) override; + + // Constraint + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual std::string nameGenInEq() const override; + +private: + std::shared_ptr fbTerm_; +}; + +class TASKS_DLLAPI TorqueFbTermMotionFrictionConstr : public MotionFrictionConstr +{ +public: + TorqueFbTermMotionFrictionConstr(const std::vector& mbs, int robotIndex, + const std::shared_ptr fd, + const std::shared_ptr friction, + const std::shared_ptr fbTerm, + const TorqueBound& tb); + + void computeTorque(const Eigen::VectorXd& alphaD, const Eigen::VectorXd& lambda) override; + + // Constraint + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual std::string nameGenInEq() const override; + +private: + std::shared_ptr fbTerm_; +}; + } // namespace qp } // namespace tasks diff --git a/src/Tasks/QPSolver.h b/src/Tasks/QPSolver.h index ccb8f914c..336bcaa2c 100644 --- a/src/Tasks/QPSolver.h +++ b/src/Tasks/QPSolver.h @@ -42,11 +42,13 @@ class Bound; class Task; class GenQPSolver; +// class MotionConstr; + class TASKS_DLLAPI QPSolver { public: QPSolver(); - ~QPSolver(); + virtual ~QPSolver(); // declared as virtual for QPSolver to be polymorphic /** solve the problem * \param mbs current multibody @@ -81,6 +83,9 @@ class TASKS_DLLAPI QPSolver /// call updateNrVars on all tasks and constraints void updateNrVars(const std::vector & mbs) const; + bool hasConstraint(const Constraint* co); + // std::shared_ptr getMotionConstr(); + void addEqualityConstraint(Equality * co); void removeEqualityConstraint(Equality * co); int nrEqualityConstraints() const; @@ -108,6 +113,12 @@ class TASKS_DLLAPI QPSolver void resetTasks(); int nrTasks() const; + const std::vector & getTasks() const { return tasks_; } + const std::vector & getEqConstr() const { return eqConstr_; } + const std::vector & getInEqConstr() const { return inEqConstr_; } + const std::vector & getGenInEqConstr() const { return genInEqConstr_; } + const std::vector & getBoundConstr() const { return boundConstr_; } + void solver(const std::string & name); std::string solver() const; @@ -130,7 +141,6 @@ class TASKS_DLLAPI QPSolver void preUpdate(const std::vector & mbs, const std::vector & mbcs); void postUpdate(const std::vector & mbs, std::vector & mbcs, bool success); -private: std::vector constr_; std::vector eqConstr_; std::vector inEqConstr_; @@ -148,6 +158,25 @@ class TASKS_DLLAPI QPSolver boost::timer::cpu_timer solverTimer_, solverAndBuildTimer_; }; +class TASKS_DLLAPI PassivityPIDTerm_QPSolver : public QPSolver +{ +public: + PassivityPIDTerm_QPSolver(); + + bool solve(const std::vector & mbs, + std::vector & mbcs_real, + std::vector & mbcs_calc); + + bool solveNoMbcUpdate(const std::vector & mbs, + const std::vector & mbcs_real, + const std::vector & mbcs_calc); + + protected: + void preUpdate(const std::vector & mbs, + const std::vector & mbcs_real, + const std::vector & mbcs_calc); +}; + class TASKS_DLLAPI Constraint { public: @@ -314,6 +343,11 @@ class TASKS_DLLAPI Task Task(double weight) : weight_(weight) {} virtual ~Task() {} + virtual std::string nameTask() const + { + return "Task"; + } + virtual double weight() const { return weight_; @@ -343,6 +377,11 @@ class TASKS_DLLAPI HighLevelTask public: virtual ~HighLevelTask() {} + virtual std::string nameHighLevelTask() const + { + return "HighLevelTask"; + } + virtual int dim() = 0; virtual void update(const std::vector & mbs, diff --git a/src/Tasks/QPSolverData.h b/src/Tasks/QPSolverData.h index 37a222869..52dbf1313 100644 --- a/src/Tasks/QPSolverData.h +++ b/src/Tasks/QPSolverData.h @@ -28,6 +28,7 @@ class TASKS_DLLAPI SolverData { public: friend class QPSolver; + friend class PassivityPIDTerm_QPSolver; SolverData(); @@ -128,7 +129,13 @@ class TASKS_DLLAPI SolverData return normalAccB_[robotIndex]; } -private: + const Eigen::VectorXd& lambdaVecPrev() const + { + return lambdaVecPrev_; + } + +// private: +protected: std::vector alphaD_; //< each robot alphaD vector size std::vector alphaDBegin_; //< each robot alphaD vector begin in x std::vector lambda_; //< each contact lambda @@ -144,6 +151,9 @@ class TASKS_DLLAPI SolverData std::vector mobileRobotIndex_; //< robot index with dof > 0 /// normal acceleration of each body of each robot std::vector> normalAccB_; + + Eigen::VectorXd passiveTorque_; + Eigen::VectorXd lambdaVecPrev_; }; } // namespace qp diff --git a/src/Tasks/QPTasks.h b/src/Tasks/QPTasks.h index a82d41df1..92a69d611 100644 --- a/src/Tasks/QPTasks.h +++ b/src/Tasks/QPTasks.h @@ -7,10 +7,15 @@ // includes // #include +#include // Eigen #include +// RBDyn, added by Rafa +#include +#include + // Tasks #include "QPMotionConstr.h" #include "QPSolver.h" @@ -40,6 +45,11 @@ class TASKS_DLLAPI SetPointTaskCommon : public Task const Eigen::VectorXd & dimWeight, double weight); + virtual std::string nameTask() const override + { + return hlTask_->nameHighLevelTask(); + } + virtual std::pair begin() const override { return std::make_pair(alphaDBegin_, alphaDBegin_); @@ -394,37 +404,37 @@ struct JointGains class TASKS_DLLAPI TorqueTask : public Task { public: - TorqueTask(const std::vector & mbs, int robotIndex, const TorqueBound & tb, double weight); + TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, + const TorqueBound & tb, double weight); - TorqueTask(const std::vector & mbs, - int robotIndex, - const TorqueBound & tb, - const Eigen::VectorXd & jointSelect, + TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, + const TorqueBound & tb, const Eigen::VectorXd & jointSelect, double weight); - TorqueTask(const std::vector & mbs, - int robotIndex, - const TorqueBound & tb, - const std::string & efName, + TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, + const TorqueBound & tb, const std::string & efName, double weight); - TorqueTask(const std::vector & mbs, - int robotIndex, + TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt, double weight); - TorqueTask(const std::vector & mbs, - int robotIndex, + TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt, const Eigen::VectorXd & jointSelect, double weight); - TorqueTask(const std::vector & mbs, - int robotIndex, + TorqueTask(const std::vector & mbs, int robotIndex, + const std::shared_ptr fd, const TorqueBound & tb, const TorqueDBound & tdb, double dt, @@ -474,6 +484,11 @@ class TASKS_DLLAPI PostureTask : public Task double stiffness, double weight); + virtual std::string nameTask() const override + { + return "PostureTask"; + } + tasks::PostureTask & task() { return pt_; @@ -584,6 +599,11 @@ class TASKS_DLLAPI PositionTask : public HighLevelTask const Eigen::Vector3d & pos, const Eigen::Vector3d & bodyPoint = Eigen::Vector3d::Zero()); + virtual std::string nameHighLevelTask() const override + { + return "PositionTask"; + } + tasks::PositionTask & task() { return pt_; @@ -636,6 +656,11 @@ class TASKS_DLLAPI OrientationTask : public HighLevelTask const std::string & bodyName, const Eigen::Matrix3d & ori); + virtual std::string nameHighLevelTask() const override + { + return "OrientationTask"; + } + tasks::OrientationTask & task() { return ot_; @@ -912,6 +937,11 @@ class TASKS_DLLAPI CoMTask : public HighLevelTask const Eigen::Vector3d & com, std::vector weight); + virtual std::string nameHighLevelTask() const override + { + return "CoMTask"; + } + tasks::CoMTask & task() { return ct_; @@ -948,7 +978,7 @@ class TASKS_DLLAPI CoMTask : public HighLevelTask tasks::CoMTask ct_; int robotIndex_; }; - + class TASKS_DLLAPI MultiCoMTask : public Task { public: @@ -1129,6 +1159,83 @@ class TASKS_DLLAPI MomentumTask : public HighLevelTask int robotIndex_; }; +class TASKS_DLLAPI CentroidalAngularMomentumTask : public Task +{ +public: + CentroidalAngularMomentumTask(const std::vector& mbs, int robotIndex, + double gain, const Eigen::Vector3d angMomentum, double weight); + + virtual std::string nameTask() const override + { + return "CentroidalAngularMomentumTask"; + } + + virtual std::pair begin() const + { + return std::make_pair(alphaDBegin_, alphaDBegin_); + } + + void angMomentum(const Eigen::Vector3d & angMomentum) + { + angMomentum_ = angMomentum; + } + + const Eigen::Vector3d angMomentum() const + { + return angMomentum_; + } + + void setGain(double gain) + { + gain_ = gain; + } + + void dimWeight(const Eigen::Vector3d & dim) + { + dimWeight_ = dim; + } + + const Eigen::Vector3d & dimWeight() const + { + return dimWeight_; + } + + virtual void updateNrVars(const std::vector & mbs, + const SolverData & data); + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual const Eigen::MatrixXd & Q() const + { + return Q_; + } + + virtual const Eigen::VectorXd & C() const + { + return C_; + } + +private: + + int robotIndex_, alphaDBegin_; + Eigen::Vector3d dimWeight_; + + Eigen::Vector3d angMomentum_; + double gain_; + + rbd::CentroidalMomentumMatrix centroidalMomentumMatrix_; + + Eigen::MatrixXd Q_; + Eigen::VectorXd C_; + // cache + Eigen::MatrixXd jacMat_; + Eigen::MatrixXd preQ_; + Eigen::Vector3d CSum_; + Eigen::Vector3d normalAcc_; +}; + class TASKS_DLLAPI ContactTask : public Task { public: @@ -1405,6 +1512,787 @@ class TASKS_DLLAPI VectorOrientationTask : public HighLevelTask int robotIndex_; }; +class TASKS_DLLAPI WrenchTask : public Task +{ + public: + WrenchTask(const std::vector & mbs, int robotIndex, const std::string & bodyName, + const Eigen::Vector3d & bodyPoint, double weight); + + virtual std::string nameTask() const override + { + return "WrenchTask"; + } + + virtual std::pair begin() const + { + return std::make_pair(lambdaBegin_, lambdaBegin_); + } + + void bodyPoint(const Eigen::Vector3d & point) + { + bodyPoint_ = point; + } + + const Eigen::Vector3d & bodyPoint() const + { + return bodyPoint_; + } + + void treatDesWrenchAsLocal(bool local) + { + local_ = local; + } + + bool treatSettingsAsLocal() + { + return local_; + } + + void wrench(const std::vector & mbcs, const sva::ForceVecd & wrench); + + const sva::ForceVecd & wrench() const + { + return wrench_; + } + + void dimWeight(const std::vector & mbcs, const Eigen::Vector6d & dim); + + const Eigen::Vector6d & dimWeight() const + { + return dimWeight_; + } + + virtual void updateNrVars(const std::vector & mbs, + const SolverData& data); + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual const Eigen::MatrixXd & Q() const + { + return Q_; + } + + virtual const Eigen::VectorXd & C() const + { + return C_; + } + + private: + + int robotIndex_, bodyIndex_, lambdaBegin_; + Eigen::Vector6d dimWeight_; + + Eigen::Vector3d bodyPoint_; + sva::ForceVecd wrench_; + + Eigen::MatrixXd W_; + + // the following flag indicates if the desired values are expressed or not in the local frame + bool local_; + + Eigen::MatrixXd Q_; + Eigen::VectorXd C_; + // cache + Eigen::MatrixXd preQ_; + Eigen::MatrixXd preC_; +}; + +class TASKS_DLLAPI LocalCoPTask : public Task +{ + public: + LocalCoPTask(const std::vector & mbs, int robotIndex, + const std::string & bodyName, const Eigen::Vector3d & localCoP, + double weight); + + virtual std::string nameTask() const override + { + return "LocalCoPTask"; + } + + virtual std::pair begin() const + { + return std::make_pair(lambdaBegin_, lambdaBegin_); + } + + void localCoP(const Eigen::Vector3d & point) + { + localCoP_ = point; + } + + const Eigen::Vector3d & localCoP() const + { + return localCoP_; + } + + void dimWeight(const std::vector & mbcs, const Eigen::Vector3d & dim); + + const Eigen::Vector3d & dimWeight() const + { + return dimWeight_; + } + + virtual void updateNrVars(const std::vector & mbs, + const SolverData& data); + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual const Eigen::MatrixXd & Q() const + { + return Q_; + } + + virtual const Eigen::VectorXd & C() const + { + return C_; + } + + private: + + int robotIndex_, bodyIndex_, lambdaBegin_; + Eigen::Vector3d dimWeight_; + + Eigen::Vector3d localCoP_; + + Eigen::MatrixXd W_; + + Eigen::MatrixXd Q_; + Eigen::VectorXd C_; + // cache + Eigen::MatrixXd preQ_; +}; + +class TASKS_DLLAPI AdmittanceTaskCommon : public Task +{ +public: + AdmittanceTaskCommon(const std::vector & mbs, int robotIndex, + const std::string & bodyName, + const Eigen::Vector3d & bodyPoint, + double timeStep, double gainForceP, double gainForceD, + double gainCoupleP, double gainCoupleD, double weight); + + virtual std::pair begin() const + { + return std::make_pair(alphaDBegin_, alphaDBegin_); + } + + void bodyPoint(const Eigen::Vector3d & point) + { + jac_.point(point); + } + + const Eigen::Vector3d & bodyPoint() const + { + return jac_.point(); + } + + void treatSettingsAsLocal(bool local) + { + local_ = local; + } + + void setForceGains(double gainP, double gainD) + { + gainForceP_ = gainP; + gainForceD_ = gainD; + } + + void setCoupleGains(double gainP, double gainD) + { + gainCoupleP_ = gainP; + gainCoupleD_ = gainD; + } + + void dimWeight(const std::vector & mbcs, + const Eigen::Vector6d & dim); + + const Eigen::Vector6d & dimWeight() const + { + return dimWeight_; + } + + virtual void updateNrVars(const std::vector & mbs, + const SolverData & data); + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data) = 0; + + virtual const Eigen::MatrixXd & Q() const + { + return Q_; + } + + virtual const Eigen::VectorXd & C() const + { + return C_; + } + + struct BodyLambda + { + std::string body; + int begin; + int size; + }; + +protected: + + sva::ForceVecd computeWrench(const rbd::MultiBodyConfig & mbc, + const tasks::qp::BilateralContact & contact, + Eigen::VectorXd lambdaVec, int pos); + + int robotIndex_, bodyIndex_, alphaDBegin_; + std::vector contactBodies_, contactBodiesPrev_; + double dt_; + double gainForceP_, gainForceD_; + double gainCoupleP_, gainCoupleD_; + + rbd::Jacobian jac_; + Eigen::MatrixXd jacMat_; + Eigen::Vector6d error_; + Eigen::Vector6d normalAcc_; + Eigen::Vector6d dimWeight_; + + // the following flag indicates if dimWeight values are related or not to the local frame + bool local_; + + Eigen::MatrixXd Q_; + Eigen::VectorXd C_; + // cache + Eigen::MatrixXd preQ_; + Eigen::Vector6d preC_; +}; + +class TASKS_DLLAPI AdmittanceTask : public AdmittanceTaskCommon +{ +public: + AdmittanceTask(const std::vector & mbs, int robotIndex, + const std::string & bodyName, + const Eigen::Vector3d & bodyPoint, + double timeStep, double gainForceP, double gainForceD, + double gainCoupleP, double gainCoupleD, double weight); + + virtual std::string nameTask() const override + { + return "AdmittanceTask"; + } + + void measuredWrench(const sva::ForceVecd wrench) + { + measuredWrench_ = wrench; + } + + const sva::ForceVecd & measuredWrench() const + { + return measuredWrench_; + } + + const sva::ForceVecd & calculatedWrench() const + { + return calculatedWrench_; + } + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + +private: + + sva::ForceVecd measuredWrench_, measuredWrenchPrev_; + sva::ForceVecd calculatedWrench_, calculatedWrenchPrev_; +}; + +class TASKS_DLLAPI NullSpaceAdmittanceTask : public AdmittanceTaskCommon +{ +public: + NullSpaceAdmittanceTask(const std::vector & mbs, int robotIndex, + const std::string & bodyName, + const Eigen::Vector3d & bodyPoint, + double timeStep, double gainForceP, double gainForceD, + double gainCoupleP, double gainCoupleD, double weight); + + virtual std::string nameTask() const override + { + return "NullSpaceAdmittanceTask"; + } + + void measuredWrench(const std::string & bodyName, const sva::ForceVecd & wrench); + + void measuredWrenches(const std::map & wrenches); + + const sva::ForceVecd & measuredWrench(const std::string & bodyName) const + { + return measuredWrenches_.at(bodyName); + } + + const std::map & measuredWrenches() const + { + return measuredWrenches_; + } + + const Eigen::Vector3d & calculatedForce(const std::string & bodyName) const + { + return calculatedForces_.at(bodyName); + } + + const std::map & calculatedForces() const + { + return calculatedForces_; + } + + const sva::ForceVecd & calculatedWrench() const + { + return calculatedBodyWrench_; + } + + void fdistRatio(const Eigen::Vector3d & ratio) + { + fdistRatio_ = ratio; + } + + const Eigen::Vector3d & fdistRatio() const + { + return fdistRatio_; + } + + 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); + +private: + + int nrBodies_; + + // std::set bodies_; + + std::map measuredWrenches_; + std::map calculatedForces_; + + sva::ForceVecd calculatedBodyWrench_; + sva::ForceVecd measuredBodyWrenchPrev_; + sva::ForceVecd calculatedBodyWrenchPrev_; + + Eigen::Vector3d fdistRatio_; + + Eigen::Vector3d projForceErr_; +}; + +class TASKS_DLLAPI ForceDistributionTaskCommon : public Task +{ + public: + ForceDistributionTaskCommon(const std::vector & mbs, + int robotIndex, double weight); + + virtual std::pair begin() const = 0; + + void fdistRatio(const std::string & bodyName, const Eigen::Vector3d & ratio) + { + //fdistRatios_.at(bodyName) = ratio; + fdistRatios_[bodyName] = ratio; + } + + void fdistRatios(const std::map & ratios); + + Eigen::Vector3d fdistRatio(const std::string & bodyName) const; + + const std::map & fdistRatios() const + { + return fdistRatios_; + } + + Eigen::Vector3d refForce(const std::string & bodyName) const; + + const std::map & refForces() const + { + return refForces_; + } + + virtual void updateNrVars(const std::vector & mbs, + const SolverData& data); + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual const Eigen::MatrixXd & Q() const + { + return Q_; + } + + virtual const Eigen::VectorXd & C() const + { + return C_; + } + + struct BodyLambda + { + std::string body; + int begin; + int size; + }; + + protected: + + int robotIndex_, lambdaBegin_, nrBodies_; + std::vector contactBodies_, contactBodiesPrev_; + + std::map fdistRatios_; + std::map refForces_; + + Eigen::MatrixXd W_; + Eigen::MatrixXd fdistRatioMat_; + Eigen::MatrixXd A_; + + Eigen::MatrixXd Q_; + Eigen::VectorXd C_; +}; + +class TASKS_DLLAPI ForceDistributionTaskOriginal : public ForceDistributionTaskCommon +{ +public: + ForceDistributionTaskOriginal(const std::vector & mbs, + int robotIndex, double weight); + + virtual std::string nameTask() const override + { + return "ForceDistributionTaskOriginal"; + } + + virtual std::pair begin() const + { + return std::make_pair(0, 0); + } + + 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: + + int alphaDBegin_; + + double gAcc_; + double totalMass_; + + rbd::CoMJacobian comJac_; + + Eigen::MatrixXd comJacMat_; + Eigen::Vector3d normalAcc_; + + // cache + Eigen::VectorXd CSum_; +}; + +class TASKS_DLLAPI ForceDistributionTaskOptimized : public ForceDistributionTaskCommon +{ + public: + ForceDistributionTaskOptimized(const std::vector & mbs, + int robotIndex, double weight); + + virtual std::string nameTask() const override + { + return "ForceDistributionTaskOriginal"; + } + + virtual std::pair begin() const + { + return std::make_pair(lambdaBegin_, lambdaBegin_); + } + + 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; + + protected: + + // cache + Eigen::MatrixXd SumMat_; + Eigen::MatrixXd preA_; +}; + +class TASKS_DLLAPI ZMPBasedCoMTask : public Task +{ + public: + ZMPBasedCoMTask(const std::vector & mbs, int robotIndex, + const Eigen::Vector3d & com, const Eigen::Vector3d & zmp, + double weight); + + virtual std::string nameTask() const override + { + return "ZMPBasedCoMTask"; + } + + virtual std::pair begin() const + { + return std::make_pair(alphaDBegin_, alphaDBegin_); + } + + void com(const Eigen::Vector3d & com) + { + com_ = com; + } + + const Eigen::Vector3d & com() const + { + return com_; + } + + void zmp(const Eigen::Vector3d & zmp) + { + zmp_ = zmp; + } + + const Eigen::Vector3d & zmp() const + { + return zmp_; + } + + void ddcom(const Eigen::Vector3d & ddcom) + { + ddcom_ = ddcom; + } + + const Eigen::Vector3d & ddcom() const + { + return ddcom_; + } + + void dimWeight(const Eigen::Vector3d & dim) + { + dimWeight_ = dim; + } + + const Eigen::Vector3d & dimWeight() const + { + return dimWeight_; + } + + virtual void updateNrVars(const std::vector & mbs, + const SolverData& data); + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual const Eigen::MatrixXd & Q() const + { + return Q_; + } + + virtual const Eigen::VectorXd & C() const + { + return C_; + } + + private: + + int robotIndex_, alphaDBegin_; + + Eigen::Vector3d com_, ddcom_, zmp_; + double gAcc_; + + Eigen::Vector3d dimWeight_; + + rbd::CoMJacobian jac_; + + Eigen::MatrixXd Q_; + Eigen::VectorXd C_; + // cache + Eigen::MatrixXd jacMat_; + Eigen::MatrixXd preQ_; + Eigen::Vector3d CSum_; + Eigen::Vector3d normalAcc_; +}; + +class TASKS_DLLAPI ZMPTask : public Task +{ + public: + ZMPTask(const std::vector & mbs, int robotIndex, + const Eigen::Vector3d & zmp, double weight); + + virtual std::string nameTask() const override + { + return "ZMPTask"; + } + + virtual std::pair begin() const + { + return std::make_pair(lambdaBegin_, lambdaBegin_); + } + + void zmp(const Eigen::Vector3d & zmp) + { + zmp_ = zmp; + } + + const Eigen::Vector3d & zmp() const + { + return zmp_; + } + + const Eigen::Vector3d & totalForce() const + { + return totalForce_; + } + + const Eigen::Vector3d & totalMomentZMP() const + { + return totalMomentZMP_; + } + + void dimWeight(const Eigen::Vector3d & dim) + { + dimWeight_ = dim; + } + + const Eigen::Vector3d & dimWeight() const + { + return dimWeight_; + } + + virtual void updateNrVars(const std::vector & mbs, + const SolverData& data); + + virtual void update(const std::vector & mbs, + const std::vector & mbcs, + const SolverData & data); + + virtual const Eigen::MatrixXd & Q() const + { + return Q_; + } + + virtual const Eigen::VectorXd & C() const + { + return C_; + } + + struct BodyLambda + { + std::string body; + int begin; + int size; + }; + + private: + + int robotIndex_, lambdaBegin_, nrBodies_; + std::vector contactBodies_, contactBodiesPrev_; + Eigen::Vector3d dimWeight_; + + Eigen::Vector3d zmp_; + + Eigen::Vector3d totalForce_; + Eigen::Vector3d totalMomentZMP_; + + Eigen::MatrixXd W_; + Eigen::MatrixXd pW_; + + Eigen::MatrixXd Q_; + Eigen::VectorXd C_; + // cache + Eigen::MatrixXd preQ_; +}; + +// Pending to finish... + +class TASKS_DLLAPI YawMomentCompensationTask : public Task +{ + public: + YawMomentCompensationTask(const std::vector& mbs, int robotIndex, double timeStep, + double gainKp, double gainKd, double gainKi, double gainKii, double weight); + + virtual std::string nameTask() const override + { + return "YawMomentCompensationTask"; + } + + virtual std::pair begin() const + { + return std::make_pair(alphaDBegin_, alphaDBegin_); + } + + void deltaTauP(const double delta_tau_p) + { + delta_tau_p_ = delta_tau_p; + } + + double deltaTauP() + { + return delta_tau_p_; + } + + void zmp(const Eigen::Vector3d p) + { + zmp_ = p; + } + + const Eigen::Vector3d zmp() const + { + return zmp_; + } + + void setGains(double gainKp, double gainKd, double gainKi, double gainKii) + { + gainKp_ = gainKp; + gainKd_ = gainKd; + gainKi_ = gainKi; + gainKii_ = gainKii; + } + + virtual void updateNrVars(const std::vector& mbs, + const SolverData& data); + + virtual void update(const std::vector& mbs, + const std::vector& mbcs, + const SolverData& data); + + virtual const Eigen::MatrixXd& Q() const + { + return Q_; + } + + virtual const Eigen::VectorXd& C() const + { + return C_; + } + + private: + + double computeTauP(); + sva::ForceVecd computeWrench(const rbd::MultiBodyConfig& mbc, + const tasks::qp::BilateralContact& contact, Eigen::VectorXd lambdaVec, int pos); + Eigen::Matrix3d skewMatrix(const Eigen::Vector3d& v); + + int robotIndex_, alphaDBegin_; + + double dt_; + double gainKp_, gainKd_, gainKi_, gainKii_; + + Eigen::Vector3d com_prev_; + Eigen::Vector3d zmp_, zmp_prev_; + double delta_tau_p_, delta_tau_p_prev_; + double Lpz_, iLpz_; + + rbd::CentroidalMomentumMatrix centroidalMomentumMatrix_; + Eigen::MatrixXd jacMat_; + + Eigen::MatrixXd Q_; + Eigen::VectorXd C_; +}; + } // namespace qp } // namespace tasks diff --git a/src/Tasks/Tasks.h b/src/Tasks/Tasks.h index df3f3a0fa..c571f9deb 100644 --- a/src/Tasks/Tasks.h +++ b/src/Tasks/Tasks.h @@ -592,6 +592,8 @@ class TASKS_DLLAPI OrientationTrackingTask Eigen::MatrixXd jacDotMat_; }; + + class TASKS_DLLAPI RelativeDistTask { public: @@ -646,6 +648,8 @@ class TASKS_DLLAPI RelativeDistTask RelativeBodiesInfo rbInfo_[2]; }; + + class TASKS_DLLAPI VectorOrientationTask { public: @@ -684,4 +688,5 @@ class TASKS_DLLAPI VectorOrientationTask Eigen::MatrixXd jacMat_; }; + } // namespace tasks diff --git a/tests/QPMultiRobotTest.cpp b/tests/QPMultiRobotTest.cpp index 212d1df29..7d968216a 100644 --- a/tests/QPMultiRobotTest.cpp +++ b/tests/QPMultiRobotTest.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -176,6 +177,10 @@ BOOST_AUTO_TEST_CASE(TwoArmDDynamicContactTest) forwardKinematics(mb1, mbc1Init); forwardVelocity(mb1, mbc1Init); + auto fd1 = std::make_shared(mb1); + fd1->computeH(mb1, mbc1Init); + fd1->computeC(mb1, mbc1Init); + std::tie(mb2, mbc2Init) = makeZXZArm(false); Vector3d mb2InitPos = mbc1Init.bodyPosW.back().translation(); Quaterniond mb2InitOri(RotY(cst::pi() / 2.)); @@ -184,12 +189,17 @@ BOOST_AUTO_TEST_CASE(TwoArmDDynamicContactTest) forwardKinematics(mb2, mbc2Init); forwardVelocity(mb2, mbc2Init); + auto fd2 = std::make_shared(mb2); + fd2->computeH(mb2, mbc2Init); + fd2->computeC(mb2, mbc2Init); + sva::PTransformd X_0_b1(mbc1Init.bodyPosW.back()); sva::PTransformd X_0_b2(mbc2Init.bodyPosW.front()); sva::PTransformd X_b1_b2(X_0_b2 * X_0_b1.inv()); std::vector mbs = {mb1, mb2}; std::vector mbcs = {mbc1Init, mbc2Init}; + std::vector> fds = {fd1, fd2}; // Test ContactAccConstr constraint // Also test PositionTask on the second robot @@ -240,12 +250,13 @@ BOOST_AUTO_TEST_CASE(TwoArmDDynamicContactTest) std::vector> torqueMax1 = {{}, {Inf}, {Inf}, {Inf}}; std::vector> torqueMin2 = {{0., 0., 0., 0., 0., 0.}, {-Inf}, {-Inf}, {-Inf}}; std::vector> torqueMax2 = {{0., 0., 0., 0., 0., 0.}, {Inf}, {Inf}, {Inf}}; + std::vector> torqueDtMin1 = {{}, {-Inf}, {-Inf}, {-Inf}}; std::vector> torqueDtMax1 = {{}, {Inf}, {Inf}, {Inf}}; std::vector> torqueDtMin2 = {{0., 0., 0., 0., 0., 0.}, {-Inf}, {-Inf}, {-Inf}}; std::vector> torqueDtMax2 = {{0., 0., 0., 0., 0., 0.}, {Inf}, {Inf}, {Inf}}; - qp::MotionConstr motion1(mbs, 0, {torqueMin1, torqueMax1}, {torqueDtMin1, torqueDtMax1}, 0.005); - qp::MotionConstr motion2(mbs, 1, {torqueMin2, torqueMax2}, {torqueDtMin2, torqueDtMax2}, 0.005); + qp::MotionConstr motion1(mbs, 0, fd1, {torqueMin1, torqueMax1}, {torqueDtMin1, torqueDtMax1}, 0.005); + qp::MotionConstr motion2(mbs, 1, fd2, {torqueMin2, torqueMax2}, {torqueDtMin2, torqueDtMax2}, 0.005); qp::PositiveLambda plCstr; motion1.addToSolver(solver); @@ -308,6 +319,9 @@ BOOST_AUTO_TEST_CASE(TwoArmDDynamicContactTest) forwardKinematics(mbs[r], mbcs[r]); forwardVelocity(mbs[r], mbcs[r]); + + fds[r]->computeH(mbs[r], mbcs[r]); + fds[r]->computeC(mbs[r], mbcs[r]); } // check that the link hold sva::PTransformd X_0_b1_post(mbcs[0].bodyPosW.back()); @@ -498,13 +512,22 @@ BOOST_AUTO_TEST_CASE(TorqueTaskTest) forwardKinematics(mb1, mbc1Init); forwardVelocity(mb1, mbc1Init); + auto fd1 = std::make_shared(mb1); + fd1->computeH(mb1, mbc1Init); + fd1->computeC(mb1, mbc1Init); + std::tie(mb2, mbc2Init) = makeZXZArm(false, sva::PTransformd(sva::RotZ(cst::pi() / 2.), Vector3d(0.5, 0., 0.))); forwardKinematics(mb2, mbc2Init); forwardVelocity(mb2, mbc2Init); + auto fd2 = std::make_shared(mb2); + fd2->computeH(mb2, mbc2Init); + fd2->computeC(mb2, mbc2Init); + std::vector mbs = {mb1, mb2}; std::vector mbcs = {mbc1Init, mbc2Init}; + std::vector> fds = {fd1, fd2}; // Test ContactAccConstr constraint // Also test PositionTask on the second robot @@ -542,7 +565,7 @@ BOOST_AUTO_TEST_CASE(TorqueTaskTest) qp::PostureTask posture1Task(mbs, 0, mbc1Init.q, 0.1, 10.); qp::PostureTask posture2Task(mbs, 1, mbc2Init.q, 0.1, 10.); - qp::TorqueTask tt(mbs, 0, tb, tdb, 0.005, 1); + qp::TorqueTask tt(mbs, 0, fd1, tb, tdb, 0.005, 1); solver.addTask(&posture1Task); solver.addTask(&posture2Task); @@ -562,6 +585,9 @@ BOOST_AUTO_TEST_CASE(TorqueTaskTest) forwardKinematics(mbs[r], mbcs[r]); forwardVelocity(mbs[r], mbcs[r]); + + fds[r]->computeH(mbs[r], mbcs[r]); + fds[r]->computeC(mbs[r], mbcs[r]); } } solver.removeTask(&posture1Task); diff --git a/tests/QPSolverTest.cpp b/tests/QPSolverTest.cpp index c4da87f36..f283366b9 100644 --- a/tests/QPSolverTest.cpp +++ b/tests/QPSolverTest.cpp @@ -246,6 +246,10 @@ BOOST_AUTO_TEST_CASE(QPConstrTest) forwardKinematics(mbEnv, mbcEnv); forwardVelocity(mbEnv, mbcEnv); + auto fd = std::make_shared(mb); + fd->computeH(mb, mbcInit); + fd->computeC(mb, mbcInit); + std::vector mbs = {mb, mbEnv}; std::vector mbcs = {mbcInit, mbcEnv}; @@ -295,6 +299,9 @@ BOOST_AUTO_TEST_CASE(QPConstrTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); } BOOST_CHECK_SMALL((posTask.eval() - evalPos).norm(), 0.00001); @@ -340,6 +347,9 @@ BOOST_AUTO_TEST_CASE(QPConstrTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); } BOOST_CHECK_SMALL((posTask.eval() - evalPos).norm(), 0.00001); @@ -364,7 +374,7 @@ BOOST_AUTO_TEST_CASE(QPConstrTest) std::vector> torqueMax = {{}, {Inf}, {Inf}, {Inf}}; std::vector> torqueDtMin = {{}, {-Inf}, {-Inf}, {-Inf}}; std::vector> torqueDtMax = {{}, {Inf}, {Inf}, {Inf}}; - qp::MotionConstr motionCstr(mbs, 0, {torqueMin, torqueMax}, {torqueDtMin, torqueDtMax}, 0.005); + qp::MotionConstr motionCstr(mbs, 0, fd, {torqueMin, torqueMax}, {torqueDtMin, torqueDtMax}, 0.005); qp::PositiveLambda plCstr; motionCstr.addToSolver(solver); @@ -389,6 +399,9 @@ BOOST_AUTO_TEST_CASE(QPConstrTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); } motionCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec()); motionCstr.torque(mbs, mbcs); @@ -437,6 +450,9 @@ BOOST_AUTO_TEST_CASE(QPConstrTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); } BOOST_CHECK_SMALL(posTask.eval().norm(), 5e-5); @@ -730,6 +746,10 @@ BOOST_AUTO_TEST_CASE(QPTorqueLimitsTest) forwardKinematics(mb, mbcInit); forwardVelocity(mb, mbcInit); + auto fd = std::make_shared(mb); + fd->computeH(mb, mbcInit); + fd->computeC(mb, mbcInit); + std::vector mbs = {mb}; std::vector mbcs = {mbcInit}; @@ -746,7 +766,7 @@ BOOST_AUTO_TEST_CASE(QPTorqueLimitsTest) constexpr double dt = 0.001; - qp::MotionConstr motionCstr(mbs, 0, {lBound, uBound}, {lBoundDt, uBoundDt}, dt); + qp::MotionConstr motionCstr(mbs, 0, fd, {lBound, uBound}, {lBoundDt, uBoundDt}, dt); qp::PositiveLambda plCstr; // Test add*Constraint @@ -775,6 +795,8 @@ BOOST_AUTO_TEST_CASE(QPTorqueLimitsTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); motionCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec()); motionCstr.torque(mbs, mbcs); for(int i = 0; i < 3; ++i) @@ -792,6 +814,8 @@ BOOST_AUTO_TEST_CASE(QPTorqueLimitsTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); motionCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec()); motionCstr.torque(mbs, mbcs); for(int i = 0; i < 3; ++i) @@ -811,7 +835,7 @@ BOOST_AUTO_TEST_CASE(QPTorqueLimitsTest) upoly << 30, 1.; std::vector> lBoundPoly = {{null}, {lpoly}, {lpoly}, {lpoly}}; std::vector> uBoundPoly = {{null}, {upoly}, {upoly}, {upoly}}; - qp::MotionPolyConstr motionPolyCstr(mbs, 0, {lBoundPoly, uBoundPoly}); + qp::MotionPolyConstr motionPolyCstr(mbs, 0, fd, {lBoundPoly, uBoundPoly}); motionPolyCstr.addToSolver(solver); BOOST_CHECK_EQUAL(solver.nrGenInequalityConstraints(), 1); @@ -831,6 +855,8 @@ BOOST_AUTO_TEST_CASE(QPTorqueLimitsTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); motionPolyCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec()); motionPolyCstr.torque(mbs, mbcs); for(int i = 0; i < 3; ++i) @@ -849,6 +875,8 @@ BOOST_AUTO_TEST_CASE(QPTorqueLimitsTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); motionPolyCstr.computeTorque(solver.alphaDVec(), solver.lambdaVec()); motionPolyCstr.torque(mbs, mbcs); for(int i = 0; i < 3; ++i) @@ -1105,6 +1133,11 @@ BOOST_AUTO_TEST_CASE(QPBilatContactTest) forwardKinematics(mb, mbcInit); forwardVelocity(mb, mbcInit); + + auto fd = std::make_shared(mb); + fd->computeH(mb, mbcInit); + fd->computeC(mb, mbcInit); + forwardKinematics(mbEnv, mbcEnv); forwardVelocity(mbEnv, mbcEnv); @@ -1118,7 +1151,7 @@ BOOST_AUTO_TEST_CASE(QPBilatContactTest) std::vector> torqueMax = {{0., 0., 0., 0., 0., 0.}, {Inf}, {Inf}, {Inf}}; std::vector> torqueDtMin = {{0., 0., 0., 0., 0., 0.}, {-Inf}, {-Inf}, {-Inf}}; std::vector> torqueDtMax = {{0., 0., 0., 0., 0., 0.}, {Inf}, {Inf}, {Inf}}; - qp::MotionConstr motionCstr(mbs, 0, {torqueMin, torqueMax}, {torqueDtMin, torqueDtMax}, 0.005); + qp::MotionConstr motionCstr(mbs, 0, fd, {torqueMin, torqueMax}, {torqueDtMin, torqueDtMax}, 0.005); qp::PositiveLambda plCstr; qp::ContactAccConstr contCstrAcc; @@ -1169,6 +1202,9 @@ BOOST_AUTO_TEST_CASE(QPBilatContactTest) forwardKinematics(mbs[0], mbcs[0]); forwardVelocity(mbs[0], mbcs[0]); + + fd->computeH(mbs[0], mbcs[0]); + fd->computeC(mbs[0], mbcs[0]); } plCstr.removeFromSolver(solver);