21 #include "../src/task/task-command.h" 34 Task::Task(
const std::string &
n)
37 withDerivative(false),
38 controlGainSIN(NULL,
"sotTask(" + n +
")::input(double)::controlGain"),
39 dampingGainSINOUT(NULL,
"sotTask(" + n +
")::in/output(double)::damping")
42 controlSelectionSIN(NULL,
43 "sotTask(" + n +
")::input(flag)::controlSelec"),
45 "sotTask(" + n +
")::output(vector)::error"),
46 errorTimeDerivativeSOUT(
47 boost::bind(&
Task::computeErrorTimeDerivative, this, _1, _2),
49 "sotTask(" + n +
")::output(vector)::errorTimeDerivative") {
73 std::string docstring;
77 " Add a feature to the task\n" 80 " - name of the feature\n" 94 " Clear the list of features of the task\n" 102 " Returns the list of features of the task\n" 122 for (FeatureList_t::iterator iter =
featureList.begin();
174 error.resize(dimError);
182 for (FeatureList_t::iterator iter =
featureList.begin();
191 while (cursorError + dim > dimError)
194 error.resize(dimError);
198 for (
int k = 0; k < dim; ++k) {
199 error(cursorError++) = partialError(k);
201 sotDEBUG(35) <<
"feature: " << partialError << std::endl;
202 sotDEBUG(35) <<
"error: " << error << std::endl;
206 error.conservativeResize(cursorError);
209 sotDEBUG(35) <<
"error_final: " << error << std::endl;
219 for (FeatureList_t::iterator iter =
featureList.begin();
225 res.segment(cursor, dim) = partialErrorDot;
237 errorRef.resize(errSingleBound.size());
239 for (
unsigned int i = 0;
i < errorRef.size(); ++
i)
240 errorRef[
i] = -errSingleBound(
i) * gain;
244 for (
unsigned int i = 0;
i < errorRef.size(); ++
i)
245 errorRef[
i] = errorRef[
i].getSingleBound() - de(
i);
271 for (FeatureList_t::iterator iter =
featureList.begin();
279 sotDEBUG(25) <<
"Jp =" << endl << partialJacobian << endl;
282 nbc = partialJacobian.cols();
284 }
else if (partialJacobian.cols() != nbc)
287 "Features from the list don't have compatible-size jacobians.");
289 while (cursorJ + nbr >= dimJ) {
291 J.conservativeResize(dimJ, nbc);
296 for (
int kc = 0; kc < nbc; ++kc) {
298 for (
unsigned int k = 0; k < nbr; ++k) {
299 J(cursorJ + k, kc) = partialJacobian(k, kc);
308 J.conservativeResize(cursorJ, nbc);
320 os <<
"Task " <<
name <<
": " << endl;
321 os <<
"--- LIST --- " << std::endl;
323 for (FeatureList_t::const_iterator iter =
featureList.begin();
325 os <<
"-> " << (*iter)->getName() << endl;
330 FeatureList_t::const_iterator itFeatureAbstract;
333 os <<
"\t\"" << (*itFeatureAbstract)->getName() <<
"\" -> \"" <<
getName()
std::string docDirectGetter(const std::string &name, const std::string &type)
CommandVoid0< E > * makeCommandVoid0(E &entity, boost::function< void(void)> function, const std::string &docString)
void addControlSelection(const Flags &act)
void clearFeatureList(void)
DirectGetter< E, T > * makeDirectGetter(E &entity, T *ptr, const std::string &docString)
void signalRegistration(const SignalArray< int > &signals)
dynamicgraph::SignalTimeDependent< VectorMultiBound, int > taskSOUT
FeatureList_t featureList
dynamicgraph::SignalPtr< double, int > controlGainSIN
void addFeature(FeatureAbstract &s)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Task, "Task")
std::vector< MultiBound > VectorMultiBound
dynamicgraph::SignalPtr< Flags, int > controlSelectionSIN
void setControlSelection(const Flags &act)
CommandVoid1< E, T > * makeCommandVoid1(E &entity, boost::function< void(const T &)> function, const std::string &docString)
void setWithDerivative(const bool &s)
void addFeatureFromName(const std::string &name)
dynamicgraph::Vector & computeErrorTimeDerivative(dynamicgraph::Vector &res, int time)
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
virtual std::ostream & writeGraph(std::ostream &os) const
SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
bool getWithDerivative(void)
void display(std::ostream &os) const
VectorMultiBound & computeTaskExponentialDecrease(VectorMultiBound &errorRef, int time)
virtual SignalTimeDependent< dynamicgraph::Vector, int > & getErrorDot()
virtual void removeDependency(const SignalBase< Time > &signal)
dynamicgraph::Matrix & computeJacobian(dynamicgraph::Matrix &J, int time)
virtual void addDependency(const SignalBase< Time > &signal)
static PoolStorage * getInstance()
virtual const T & accessCopy() const
dynamicgraph::SignalPtr< double, int > dampingGainSINOUT
void clearControlSelection(void)
dynamicgraph::SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
This class gives the abstract definition of a feature.
const std::string & getName() const
DYNAMIC_GRAPH_DLLAPI SignalArray< int > sotNOSIGNAL
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > errorTimeDerivativeSOUT
Class that defines the basic elements of a task.
dynamicgraph::Vector & computeError(dynamicgraph::Vector &error, int time)
void addCommand(const std::string &name, command::Command *command)
SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : . ...
std::string docDirectSetter(const std::string &name, const std::string &type)
virtual void setFunction(boost::function2< T &, T &, Time > t, Mutex *mutexref=NULL)
DirectSetter< E, T > * makeDirectSetter(E &entity, T *ptr, const std::string &docString)