variables vars to qp

src/QPSolver.cpp

@@ -138,6 +138,9 @@ void QPSolver::nrVars(const std::vector<rbd::MultiBody>& 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<rbd::MultiBody>& mbs,
 	solver_->updateSize(data_.nrVars_, maxEqLines_, maxInEqLines_, maxGenInEqLines_);
 }
 
+
+void QPSolver::updateVariableVars( const std::vector<rbd::MultiBody>& 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<std::tuple<int, int, double>> 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_);

src/Tasks.cpp

@@ -1004,28 +1004,30 @@ 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());
 
 			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 */)
 {}
 

src/Tasks/QPSolver.h

@@ -92,6 +92,9 @@ class TASKS_DLLAPI QPSolver
 		std::vector<BilateralContact> bi);
 	int nrVars() const;
 
+	/// call if before solve if using variable vars
+	void updateVariableVars( const std::vector<rbd::MultiBody>& mbs );
+
 	/// call updateNrVars on all tasks
 	void updateTasksNrVars(const std::vector<rbd::MultiBody>& 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_;
 };

src/Tasks/QPSolverData.h

@@ -143,12 +143,24 @@ class TASKS_DLLAPI SolverData
 		return normalAccB_[robotIndex];
 	}
 
+	std::vector<int>& variableVars()
+	{
+		return variableVars_;
+	}
+
+	int variableVarsBegin(int robotIndex) const
+	{
+		return variableVarsBegin_[robotIndex];
+	}
+
 private:
 	std::vector<int> alphaD_; //< each robot alphaD vector size
 	std::vector<int> alphaDBegin_; //< each robot alphaD vector begin in x
+	std::vector<int> variableVars_; //< each robot variables vars size
+	std::vector<int> variableVarsBegin_; //< each robot variables vars begin in x
 	std::vector<int> lambda_; //< each contact lambda
 	std::vector<int> lambdaBegin_; //< each contact lambda vector begin in x
-	int totalAlphaD_, totalLambda_;
+	int totalAlphaD_, totalLambda_, totalVariableVars_;
 	int nrUniLambda_, nrBiLambda_;
 	int nrVars_; //< total number of var
 

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 <http://www.gnu.org/licenses/>.
+
+#pragma once
+
+// includes
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+
+// RBDyn
+//#include <RBDyn/Jacobian.h>
+//#include <RBDyn/CoM.h>
+
+// sch
+//#include <sch/Matrix/SCH_Types.h>
+
+// Tasks
+#include "QPSolver.h"
+
+// unique_ptr
+#include <memory>
+
+
+namespace tasks
+{
+namespace qp
+{
+
+class TASKS_DLLAPI VariableVarsWeightedLeastSquaresTask : public Task
+{
+private:
+    //const std::vector<rbd::MultiBody>& mbs_;
+    Eigen::VectorXd dimWeight_;
+    int robotIndex_;
+    int variableVarsPos, variableVarsBegin_;
+
+    Eigen::MatrixXd Q_; // < diagonal dimWeight_
+    Eigen::VectorXd C_; // < zero
+
+public:
+    VariableVarsWeightedLeastSquaresTask(const std::vector<rbd::MultiBody>& 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<int, int> 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<rbd::MultiBody>& mbs,
+                        const std::vector<rbd::MultiBodyConfig>& 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<rbd::MultiBody>& 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
+