From 75b8f4c28b667cf542ed88c8e6cb32a52fc8e45b Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 11:45:17 +0100 Subject: [PATCH 1/9] Update QPSolverData.h --- src/Tasks/QPSolverData.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/Tasks/QPSolverData.h b/src/Tasks/QPSolverData.h index c2f54b35b..6b6d666f5 100644 --- a/src/Tasks/QPSolverData.h +++ b/src/Tasks/QPSolverData.h @@ -143,9 +143,21 @@ 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_; From 49cdb8f9995715465ace9a62a97a3d813bc034f4 Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 11:47:18 +0100 Subject: [PATCH 2/9] Update QPSolver.h --- src/Tasks/QPSolver.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/Tasks/QPSolver.h b/src/Tasks/QPSolver.h index ec82f091a..1b945355d 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; From 5de7b9e0af2fbfce4d53c58c0b65b8c277e333db Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 11:49:32 +0100 Subject: [PATCH 3/9] Update QPSolver.h --- src/Tasks/QPSolver.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/Tasks/QPSolver.h b/src/Tasks/QPSolver.h index 1b945355d..cc3aa16d9 100644 --- a/src/Tasks/QPSolver.h +++ b/src/Tasks/QPSolver.h @@ -372,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_; }; From 915e7c7903e636bd2c53662ea004b872baaa3c44 Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 11:56:11 +0100 Subject: [PATCH 4/9] Update QPSolver.cpp --- src/QPSolver.cpp | 58 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/src/QPSolver.cpp b/src/QPSolver.cpp index a2472dc6a..b24c0220d 100644 --- a/src/QPSolver.cpp +++ b/src/QPSolver.cpp @@ -228,6 +228,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 +504,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_); From 40ab250b109a50c0bda523e2ecbdf31b0ee6d0cb Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 11:57:45 +0100 Subject: [PATCH 5/9] Add files via upload --- .../VariableVarsWeightedLeastSquaresTask.h | 108 ++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 src/Tasks/VariableVarsWeightedLeastSquaresTask.h diff --git a/src/Tasks/VariableVarsWeightedLeastSquaresTask.h b/src/Tasks/VariableVarsWeightedLeastSquaresTask.h new file mode 100644 index 000000000..15cee14fe --- /dev/null +++ b/src/Tasks/VariableVarsWeightedLeastSquaresTask.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 + From d9d37ece5f1f2d0b5852ca4d9439ee37233070b6 Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 11:58:55 +0100 Subject: [PATCH 6/9] Rename VariableVarsWeightedLeastSquaresTask.h to QPVariableVarsWeightedLeastSquaresTask.h --- ...eastSquaresTask.h => QPVariableVarsWeightedLeastSquaresTask.h} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/Tasks/{VariableVarsWeightedLeastSquaresTask.h => QPVariableVarsWeightedLeastSquaresTask.h} (100%) diff --git a/src/Tasks/VariableVarsWeightedLeastSquaresTask.h b/src/Tasks/QPVariableVarsWeightedLeastSquaresTask.h similarity index 100% rename from src/Tasks/VariableVarsWeightedLeastSquaresTask.h rename to src/Tasks/QPVariableVarsWeightedLeastSquaresTask.h From 6afaa045cbcaa827d360ddb7da832212dd9fd0c0 Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 12:06:00 +0100 Subject: [PATCH 7/9] Update Tasks.cpp --- src/Tasks.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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 */) {} From 1c312333c58edac6890c18e00c875293c5ae2763 Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 12:10:10 +0100 Subject: [PATCH 8/9] Update QPSolverData.h --- src/Tasks/QPSolverData.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Tasks/QPSolverData.h b/src/Tasks/QPSolverData.h index 6b6d666f5..61f91839c 100644 --- a/src/Tasks/QPSolverData.h +++ b/src/Tasks/QPSolverData.h @@ -160,7 +160,7 @@ class TASKS_DLLAPI SolverData 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 From 7ed1a0802ee92a9b8a31ca61d7bc4ed73db55a24 Mon Sep 17 00:00:00 2001 From: wuv-ogre Date: Wed, 16 May 2018 12:13:43 +0100 Subject: [PATCH 9/9] Update QPSolver.cpp --- src/QPSolver.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/QPSolver.cpp b/src/QPSolver.cpp index b24c0220d..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);