diff --git a/src/QPSolver.cpp b/src/QPSolver.cpp index a2472dc6a..31acb5a5a 100644 --- a/src/QPSolver.cpp +++ b/src/QPSolver.cpp @@ -138,6 +138,9 @@ void QPSolver::nrVars(const std::vector& mbs, data_.alphaD_.resize(mbs.size()); data_.alphaDBegin_.resize(mbs.size()); + data_.variableVars_.resize(mbs.size(), 0); + data_.variableVarsBegin_.resize(mbs.size(), 0); + data_.uniCont_ = std::move(uni); data_.biCont_ = std::move(bi); @@ -228,6 +231,57 @@ void QPSolver::nrVars(const std::vector& mbs, solver_->updateSize(data_.nrVars_, maxEqLines_, maxInEqLines_, maxGenInEqLines_); } + +void QPSolver::updateVariableVars( const std::vector& mbs ) +{ + for(int& nrVariableVars : data_.variableVars_) + { + nrVariableVars = 0; + } + + for(Task* t: tasks_) + { + t->addNrVariableVars( data_ ); + } + + data_.totalVariableVars_ = 0; + for(const int& nrVariableVars : data_.variableVars_) + { + data_.totalVariableVars_ += nrVariableVars; + } + + std::vector> dependencies; + int cumAlphaD = 0; + for(std::size_t r = 0; r < mbs.size(); ++r) + { + const rbd::MultiBody& mb = mbs[r]; + data_.alphaDBegin_[r] = cumAlphaD; + data_.variableVarsBegin_[r] = cumAlphaD + data_.alphaD_[r]; + cumAlphaD += data_.alphaD_[r] + data_.variableVars_[r]; + + if(mb.nrDof() > 0) + { + for(const auto & j : mb.joints()) + { + if(j.isMimic()) + { + dependencies.emplace_back(data_.alphaDBegin_[r] + mb.jointPosInDof(mb.jointIndexByName(j.mimicName())), + data_.alphaDBegin_[r] + mb.jointPosInDof(mb.jointIndexByName(j.name())), + j.mimicMultiplier()); + } + } + } + } + + data_.nrVars_ = data_.totalAlphaD_ + data_.totalVariableVars_ + data_.totalLambda_; + solver_->setDependencies(data_.nrVars_, dependencies); + solver_->updateSize(data_.nrVars_, maxEqLines_, maxInEqLines_, maxGenInEqLines_); + + updateNrVars(mbs); + + // will break QPSolver::alphaDVec() since (alphaD variableVars | alphaD variableVars | ... ) +} + int QPSolver::nrVars() const { @@ -453,7 +507,14 @@ Eigen::VectorXd QPSolver::alphaDVec(int rIndex) const data_.alphaD_[rIndex]); } + +Eigen::VectorXd QPSolver::variableVarsVec(int rIndex) const +{ + return solver_->result().segment(data_.variableVarsBegin_[rIndex], + data_.variableVars_[rIndex]); +} + Eigen::VectorXd QPSolver::lambdaVec() const { return solver_->result().segment(data_.lambdaBegin(), data_.totalLambda_); diff --git a/src/Tasks.cpp b/src/Tasks.cpp index e34c92d07..62b1c5690 100644 --- a/src/Tasks.cpp +++ b/src/Tasks.cpp @@ -1004,15 +1004,15 @@ void PostureTask::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& m // we drop the first joint (fixed or free flyier). for(int i = 1; i < mb.nrJoints(); ++i) { - // if dof == 1 is a prismatic/revolute joint - // else if dof == 4 is a spherical one + // if params == 1 is a prismatic/revolute joint + // else if params == 4 is a spherical one // else is a fixed one - if(mb.joint(i).dof() == 1) + if(mb.joint(i).params() == 1) { eval_(pos) = q_[i][0] - mbc.q[i][0]; ++pos; } - else if(mb.joint(i).dof() == 4) + else if(mb.joint(i).params() == 4) { Matrix3d orid( Quaterniond(q_[i][0], q_[i][1], q_[i][2], q_[i][3]).matrix()); @@ -1020,12 +1020,14 @@ void PostureTask::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& m Vector3d err = sva::rotationError(mbc.jointConfig[i].rotation(), orid); eval_.segment(pos, 3) = err; + pos += 3; } } } + void PostureTask::updateDot(const rbd::MultiBody& /* mb */, const rbd::MultiBodyConfig& /* mbc */) {} diff --git a/src/Tasks/QPSolver.h b/src/Tasks/QPSolver.h index ec82f091a..cc3aa16d9 100644 --- a/src/Tasks/QPSolver.h +++ b/src/Tasks/QPSolver.h @@ -92,6 +92,9 @@ class TASKS_DLLAPI QPSolver std::vector bi); int nrVars() const; + /// call if before solve if using variable vars + void updateVariableVars( const std::vector& mbs ); + /// call updateNrVars on all tasks void updateTasksNrVars(const std::vector& mbs) const; /// call updateNrVars on all constraints @@ -135,6 +138,8 @@ class TASKS_DLLAPI QPSolver Eigen::VectorXd alphaDVec() const; Eigen::VectorXd alphaDVec(int rIndex) const; + Eigen::VectorXd variableVarsVec(int rIndex) const; + Eigen::VectorXd lambdaVec() const; Eigen::VectorXd lambdaVec(int cIndex) const; @@ -367,6 +372,9 @@ class TASKS_DLLAPI Task virtual const Eigen::MatrixXd& Q() const = 0; virtual const Eigen::VectorXd& C() const = 0; + /// override and add to data.variableVars() if want additional variables + virtual void addNrVariableVars( SolverData& data ) { } + private: double weight_; }; diff --git a/src/Tasks/QPSolverData.h b/src/Tasks/QPSolverData.h index c2f54b35b..61f91839c 100644 --- a/src/Tasks/QPSolverData.h +++ b/src/Tasks/QPSolverData.h @@ -143,12 +143,24 @@ class TASKS_DLLAPI SolverData return normalAccB_[robotIndex]; } + std::vector& variableVars() + { + return variableVars_; + } + + int variableVarsBegin(int robotIndex) const + { + return variableVarsBegin_[robotIndex]; + } + private: std::vector alphaD_; //< each robot alphaD vector size std::vector alphaDBegin_; //< each robot alphaD vector begin in x + std::vector variableVars_; //< each robot variables vars size + std::vector variableVarsBegin_; //< each robot variables vars begin in x std::vector lambda_; //< each contact lambda std::vector lambdaBegin_; //< each contact lambda vector begin in x - int totalAlphaD_, totalLambda_; + int totalAlphaD_, totalLambda_, totalVariableVars_; int nrUniLambda_, nrBiLambda_; int nrVars_; //< total number of var diff --git a/src/Tasks/QPVariableVarsWeightedLeastSquaresTask.h b/src/Tasks/QPVariableVarsWeightedLeastSquaresTask.h new file mode 100644 index 000000000..15cee14fe --- /dev/null +++ b/src/Tasks/QPVariableVarsWeightedLeastSquaresTask.h @@ -0,0 +1,108 @@ +// Copyright 2012-2016 CNRS-UM LIRMM, CNRS-AIST JRL +// +// This file is part of Tasks. +// +// Tasks is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// Tasks is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with Tasks. If not, see . + +#pragma once + +// includes +// Eigen +#include +#include + +// RBDyn +//#include +//#include + +// sch +//#include + +// Tasks +#include "QPSolver.h" + +// unique_ptr +#include + + +namespace tasks +{ +namespace qp +{ + +class TASKS_DLLAPI VariableVarsWeightedLeastSquaresTask : public Task +{ +private: + //const std::vector& mbs_; + Eigen::VectorXd dimWeight_; + int robotIndex_; + int variableVarsPos, variableVarsBegin_; + + Eigen::MatrixXd Q_; // < diagonal dimWeight_ + Eigen::VectorXd C_; // < zero + +public: + VariableVarsWeightedLeastSquaresTask(const std::vector& mbs, int robotIndex, double weight) + : Task(weight), + robotIndex_(robotIndex) + { + } + + virtual ~VariableVarsWeightedLeastSquaresTask(){} + + virtual void addNrVariableVars( SolverData& data ) override + { + variableVarsPos = data.variableVars()[robotIndex_]; + data.variableVars()[robotIndex_] += dimWeight_.rows(); + } + + virtual std::pair begin() const override + { + return std::make_pair(variableVarsBegin_, variableVarsBegin_); + } + + void dimWeight(const Eigen::VectorXd& dim) + { + dimWeight_ = dim; + } + + const Eigen::VectorXd& dimWeight() const + { + return dimWeight_; + } + + virtual void update(const std::vector& mbs, + const std::vector& mbcs, + const SolverData& data) override + { + // don't need to do this every solver update. + Q_ = dimWeight_.asDiagonal(); + C_ = Eigen::VectorXd::Zero( dimWeight_.rows() ); + } + + virtual void updateNrVars(const std::vector& mbs, + const SolverData& data) override + { + variableVarsBegin_ = data.variableVarsBegin(robotIndex_) + variableVarsPos; + } + + virtual const Eigen::MatrixXd& Q() const override { return Q_; } + virtual const Eigen::VectorXd& C() const override { return C_; } +}; + + +} // namespace qp + +} // namespace tasks +