Skip to content

GitLab

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

move everything to shared_ptr

parent e30037f3
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 2443dae9
......@@ -69,6 +69,8 @@ IF(EIGEN_NO_AUTOMATIC_RESIZING)
ADD_DEFINITIONS(-DEIGEN_NO_AUTOMATIC_RESIZING)
ENDIF(EIGEN_NO_AUTOMATIC_RESIZING)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
# Project dependencies
ADD_PROJECT_DEPENDENCY(pinocchio 2.3.1 REQUIRED
PKG_CONFIG_REQUIRES "pinocchio >= 2.3.1")
......
include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
View file @ 2443dae9
......@@ -25,15 +25,13 @@
namespace tsid
{
class TaskLevel
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
tasks::TaskBase & task;
math::ConstraintBase * constraint;
// double weight;
std::shared_ptr<math::ConstraintBase> constraint;
unsigned int priority;
TaskLevel(tasks::TaskBase & task,
......@@ -46,9 +44,9 @@ namespace tsid
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
contacts::ContactBase & contact;
math::ConstraintBase * motionConstraint;
math::ConstraintInequality * forceConstraint;
math::ConstraintEquality * forceRegTask;
std::shared_ptr<math::ConstraintBase> motionConstraint;
std::shared_ptr<math::ConstraintInequality> forceConstraint;
std::shared_ptr<math::ConstraintEquality> forceRegTask;
unsigned int index; /// index of 1st element of associated force variable in the force vector
ContactLevel(contacts::ContactBase & contact);
......@@ -63,7 +61,7 @@ namespace tsid
double time_end;
double fMax_start; /// max normal force at time time_start
double fMax_end; /// max normal force at time time_end
ContactLevel * contactLevel;
std::shared_ptr<ContactLevel> contactLevel;
};
class InverseDynamicsFormulationAccForce:
......@@ -140,7 +138,7 @@ namespace tsid
public:
void addTask(TaskLevel* task,
void addTask(std::shared_ptr<TaskLevel> task,
double weight,
unsigned int priorityLevel);
......@@ -152,10 +150,10 @@ namespace tsid
Data m_data;
HQPData m_hqpData;
std::vector<TaskLevel*> m_taskMotions;
std::vector<TaskLevel*> m_taskContactForces;
std::vector<TaskLevel*> m_taskActuations;
std::vector<ContactLevel*> m_contacts;
std::vector<std::shared_ptr<TaskLevel>> m_taskMotions;
std::vector<std::shared_ptr<TaskLevel>> m_taskContactForces;
std::vector<std::shared_ptr<TaskLevel>> m_taskActuations;
std::vector<std::shared_ptr<ContactLevel>> m_contacts;
double m_t; /// time
unsigned int m_k; /// number of contact-force variables
unsigned int m_v; /// number of acceleration variables
......@@ -163,14 +161,14 @@ namespace tsid
unsigned int m_eq; /// number of equality constraints
unsigned int m_in; /// number of inequality constraints
Matrix m_Jc; /// contact force Jacobian
math::ConstraintEquality m_baseDynamics;
std::shared_ptr<math::ConstraintEquality> m_baseDynamics;
bool m_solutionDecoded;
Vector m_dv;
Vector m_f;
Vector m_tau;
std::vector<ContactTransitionInfo*> m_contactTransitions;
std::vector<std::shared_ptr<ContactTransitionInfo>> m_contactTransitions;
};
}
......
include/tsid/math/constraint-base.hpp
View file @ 2443dae9
......@@ -46,6 +46,7 @@ namespace tsid
ConstraintBase(const std::string & name,
ConstRefMatrix A);
virtual ~ConstraintBase() {}
virtual const std::string & name() const;
virtual unsigned int rows() const = 0;
......
include/tsid/math/constraint-bound.hpp
View file @ 2443dae9
......@@ -38,6 +38,7 @@ namespace tsid
ConstraintBound(const std::string & name,
ConstRefVector lb,
ConstRefVector ub);
virtual ~ConstraintBound() {}
unsigned int rows() const;
unsigned int cols() const;
......
include/tsid/math/constraint-equality.hpp
View file @ 2443dae9
......@@ -39,6 +39,7 @@ namespace tsid
ConstraintEquality(const std::string & name,
ConstRefMatrix A,
ConstRefVector b);
virtual ~ConstraintEquality() {}
unsigned int rows() const;
unsigned int cols() const;
......
include/tsid/math/constraint-inequality.hpp
View file @ 2443dae9
......@@ -40,6 +40,7 @@ namespace tsid
ConstRefMatrix A,
ConstRefVector lb,
ConstRefVector ub);
virtual ~ConstraintInequality() {}
unsigned int rows() const;
unsigned int cols() const;
......
include/tsid/solvers/fwd.hpp
View file @ 2443dae9
......@@ -18,6 +18,8 @@
#ifndef __invdyn_solvers_fwd_hpp__
#define __invdyn_solvers_fwd_hpp__
#include <memory>
#include "tsid/config.hpp"
#include "tsid/math/fwd.hpp"
#include <pinocchio/container/aligned-vector.hpp>
......@@ -81,8 +83,8 @@ namespace tsid
{ 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, const math::ConstraintBase*> > ConstConstraintLevel;
typedef pinocchio::container::aligned_vector< aligned_pair<double, std::shared_ptr<math::ConstraintBase>> > ConstraintLevel;
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<ConstConstraintLevel> ConstHQPData;
......
include/tsid/solvers/solver-HQP-eiquadprog-rt.hxx
View file @ 2443dae9
......@@ -86,7 +86,7 @@ namespace tsid
const unsigned int n = cl0[0].second->cols();
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
{
const ConstraintBase* constr = it->second;
auto constr = it->second;
assert(n==constr->cols());
if(constr->isEquality())
neq += constr->rows();
......@@ -99,7 +99,7 @@ namespace tsid
int i_eq=0, i_in=0;
for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
{
const ConstraintBase* constr = it->second;
auto constr = it->second;
if(constr->isEquality())
{
m_CE.middleRows(i_eq, constr->rows()) = constr->matrix();
......@@ -137,7 +137,7 @@ namespace tsid
for(ConstraintLevel::const_iterator it=cl1.begin(); it!=cl1.end(); it++)
{
const double & w = it->first;
const ConstraintBase* constr = it->second;
auto constr = it->second;
if(!constr->isEquality())
assert(false && "Inequalities in the cost function are not implemented yet");
......@@ -196,7 +196,7 @@ namespace tsid
{
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->isEquality())
......
src/formulations/inverse-dynamics-formulation-acc-force.cpp
View file @ 2443dae9
......@@ -44,7 +44,7 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std
bool verbose):
InverseDynamicsFormulationBase(name, robot, verbose),
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_t = 0.0;
......@@ -55,7 +55,7 @@ InverseDynamicsFormulationAccForce::InverseDynamicsFormulationAccForce(const std
m_in = 0;
m_hqpData.resize(2);
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
void InverseDynamicsFormulationAccForce::resizeHqpData()
{
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(ConstraintLevel::iterator itt=it->begin(); itt!=it->end(); itt++)
......@@ -94,7 +94,7 @@ void InverseDynamicsFormulationAccForce::resizeHqpData()
}
void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
void InverseDynamicsFormulationAccForce::addTask(std::shared_ptr<TaskLevel> tl,
double weight,
unsigned int priorityLevel)
{
......@@ -103,20 +103,20 @@ void InverseDynamicsFormulationAccForce::addTask(TaskLevel* tl,
const ConstraintBase & c = tl->task.getConstraint();
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)
m_eq += c.rows();
}
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)
m_in += c.rows();
}
// don't use bounds for now because EiQuadProg doesn't exploit them anyway
// else
// 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,
if (transition_duration<0.0)
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);
addTask(tl, weight, priorityLevel);
......@@ -160,7 +160,7 @@ bool InverseDynamicsFormulationAccForce::addForceTask(TaskContactForce & task,
// This part is not used frequently so we can do some tests.
if (transition_duration<0.0)
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);
addTask(tl, weight, priorityLevel);
return true;
......@@ -182,7 +182,7 @@ bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
if (transition_duration<0.0)
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);
if(priorityLevel > m_hqpData.size())
......@@ -191,18 +191,18 @@ bool InverseDynamicsFormulationAccForce::addActuationTask(TaskActuation & task,
const ConstraintBase & c = tl->task.getConstraint();
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)
m_eq += c.rows();
}
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)
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;
}
......@@ -232,23 +232,23 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact,
double motion_weight,
unsigned int motionPriorityLevel)
{
ContactLevel *cl = new ContactLevel(contact);
auto cl = std::make_shared<ContactLevel>(contact);
cl->index = m_k;
m_k += contact.n_force();
m_contacts.push_back(cl);
resizeHqpData();
const ConstraintBase & motionConstr = contact.getMotionConstraint();
cl->motionConstraint = new ConstraintEquality(contact.name()+"_motion_task", motionConstr.rows(), m_v+m_k);
m_hqpData[motionPriorityLevel].push_back(make_pair<double, ConstraintBase*>(motion_weight, cl->motionConstraint));
cl->motionConstraint = std::make_shared<ConstraintEquality>(contact.name()+"_motion_task", motionConstr.rows(), m_v+m_k);
m_hqpData[motionPriorityLevel].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(motion_weight, cl->motionConstraint));
const ConstraintInequality & forceConstr = contact.getForceConstraint();
cl->forceConstraint = new ConstraintInequality(contact.name()+"_force_constraint", forceConstr.rows(), m_v+m_k);
m_hqpData[0].push_back(make_pair<double, ConstraintBase*>(1.0, cl->forceConstraint));
cl->forceConstraint = std::make_shared<ConstraintInequality>(contact.name()+"_force_constraint", forceConstr.rows(), m_v+m_k);
m_hqpData[0].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(1.0, cl->forceConstraint));
const ConstraintEquality & forceRegConstr = contact.getForceRegularizationTask();
cl->forceRegTask = new 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));
cl->forceRegTask = std::make_shared<ConstraintEquality>(contact.name()+"_force_reg_task", forceRegConstr.rows(), m_v+m_k);
m_hqpData[1].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase>>(force_regularization_weight, cl->forceRegTask));
m_eq += motionConstr.rows();
m_in += forceConstr.rows();
......@@ -302,10 +302,9 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
{
m_t = time;
std::vector<ContactTransitionInfo*>::iterator it_ct;
for(it_ct=m_contactTransitions.begin(); it_ct!=m_contactTransitions.end(); it_ct++)
for(auto 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);
if(m_t <= c->time_end)
{
......@@ -327,9 +326,10 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
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();
const ConstraintBase & mc = cl->contact.computeMotionTask(time, q, v, m_data);
......@@ -356,64 +356,67 @@ const HQPData & InverseDynamicsFormulationAccForce::computeProblemData(double ti
const Vector & h_u = m_robot.nonLinearEffects(m_data).head(m_u);
const Matrix & J_u = m_Jc.leftCols(m_u);
m_baseDynamics.matrix().leftCols(m_v) = M_u;
m_baseDynamics.matrix().rightCols(m_k) = -J_u.transpose();
m_baseDynamics.vector() = -h_u;
m_baseDynamics->matrix().leftCols(m_v) = M_u;
m_baseDynamics->matrix().rightCols(m_k) = -J_u.transpose();
m_baseDynamics->vector() = -h_u;
std::vector<TaskLevel*>::iterator it;
for(it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++)
// std::vector<TaskLevel*>::iterator 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())
{
(*it)->constraint->matrix().leftCols(m_v) = c.matrix();
(*it)->constraint->vector() = c.vector();
it->constraint->matrix().leftCols(m_v) = c.matrix();
it->constraint->vector() = c.vector();
}
else if(c.isInequality())
{
(*it)->constraint->matrix().leftCols(m_v) = c.matrix();
(*it)->constraint->lowerBound() = c.lowerBound();
(*it)->constraint->upperBound() = c.upperBound();
it->constraint->matrix().leftCols(m_v) = c.matrix();
it->constraint->lowerBound() = c.lowerBound();
it->constraint->upperBound() = c.upperBound();
}
else
{
(*it)->constraint->matrix().leftCols(m_v) = Matrix::Identity(m_v, m_v);
(*it)->constraint->lowerBound() = c.lowerBound();
(*it)->constraint->upperBound() = c.upperBound();
it->constraint->matrix().leftCols(m_v) = Matrix::Identity(m_v, m_v);
it->constraint->lowerBound() = c.lowerBound();
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++)
for (auto& it : m_taskContactForces)
{
assert(false);
}
for(it=m_taskActuations.begin(); it!=m_taskActuations.end(); it++)
//for(it=m_taskActuations.begin(); it!=m_taskActuations.end(); it++)
for (auto& it : m_taskActuations)
{
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())
{
(*it)->constraint->matrix().leftCols(m_v).noalias() = c.matrix() * M_a;
(*it)->constraint->matrix().rightCols(m_k).noalias() = - c.matrix() * J_a.transpose();
(*it)->constraint->vector() = c.vector();
(*it)->constraint->vector().noalias() -= c.matrix() * h_a;
it->constraint->matrix().leftCols(m_v).noalias() = c.matrix() * M_a;
it->constraint->matrix().rightCols(m_k).noalias() = - c.matrix() * J_a.transpose();
it->constraint->vector() = c.vector();
it->constraint->vector().noalias() -= c.matrix() * h_a;
}
else if(c.isInequality())
{
(*it)->constraint->matrix().leftCols(m_v).noalias() = c.matrix() * M_a;
(*it)->constraint->matrix().rightCols(m_k).noalias() = - c.matrix() * J_a.transpose();
(*it)->constraint->lowerBound() = c.lowerBound();
(*it)->constraint->lowerBound().noalias() -= c.matrix() * h_a;
(*it)->constraint->upperBound() = c.upperBound();
(*it)->constraint->upperBound().noalias() -= c.matrix() * h_a;
it->constraint->matrix().leftCols(m_v).noalias() = c.matrix() * M_a;
it->constraint->matrix().rightCols(m_k).noalias() = - c.matrix() * J_a.transpose();
it->constraint->lowerBound() = c.lowerBound();
it->constraint->lowerBound().noalias() -= c.matrix() * h_a;
it->constraint->upperBound() = c.upperBound();
it->constraint->upperBound().noalias() -= c.matrix() * h_a;
}
else
{
// NB: An actuator bound becomes an inequality
(*it)->constraint->matrix().leftCols(m_v) = M_a;
(*it)->constraint->matrix().rightCols(m_k) = - J_a.transpose();
(*it)->constraint->lowerBound() = c.lowerBound() - h_a;
(*it)->constraint->upperBound() = c.upperBound() - h_a;
it->constraint->matrix().leftCols(m_v) = M_a;
it->constraint->matrix().rightCols(m_k) = - J_a.transpose();
it->constraint->lowerBound() = c.lowerBound() - h_a;
it->constraint->upperBound() = c.upperBound() - h_a;
}
}
......@@ -462,12 +465,13 @@ Vector InverseDynamicsFormulationAccForce::getContactForces(const std::string &
const HQPOutput & sol)
{
decodeSolution(sol);
for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
// for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
for (auto &it : m_contacts)
{
if((*it)->contact.name()==name)
if(it->contact.name()==name)
{
const int k = (*it)->contact.n_force();
return m_f.segment((*it)->index, k);
const int k = it->contact.n_force();
return m_f.segment(it->index, k);
}
}
return Vector::Zero(0);
......@@ -478,13 +482,14 @@ bool InverseDynamicsFormulationAccForce::getContactForces(const std::string & na
RefVector f)
{
decodeSolution(sol);
for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
//for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
for (auto &it : m_contacts)
{
if((*it)->contact.name()==name)
if(it->contact.name()==name)
{
const int k = (*it)->contact.n_force();
const int k = it->contact.n_force();
assert(f.size()==k);
f = m_f.segment((*it)->index, k);
f = m_f.segment(it->index, k);
return true;
}
}
......@@ -501,8 +506,7 @@ bool InverseDynamicsFormulationAccForce::removeTask(const std::string & taskName
removeFromHqpData(taskName);
#endif
std::vector<TaskLevel*>::iterator it;
for(it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++)
for(auto it=m_taskMotions.begin(); it!=m_taskMotions.end(); it++)
{
if((*it)->task.name()==taskName)
{
......@@ -517,7 +521,7 @@ bool InverseDynamicsFormulationAccForce::removeTask(const std::string & taskName
return true;
}
}
for(it=m_taskContactForces.begin(); it!=m_taskContactForces.end(); it++)
for(auto it=m_taskContactForces.begin(); it!=m_taskContactForces.end(); it++)
{
if((*it)->task.name()==taskName)
{
......@@ -525,7 +529,7 @@ bool InverseDynamicsFormulationAccForce::removeTask(const std::string & taskName
return true;
}
}
for(it=m_taskActuations.begin(); it!=m_taskActuations.end(); it++)
for(auto it=m_taskActuations.begin(); it!=m_taskActuations.end(); it++)
{
if((*it)->task.name()==taskName)
{
......@@ -549,25 +553,25 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(const std::string &
{
if(transition_duration>0.0)
{
for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
for(auto &it : m_contacts)
{
if((*it)->contact.name()==contactName)
if(it->contact.name()==contactName)
{
ContactTransitionInfo * transitionInfo = new ContactTransitionInfo();
transitionInfo->contactLevel = (*it);
auto transitionInfo = std::make_shared<ContactTransitionInfo>();
transitionInfo->contactLevel = it;
transitionInfo->time_start = m_t;
transitionInfo->time_end = m_t + transition_duration;
const int k = (*it)->contact.n_force();
if(m_f.size() >= (*it)->index+k)
const int k = it->contact.n_force();
if(m_f.size() >= it->index+k)
{
const Vector f = m_f.segment((*it)->index, k);
transitionInfo->fMax_start = (*it)->contact.getNormalForce(f);
const Vector f = m_f.segment(it->index, k);
transitionInfo->fMax_start = it->contact.getNormalForce(f);
}
else
{
transitionInfo->fMax_start = (*it)->contact.getMaxNormalForce();
transitionInfo->fMax_start = it->contact.getMaxNormalForce();
}
transitionInfo->fMax_end = (*it)->contact.getMinNormalForce() + 1e-3;
transitionInfo->fMax_end = it->contact.getMinNormalForce() + 1e-3;
m_contactTransitions.push_back(transitionInfo);
return true;
}
......@@ -585,7 +589,7 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(const std::string &
assert(third_constraint_found);
bool contact_found = false;
for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
for(auto it=m_contacts.begin(); it!=m_contacts.end(); it++)
{
if((*it)->contact.name()==contactName)
{
......@@ -600,11 +604,11 @@ bool InverseDynamicsFormulationAccForce::removeRigidContact(const std::string &
}
int k=0;
for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
// for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
for (auto &it : m_contacts)
{
ContactLevel * cl = *it;
cl->index = k;
k += cl->contact.n_force();
it->index = k;
k += it->contact.n_force();
}
return contact_found && first_constraint_found && second_constraint_found && third_constraint_found;
}
......