Commit 2443dae9 authored by Jean-Baptiste Mouret's avatar Jean-Baptiste Mouret
Browse files

move everything to shared_ptr

parent e30037f3
...@@ -69,6 +69,8 @@ IF(EIGEN_NO_AUTOMATIC_RESIZING) ...@@ -69,6 +69,8 @@ IF(EIGEN_NO_AUTOMATIC_RESIZING)
ADD_DEFINITIONS(-DEIGEN_NO_AUTOMATIC_RESIZING) ADD_DEFINITIONS(-DEIGEN_NO_AUTOMATIC_RESIZING)
ENDIF(EIGEN_NO_AUTOMATIC_RESIZING) ENDIF(EIGEN_NO_AUTOMATIC_RESIZING)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
# Project dependencies # Project dependencies
ADD_PROJECT_DEPENDENCY(pinocchio 2.3.1 REQUIRED ADD_PROJECT_DEPENDENCY(pinocchio 2.3.1 REQUIRED
PKG_CONFIG_REQUIRES "pinocchio >= 2.3.1") PKG_CONFIG_REQUIRES "pinocchio >= 2.3.1")
......
...@@ -25,15 +25,13 @@ ...@@ -25,15 +25,13 @@
namespace tsid namespace tsid
{ {
class TaskLevel class TaskLevel
{ {
public: public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
tasks::TaskBase & task; tasks::TaskBase & task;
math::ConstraintBase * constraint; std::shared_ptr<math::ConstraintBase> constraint;
// double weight;
unsigned int priority; unsigned int priority;
TaskLevel(tasks::TaskBase & task, TaskLevel(tasks::TaskBase & task,
...@@ -46,9 +44,9 @@ namespace tsid ...@@ -46,9 +44,9 @@ namespace tsid
EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW
contacts::ContactBase & contact; contacts::ContactBase & contact;
math::ConstraintBase * motionConstraint; std::shared_ptr<math::ConstraintBase> motionConstraint;
math::ConstraintInequality * forceConstraint; std::shared_ptr<math::ConstraintInequality> forceConstraint;
math::ConstraintEquality * forceRegTask; std::shared_ptr<math::ConstraintEquality> forceRegTask;
unsigned int index; /// index of 1st element of associated force variable in the force vector unsigned int index; /// index of 1st element of associated force variable in the force vector
ContactLevel(contacts::ContactBase & contact); ContactLevel(contacts::ContactBase & contact);
...@@ -63,7 +61,7 @@ namespace tsid ...@@ -63,7 +61,7 @@ namespace tsid
double time_end; double time_end;
double fMax_start; /// max normal force at time time_start double fMax_start; /// max normal force at time time_start
double fMax_end; /// max normal force at time time_end double fMax_end; /// max normal force at time time_end
ContactLevel * contactLevel; std::shared_ptr<ContactLevel> contactLevel;
}; };
class InverseDynamicsFormulationAccForce: class InverseDynamicsFormulationAccForce:
...@@ -140,7 +138,7 @@ namespace tsid ...@@ -140,7 +138,7 @@ namespace tsid
public: public:
void addTask(TaskLevel* task, void addTask(std::shared_ptr<TaskLevel> task,
double weight, double weight,
unsigned int priorityLevel); unsigned int priorityLevel);
...@@ -152,10 +150,10 @@ namespace tsid ...@@ -152,10 +150,10 @@ namespace tsid
Data m_data; Data m_data;
HQPData m_hqpData; HQPData m_hqpData;
std::vector<TaskLevel*> m_taskMotions; std::vector<std::shared_ptr<TaskLevel>> m_taskMotions;
std::vector<TaskLevel*> m_taskContactForces; std::vector<std::shared_ptr<TaskLevel>> m_taskContactForces;
std::vector<TaskLevel*> m_taskActuations; std::vector<std::shared_ptr<TaskLevel>> m_taskActuations;
std::vector<ContactLevel*> m_contacts; std::vector<std::shared_ptr<ContactLevel>> m_contacts;
double m_t; /// time double m_t; /// time
unsigned int m_k; /// number of contact-force variables unsigned int m_k; /// number of contact-force variables
unsigned int m_v; /// number of acceleration variables unsigned int m_v; /// number of acceleration variables
...@@ -163,14 +161,14 @@ namespace tsid ...@@ -163,14 +161,14 @@ namespace tsid
unsigned int m_eq; /// number of equality constraints unsigned int m_eq; /// number of equality constraints
unsigned int m_in; /// number of inequality constraints unsigned int m_in; /// number of inequality constraints
Matrix m_Jc; /// contact force Jacobian Matrix m_Jc; /// contact force Jacobian
math::ConstraintEquality m_baseDynamics; std::shared_ptr<math::ConstraintEquality> m_baseDynamics;
bool m_solutionDecoded; bool m_solutionDecoded;
Vector m_dv; Vector m_dv;
Vector m_f; Vector m_f;
Vector m_tau; Vector m_tau;
std::vector<ContactTransitionInfo*> m_contactTransitions; std::vector<std::shared_ptr<ContactTransitionInfo>> m_contactTransitions;
}; };
} }
......
...@@ -46,6 +46,7 @@ namespace tsid ...@@ -46,6 +46,7 @@ namespace tsid
ConstraintBase(const std::string & name, ConstraintBase(const std::string & name,
ConstRefMatrix A); ConstRefMatrix A);
virtual ~ConstraintBase() {}
virtual const std::string & name() const; virtual const std::string & name() const;
virtual unsigned int rows() const = 0; virtual unsigned int rows() const = 0;
......
...@@ -38,6 +38,7 @@ namespace tsid ...@@ -38,6 +38,7 @@ namespace tsid
ConstraintBound(const std::string & name, ConstraintBound(const std::string & name,
ConstRefVector lb, ConstRefVector lb,
ConstRefVector ub); ConstRefVector ub);
virtual ~ConstraintBound() {}
unsigned int rows() const; unsigned int rows() const;
unsigned int cols() const; unsigned int cols() const;
......
...@@ -39,6 +39,7 @@ namespace tsid ...@@ -39,6 +39,7 @@ namespace tsid
ConstraintEquality(const std::string & name, ConstraintEquality(const std::string & name,
ConstRefMatrix A, ConstRefMatrix A,
ConstRefVector b); ConstRefVector b);
virtual ~ConstraintEquality() {}
unsigned int rows() const; unsigned int rows() const;
unsigned int cols() const; unsigned int cols() const;
......
...@@ -40,6 +40,7 @@ namespace tsid ...@@ -40,6 +40,7 @@ namespace tsid
ConstRefMatrix A, ConstRefMatrix A,
ConstRefVector lb, ConstRefVector lb,
ConstRefVector ub); ConstRefVector ub);
virtual ~ConstraintInequality() {}
unsigned int rows() const; unsigned int rows() const;
unsigned int cols() const; unsigned int cols() const;
......
...@@ -18,6 +18,8 @@ ...@@ -18,6 +18,8 @@
#ifndef __invdyn_solvers_fwd_hpp__ #ifndef __invdyn_solvers_fwd_hpp__
#define __invdyn_solvers_fwd_hpp__ #define __invdyn_solvers_fwd_hpp__
#include <memory>
#include "tsid/config.hpp" #include "tsid/config.hpp"
#include "tsid/math/fwd.hpp" #include "tsid/math/fwd.hpp"
#include <pinocchio/container/aligned-vector.hpp> #include <pinocchio/container/aligned-vector.hpp>
...@@ -81,8 +83,8 @@ namespace tsid ...@@ -81,8 +83,8 @@ namespace tsid
{ return aligned_pair<T1,T2>(t1,t2); } { return aligned_pair<T1,T2>(t1,t2); }
typedef pinocchio::container::aligned_vector< aligned_pair<double, math::ConstraintBase*> > ConstraintLevel; typedef pinocchio::container::aligned_vector< aligned_pair<double, std::shared_ptr<math::ConstraintBase>> > ConstraintLevel;
typedef pinocchio::container::aligned_vector< aligned_pair<double, const math::ConstraintBase*> > ConstConstraintLevel; typedef pinocchio::container::aligned_vector< aligned_pair<double, std::shared_ptr<const math::ConstraintBase>>> ConstConstraintLevel;
typedef pinocchio::container::aligned_vector<ConstraintLevel> HQPData; typedef pinocchio::container::aligned_vector<ConstraintLevel> HQPData;
typedef pinocchio::container::aligned_vector<ConstConstraintLevel> ConstHQPData; typedef pinocchio::container::aligned_vector<ConstConstraintLevel> ConstHQPData;
......
...@@ -86,7 +86,7 @@ namespace tsid ...@@ -86,7 +86,7 @@ namespace tsid
const unsigned int n = cl0[0].second->cols(); const unsigned int n = cl0[0].second->cols();
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++) for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
{ {
const ConstraintBase* constr = it->second; auto constr = it->second;
assert(n==constr->cols()); assert(n==constr->cols());
if(constr->isEquality()) if(constr->isEquality())
neq += constr->rows(); neq += constr->rows();
...@@ -99,7 +99,7 @@ namespace tsid ...@@ -99,7 +99,7 @@ namespace tsid
int i_eq=0, i_in=0; int i_eq=0, i_in=0;
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++) for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
{ {
const ConstraintBase* constr = it->second; auto constr = it->second;
if(constr->isEquality()) if(constr->isEquality())
{ {
m_CE.middleRows(i_eq, constr->rows()) = constr->matrix(); m_CE.middleRows(i_eq, constr->rows()) = constr->matrix();
...@@ -137,7 +137,7 @@ namespace tsid ...@@ -137,7 +137,7 @@ namespace tsid
for(ConstraintLevel::const_iterator it=cl1.begin(); it!=cl1.end(); it++) for(ConstraintLevel::const_iterator it=cl1.begin(); it!=cl1.end(); it++)
{ {
const double & w = it->first; const double & w = it->first;
const ConstraintBase* constr = it->second; auto constr = it->second;
if(!constr->isEquality()) if(!constr->isEquality())
assert(false && "Inequalities in the cost function are not implemented yet"); assert(false && "Inequalities in the cost function are not implemented yet");
...@@ -196,7 +196,7 @@ namespace tsid ...@@ -196,7 +196,7 @@ namespace tsid
{ {
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++) for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
{ {
const ConstraintBase* constr = it->second; auto constr = it->second;
if(constr->checkConstraint(x)==false) if(constr->checkConstraint(x)==false)
{ {
if(constr->isEquality()) if(constr->isEquality())
......
...@@ -44,7 +44,7 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std ...@@ -44,7 +44,7 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std
bool verbose): bool verbose):
InverseDynamicsFormulationBase(name, robot, verbose), InverseDynamicsFormulationBase(name, robot, verbose),
m_data(robot.model()), m_data(robot.model()),
m_baseDynamics("base-dynamics", robot.nv()-robot.na(), robot.nv()), m_baseDynamics(new math::ConstraintEquality("base-dynamics", robot.nv()-robot.na(), robot.nv())),
m_solutionDecoded(false) m_solutionDecoded(false)
{ {
m_t = 0.0; m_t = 0.0;
...@@ -55,7 +55,7 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std ...@@ -55,7 +55,7 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std
m_in = 0; m_in = 0;
m_hqpData.resize(2); m_hqpData.resize(2);
m_Jc.setZero(m_k, m_v); m_Jc.setZero(m_k, m_v);
m_hqpData[0].push_back(make_pair<double, ConstraintBase*>(1.0, &m_baseDynamics)); m_hqpData[0].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(1.0, m_baseDynamics));
} }
...@@ -83,7 +83,7 @@ unsigned int InverseDynamicsFormulationAccForce::nIn() const ...@@ -83,7 +83,7 @@ unsigned int InverseDynamicsFormulationAccForce::nIn() const
void InverseDynamicsFormulationAccForce::resizeHqpData() void InverseDynamicsFormulationAccForce::resizeHqpData()
{ {
m_Jc.setZero(m_k, m_v); m_Jc.setZero(m_k, m_v);
m_baseDynamics.resize(m_u, m_v+m_k); m_baseDynamics->resize(m_u, m_v+m_k);
for(HQPData::iterator it=m_hqpData.begin(); it!=m_hqpData.end(); it++) for(HQPData::iterator it=m_hqpData.begin(); it!=m_hqpData.end(); it++)
{ {
for(ConstraintLevel::iterator itt=it->begin(); itt!=it->end(); itt++) for(ConstraintLevel::iterator itt=it->begin(); itt!=it->end(); itt++)
...@@ -94,7 +94,7 @@ void InverseDynamicsFormulationAccForce::resizeHqpData() ...@@ -94,7 +94,7 @@ void InverseDynamicsFormulationAccForce::resizeHqpData()
} }
void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl, void InverseDynamicsFormulationAccForce::addTask(std::shared_ptr<TaskLevel> tl,
double weight, double weight,
unsigned int priorityLevel) unsigned int priorityLevel)
{ {
...@@ -103,20 +103,20 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl, ...@@ -103,20 +103,20 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
const ConstraintBase & c = tl->task.getConstraint(); const ConstraintBase & c = tl->task.getConstraint();
if(c.isEquality()) if(c.isEquality())
{ {
tl->constraint = new ConstraintEquality(c.name(), c.rows(), m_v+m_k); tl->constraint = std::make_shared<ConstraintEquality>(c.name(), c.rows(), m_v+m_k);
if(priorityLevel==0) if(priorityLevel==0)
m_eq += c.rows(); m_eq += c.rows();
} }
else //if(c.isInequality()) else //if(c.isInequality())
{ {
tl->constraint = new ConstraintInequality(c.name(), c.rows(), m_v+m_k); tl->constraint = std::make_shared<ConstraintInequality>(c.name(), c.rows(), m_v+m_k);
if(priorityLevel==0) if(priorityLevel==0)
m_in += c.rows(); m_in += c.rows();
} }
// don't use bounds for now because EiQuadProg doesn't exploit them anyway // don't use bounds for now because EiQuadProg doesn't exploit them anyway
// else // else
// tl->constraint = new ConstraintBound(c.name(), m_v+m_k); // tl->constraint = new ConstraintBound(c.name(), m_v+m_k);
m_hqpData[priorityLevel].push_back(make_pair<double, ConstraintBase*>(weight, tl->constraint)); m_hqpData[priorityLevel].push_back(make_pair<double, std::shared_ptr<ConstraintBase>>(weight, tl->constraint));
} }
...@@ -137,7 +137,7 @@ bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task, ...@@ -137,7 +137,7 @@ bool InverseDynamicsFormulationAccForce::addMotionTask(TaskMotion & task,
if (transition_duration<0.0) if (transition_duration<0.0)
std::cerr << "transition_duration should be positive" << std::endl; std::cerr << "transition_duration should be positive" << std::endl;
TaskLevel *tl = new TaskLevel(task, priorityLevel); auto tl = std::make_shared<TaskLevel>(task, priorityLevel);
m_taskMotions.push_back(tl); m_taskMotions.push_back(tl);
addTask(tl, weight, priorityLevel); addTask(tl, weight, priorityLevel);
...@@ -160,7 +160,7 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task, ...@@ -160,7 +160,7 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
// This part is not used frequently so we can do some tests. // This part is not used frequently so we can do some tests.
if (transition_duration<0.0) if (transition_duration<0.0)
std::cerr << "transition_duration should be positive" << std::endl; std::cerr << "transition_duration should be positive" << std::endl;
TaskLevel *tl = new TaskLevel(task, priorityLevel); auto tl = std::make_shared<TaskLevel>(task, priorityLevel);
m_taskContactForces.push_back(tl); m_taskContactForces.push_back(tl);
addTask(tl, weight, priorityLevel); addTask(tl, weight, priorityLevel);
return true; return true;
...@@ -182,7 +182,7 @@ bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task, ...@@ -182,7 +182,7 @@ bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
if (transition_duration<0.0) if (transition_duration<0.0)
std::cerr << "transition_duration should be positive" << std::endl; std::cerr << "transition_duration should be positive" << std::endl;
TaskLevel *tl = new TaskLevel(task, priorityLevel); auto tl = std::make_shared<TaskLevel>(task, priorityLevel);
m_taskActuations.push_back(tl); m_taskActuations.push_back(tl);
if(priorityLevel > m_hqpData.size()) if(priorityLevel > m_hqpData.size())
...@@ -191,18 +191,18 @@ bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task, ...@@ -191,18 +191,18 @@ bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
const ConstraintBase & c = tl->task.getConstraint(); const ConstraintBase & c = tl->task.getConstraint();
if(c.isEquality()) if(c.isEquality())
{ {
tl->constraint = new ConstraintEquality(c.name(), c.rows(), m_v+m_k); tl->constraint = std::make_shared<ConstraintEquality>(c.name(), c.rows(), m_v+m_k);
if(priorityLevel==0) if(priorityLevel==0)
m_eq += c.rows(); m_eq += c.rows();
} }
else // an actuator bound becomes an inequality because actuator forces are not in the problem variables else // an actuator bound becomes an inequality because actuator forces are not in the problem variables
{ {
tl->constraint = new ConstraintInequality(c.name(), c.rows(), m_v+m_k); tl->constraint = std::make_shared<ConstraintInequality>(c.name(), c.rows(), m_v+m_k);
if(priorityLevel==0) if(priorityLevel==0)
m_in += c.rows(); m_in += c.rows();
} }
m_hqpData[priorityLevel].push_back(make_pair<double, ConstraintBase*>(weight, tl->constraint)); m_hqpData[priorityLevel].push_back(make_pair<double, std::shared_ptr<ConstraintBase>>(weight, tl->constraint));
return true; return true;
} }
...@@ -232,23 +232,23 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact, ...@@ -232,23 +232,23 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact,
double motion_weight, double motion_weight,
unsigned int motionPriorityLevel) unsigned int motionPriorityLevel)
{ {
ContactLevel *cl = new ContactLevel(contact); auto cl = std::make_shared<ContactLevel>(contact);
cl->index = m_k; cl->index = m_k;
m_k += contact.n_force(); m_k += contact.n_force();
m_contacts.push_back(cl); m_contacts.push_back(cl);
resizeHqpData(); resizeHqpData();
const ConstraintBase & motionConstr = contact.getMotionConstraint(); const ConstraintBase & motionConstr = contact.getMotionConstraint();
cl->motionConstraint = new ConstraintEquality(contact.name()+"_motion_task", motionConstr.rows(), m_v+m_k); cl->motionConstraint = std::make_shared<ConstraintEquality>(contact.name()+"_motion_task", motionConstr.rows(), m_v+m_k);
m_hqpData[motionPriorityLevel].push_back(make_pair<double, ConstraintBase*>(motion_weight, cl->motionConstraint)); m_hqpData[motionPriorityLevel].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(motion_weight, cl->motionConstraint));
const ConstraintInequality & forceConstr = contact.getForceConstraint(); const ConstraintInequality & forceConstr = contact.getForceConstraint();
cl->forceConstraint = new ConstraintInequality(contact.name()+"_force_constraint", forceConstr.rows(), m_v+m_k); cl->forceConstraint = std::make_shared<ConstraintInequality>(contact.name()+"_force_constraint", forceConstr.rows(), m_v+m_k);
m_hqpData[0].push_back(make_pair<double, ConstraintBase*>(1.0, cl->forceConstraint)); m_hqpData[0].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(1.0, cl->forceConstraint));
const ConstraintEquality & forceRegConstr = contact.getForceRegularizationTask(); const ConstraintEquality & forceRegConstr = contact.getForceRegularizationTask();
cl->forceRegTask = new ConstraintEquality(contact.name()+"_force_reg_task", forceRegConstr.rows(), m_v+m_k); cl->forceRegTask = std::make_shared<ConstraintEquality>(contact.name()+"_force_reg_task", forceRegConstr.rows(), m_v+m_k);
m_hqpData[1].push_back(make_pair<double, ConstraintBase*>(force_regularization_weight, cl->forceRegTask)); m_hqpData[1].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(force_regularization_weight, cl->forceRegTask));
m_eq += motionConstr.rows(); m_eq += motionConstr.rows();
m_in += forceConstr.rows(); m_in += forceConstr.rows();
...@@ -302,10 +302,9 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti ...@@ -302,10 +302,9 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
{ {
m_t = time; m_t = time;
std::vector<ContactTransitionInfo*>::iterator it_ct; for(auto it_ct=m_contactTransitions.begin(); it_ct!=m_contactTransitions.end(); it_ct++)
for(it_ct=m_contactTransitions.begin(); it_ct!=m_contactTransitions.end(); it_ct++)
{ {
const ContactTransitionInfo * c = *it_ct; auto c = *it_ct;
assert(c->time_start <= m_t); assert(c->time_start <= m_t);
if(m_t <= c->time_end) if(m_t <= c->time_end)
{ {
...@@ -327,9 +326,10 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti ...@@ -327,9 +326,10 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
m_robot.computeAllTerms(m_data, q, v); m_robot.computeAllTerms(m_data, q, v);
for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++) for(auto cl : m_contacts)
// std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
{ {
ContactLevel* cl = *it; // ContactLevel* cl = *it;
unsigned int m = cl->contact.n_force(); unsigned int m = cl->contact.n_force();
const ConstraintBase & mc = cl->contact.computeMotionTask(time, q, v, m_data); const ConstraintBase & mc = cl->contact.computeMotionTask(time, q, v, m_data);
...@@ -356,64 +356,67 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti ...@@ -356,64 +356,67 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
const Vector & h_u = m_robot.nonLinearEffects(m_data).head(m_u); const Vector & h_u = m_robot.nonLinearEffects(m_data).head(m_u);
const Matrix & J_u = m_Jc.leftCols(m_u); const Matrix & J_u = m_Jc.leftCols(m_u);
m_baseDynamics.matrix().leftCols(m_v) = M_u; m_baseDynamics->matrix().leftCols(m_v) = M_u;
m_baseDynamics.matrix().rightCols(m_k) = -J_u.transpose(); m_baseDynamics->matrix().rightCols(m_k) = -J_u.transpose();
m_baseDynamics.vector() = -h_u; m_baseDynamics->vector() = -h_u;
std::vector<TaskLevel*>::iterator it; // std::vector<TaskLevel*>::iterator it;
for(it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++) // for(it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++)
for (auto& it : m_taskMotions)
{ {
const ConstraintBase & c = (*it)->task.compute(time, q, v, m_data); const ConstraintBase & c = it->task.compute(time, q, v, m_data);
if(c.isEquality()) if(c.isEquality())
{ {
(*it)->constraint->matrix().leftCols(m_v) = c.matrix(); it->constraint->matrix().leftCols(m_v) = c.matrix();
(*it)->constraint->vector() = c.vector(); it->constraint->vector() = c.vector();
} }
else if(c.isInequality()) else if(c.isInequality())
{ {
(*it)->constraint->matrix().leftCols(m_v) = c.matrix(); it->constraint->matrix().leftCols(m_v) = c.matrix();
(*it)->constraint->lowerBound() = c.lowerBound(); it->constraint->lowerBound() = c.lowerBound();
(*it)->constraint->upperBound() = c.upperBound(); it->constraint->upperBound() = c.upperBound();
} }
else else
{ {
(*it)->constraint->matrix().leftCols(m_v) = Matrix::Identity(m_v, m_v); it->constraint->matrix().leftCols(m_v) = Matrix::Identity(m_v, m_v);
(*it)->constraint->lowerBound() = c.lowerBound(); it->constraint->lowerBound() = c.lowerBound();
(*it)->constraint->upperBound() = c.upperBound(); it->constraint->upperBound() = c.upperBound();
} }
} }
for(it=m_taskContactForces.begin(); it!=m_taskContactForces.end(); it++) //for(it=m_taskContactForces.begin(); it!=m_taskContactForces.end(); it++)