1 |
|
|
/* |
2 |
|
|
* Copyright 2010, |
3 |
|
|
* Florent Lamiraux |
4 |
|
|
* |
5 |
|
|
* CNRS/AIST |
6 |
|
|
* |
7 |
|
|
*/ |
8 |
|
|
|
9 |
|
|
#ifndef DYNAMIC_COMMAND_H |
10 |
|
|
#define DYNAMIC_COMMAND_H |
11 |
|
|
|
12 |
|
|
#include <dynamic-graph/command-getter.h> |
13 |
|
|
#include <dynamic-graph/command-setter.h> |
14 |
|
|
#include <dynamic-graph/command.h> |
15 |
|
|
|
16 |
|
|
#include <boost/assign/list_of.hpp> |
17 |
|
|
#include <fstream> |
18 |
|
|
|
19 |
|
|
namespace dynamicgraph { |
20 |
|
|
namespace sot { |
21 |
|
|
namespace command { |
22 |
|
|
using ::dynamicgraph::command::Command; |
23 |
|
|
using ::dynamicgraph::command::Value; |
24 |
|
|
|
25 |
|
|
// Command DisplayModel |
26 |
|
|
class DisplayModel : public Command { |
27 |
|
|
public: |
28 |
|
|
virtual ~DisplayModel() { |
29 |
|
|
sotDEBUGIN(15); |
30 |
|
|
sotDEBUGOUT(15); |
31 |
|
|
} |
32 |
|
|
/// Create command and store it in Entity |
33 |
|
|
/// \param entity instance of Entity owning this command |
34 |
|
|
/// \param docstring documentation of the command |
35 |
|
|
DisplayModel(DynamicPinocchio& entity, const std::string& docstring) |
36 |
|
|
: Command(entity, std::vector<Value::Type>(), docstring) {} |
37 |
|
|
virtual Value doExecute() { |
38 |
|
|
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner()); |
39 |
|
|
robot.displayModel(); |
40 |
|
|
return Value(); |
41 |
|
|
} |
42 |
|
|
}; // class DisplayModel |
43 |
|
|
|
44 |
|
|
// Command GetDimension |
45 |
|
|
class GetDimension : public Command { |
46 |
|
|
public: |
47 |
|
|
virtual ~GetDimension() { |
48 |
|
|
sotDEBUGIN(15); |
49 |
|
|
sotDEBUGOUT(15); |
50 |
|
|
} |
51 |
|
|
/// Create command and store it in Entity |
52 |
|
|
/// \param entity instance of Entity owning this command |
53 |
|
|
/// \param docstring documentation of the command |
54 |
|
|
GetDimension(DynamicPinocchio& entity, const std::string& docstring) |
55 |
|
|
: Command(entity, std::vector<Value::Type>(), docstring) {} |
56 |
|
|
virtual Value doExecute() { |
57 |
|
|
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner()); |
58 |
|
|
unsigned int dimension = robot.m_model->nv; |
59 |
|
|
return Value(dimension); |
60 |
|
|
} |
61 |
|
|
}; // class GetDimension |
62 |
|
|
|
63 |
|
|
// Command getJointNames |
64 |
|
|
class GetJointNames : public Command { |
65 |
|
|
public: |
66 |
|
|
/// Create command and store it in Entity |
67 |
|
|
/// \param entity instance of Entity owning this command |
68 |
|
|
/// \param docstring documentation of the command |
69 |
|
|
GetJointNames(DynamicPinocchio& entity, const std::string& docstring) |
70 |
|
|
: Command(entity, std::vector<Value::Type>(), docstring) {} |
71 |
|
|
virtual Value doExecute() { |
72 |
|
|
DynamicPinocchio& robot = static_cast<DynamicPinocchio&>(owner()); |
73 |
|
|
if (robot.m_model == 0x0) { |
74 |
|
|
SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC, |
75 |
|
|
"model has not been initialized."); |
76 |
|
|
} |
77 |
|
|
const std::vector<std::string>& jointNames = robot.m_model->names; |
78 |
|
|
// Remove first joint names 'universe' |
79 |
|
|
assert(jointNames.size() >= 1); |
80 |
|
|
std::vector<Value> res; |
81 |
|
|
for (std::size_t i = 1; i < jointNames.size(); ++i) { |
82 |
|
|
res.push_back(Value(jointNames[i])); |
83 |
|
|
} |
84 |
|
|
return Value(res); |
85 |
|
|
} |
86 |
|
|
}; |
87 |
|
|
} // namespace command |
88 |
|
|
} /* namespace sot */ |
89 |
|
|
} /* namespace dynamicgraph */ |
90 |
|
|
|
91 |
|
|
#endif // DYNAMIC_COMMAND_H |