Commit 5d8def3d authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

When inserting a ConfigProjector in ConstraintSet, remove dummy ConfigProjector.

parent a0ce28c2
......@@ -18,6 +18,7 @@
throw for c in constraints_:
if set.lockedDof_: c.addLockedDof (ld)
throw
set.removeFirstElement ()
set.configProjector_ = self addConstraint (c):
Constraint::addToSet (set) c.addToSet (self)
......
......@@ -19,6 +19,7 @@
#ifndef HPP_CORE_CONSTRAINT_SET_HH
# define HPP_CORE_CONSTRAINT_SET_HH
# include <deque>
# include <hpp/core/constraint.hh>
namespace hpp {
......@@ -58,7 +59,7 @@ namespace hpp {
return configProjector_;
}
protected:
typedef std::vector <ConstraintPtr_t> Constraints_t;
typedef std::deque <ConstraintPtr_t> Constraints_t;
ConstraintSet (const DevicePtr_t& robot, const std::string& name);
/// Store weak pointer to itself.
void init (const ConstraintSetPtr_t& self)
......@@ -83,6 +84,8 @@ namespace hpp {
}
}
void removeFirstElement ();
virtual std::ostream& print (std::ostream& os) const
{
os << "Constraint set " << name () << ", contains" << std::endl;
......
......@@ -290,6 +290,7 @@ namespace hpp {
throw std::runtime_error (oss.str ());
}
constraintSet->configProjector_ = weak_.lock ();
constraintSet->removeFirstElement ();
Constraint::addToConstraintSet (constraintSet);
}
......
......@@ -59,5 +59,9 @@ namespace hpp {
constraints_.push_back (ConfigProjectorTrivial::create (robot));
}
void ConstraintSet::removeFirstElement ()
{
constraints_.pop_front ();
}
} // namespace core
} // namespace core
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment