1 |
|
|
/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
2 |
|
|
* Copyright Projet JRL-Japan, 2007 |
3 |
|
|
*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
4 |
|
|
* |
5 |
|
|
* File: PGManager.cpp |
6 |
|
|
* Project: SOT |
7 |
|
|
* Author: Olivier Stasse, Paul Evrard |
8 |
|
|
* |
9 |
|
|
* Version control |
10 |
|
|
* =============== |
11 |
|
|
* |
12 |
|
|
* $Id$ |
13 |
|
|
* |
14 |
|
|
* Description |
15 |
|
|
* ============ |
16 |
|
|
* |
17 |
|
|
* PGManager entity: configures the PG and sends steps. |
18 |
|
|
* |
19 |
|
|
* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/ |
20 |
|
|
|
21 |
|
|
#include <dynamic-graph/factory.h> |
22 |
|
|
#include <sot/pattern-generator/pg-manager.h> |
23 |
|
|
#include <sot/pattern-generator/pg.h> |
24 |
|
|
|
25 |
|
|
#include <sot/core/debug.hh> |
26 |
|
|
|
27 |
|
|
namespace dynamicgraph { |
28 |
|
|
namespace sot { |
29 |
|
|
|
30 |
|
|
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(PGManager, "PGManager"); |
31 |
|
|
|
32 |
|
|
PGManager::PGManager(const std::string &name) : Entity(name) { |
33 |
|
|
sotDEBUGIN(5); |
34 |
|
|
|
35 |
|
|
sotDEBUGOUT(5); |
36 |
|
|
} |
37 |
|
|
|
38 |
|
|
void PGManager::startSequence(const StepQueue &seq) { |
39 |
|
|
if (!spg_) { |
40 |
|
|
sotERROR << "PG not set" << std::endl; |
41 |
|
|
return; |
42 |
|
|
} |
43 |
|
|
|
44 |
|
|
std::ostringstream cmdstd; |
45 |
|
|
cmdstd << ":StartOnLineStepSequencing "; |
46 |
|
|
|
47 |
|
|
for (unsigned int i = 0; i < seq.size(); ++i) { |
48 |
|
|
const FootPrint &fp = seq.getStep(i); |
49 |
|
|
cmdstd << fp.x << " " << fp.y << " " << fp.theta << " "; |
50 |
|
|
} |
51 |
|
|
|
52 |
|
|
std::istringstream cmdArg(cmdstd.str()); |
53 |
|
|
std::istringstream emptyArg; |
54 |
|
|
spg_->InitState(); |
55 |
|
|
spg_->pgCommandLine(cmdArg.str()); |
56 |
|
|
|
57 |
|
|
sotDEBUG(15) << "Cmd: " << cmdstd.str() << std::endl; |
58 |
|
|
} |
59 |
|
|
|
60 |
|
|
void PGManager::stopSequence(const StepQueue & /* seq */) { |
61 |
|
|
if (!spg_) { |
62 |
|
|
sotERROR << "PG not set" << std::endl; |
63 |
|
|
return; |
64 |
|
|
} |
65 |
|
|
|
66 |
|
|
std::ostringstream cmdstd; |
67 |
|
|
cmdstd << ":StopOnLineStepSequencing"; |
68 |
|
|
std::istringstream cmdArg(cmdstd.str()); |
69 |
|
|
spg_->pgCommandLine(cmdArg.str()); |
70 |
|
|
} |
71 |
|
|
|
72 |
|
|
void PGManager::introduceStep(StepQueue &queue) { |
73 |
|
|
if (!spg_) { |
74 |
|
|
sotERROR << "Walk plugin not found. " << std::endl; |
75 |
|
|
return; |
76 |
|
|
} |
77 |
|
|
|
78 |
|
|
const FootPrint &lastStep = queue.getLastStep(); |
79 |
|
|
|
80 |
|
|
std::string cmdLine = "addStep"; |
81 |
|
|
std::ostringstream cmdArgIn; |
82 |
|
|
cmdArgIn << lastStep.x << " " << lastStep.y << " " << lastStep.theta; |
83 |
|
|
std::istringstream cmdArg(cmdArgIn.str()); |
84 |
|
|
spg_->pgCommandLine(cmdArg.str()); |
85 |
|
|
} |
86 |
|
|
|
87 |
|
|
double PGManager::changeNextStep(StepQueue &queue) { |
88 |
|
|
double stepTime = -1.; |
89 |
|
|
|
90 |
|
|
const FootPrint &step = queue.getFirstStep(); |
91 |
|
|
stepbuf_.push_back(step); |
92 |
|
|
|
93 |
|
|
if (queue.isFirstStepChanged()) { |
94 |
|
|
PatternGeneratorJRL::FootAbsolutePosition aFAP; |
95 |
|
|
const FootPrint &change = queue.getFirstStepChange(); |
96 |
|
|
aFAP.x = change.x - step.x; |
97 |
|
|
aFAP.y = change.y - step.y; |
98 |
|
|
aFAP.theta = change.theta - step.theta; |
99 |
|
|
pgi_->ChangeOnLineStep(0.805, aFAP, stepTime); |
100 |
|
|
} |
101 |
|
|
|
102 |
|
|
return stepTime; |
103 |
|
|
} |
104 |
|
|
|
105 |
|
|
void PGManager::display(std::ostream &os) const { |
106 |
|
|
os << "PGManager <" << getName() << ">:" << std::endl; |
107 |
|
|
} |
108 |
|
|
|
109 |
|
|
void PGManager::commandLine(const std::string &cmdLine, |
110 |
|
|
std::istringstream &cmdArgs, std::ostream &os) { |
111 |
|
|
if (cmdLine == "help") { |
112 |
|
|
os << "StepTimeLine: " << std::endl << std::endl; |
113 |
|
|
} else if ("initPg" == cmdLine) { |
114 |
|
|
std::string name = "pg"; |
115 |
|
|
cmdArgs >> std::ws; |
116 |
|
|
if (cmdArgs.good()) { |
117 |
|
|
cmdArgs >> name; |
118 |
|
|
} |
119 |
|
|
Entity *pgEntity = &(PoolStorage::getInstance()->getEntity(name)); |
120 |
|
|
spg_ = dynamic_cast<PatternGenerator *>(pgEntity); |
121 |
|
|
pgi_ = spg_->GetPatternGeneratorInterface(); |
122 |
|
|
} else if ("savesteps" == cmdLine) { |
123 |
|
|
std::ofstream os("/tmp/steps.dat"); |
124 |
|
|
for (size_t i = 0; i < stepbuf_.size(); ++i) { |
125 |
|
|
os << stepbuf_[i].contact << " " << stepbuf_[i].x << " " << stepbuf_[i].y |
126 |
|
|
<< " " << stepbuf_[i].theta << "\n"; |
127 |
|
|
} |
128 |
|
|
os << std::endl; |
129 |
|
|
stepbuf_.clear(); |
130 |
|
|
} else { |
131 |
|
|
} |
132 |
|
|
} |
133 |
|
|
|
134 |
|
|
} // namespace sot |
135 |
|
|
} // namespace dynamicgraph |