Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::ArmarXObjects::DMPComponent
* @author Mirko Waechter ( mirko dot waechter at kit dot edu )
* @date 2015
* @copyright http://www.gnu.org/licenses/gpl.txt
* GNU General Public License
*/
#ifndef _ARMARX_COMPONENT_RobotAPI_DMPComponent_H
#define _ARMARX_COMPONENT_RobotAPI_DMPComponent_H
#include <RobotComponents/interface/components/DMPComponentBase.h>
#include <MemoryX/interface/component/WorkingMemoryInterface.h>
#include <MemoryX/interface/component/LongtermMemoryInterface.h>
#include <MemoryX/libraries/memorytypes/entity/DMPEntity.h>
#include <MemoryX/interface/memorytypes/MemoryEntities.h>
#include <MemoryX/interface/memorytypes/MemorySegments.h>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/variant/variant.hpp>
#include <boost/variant/get.hpp>
#include <dmp/representation/dmp/dmpregistration.h>
#include <ArmarXCore/core/Component.h>
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
#include "dmp/representation/dmp/dmpinterface.h"
#include "dmp/representation/dmp/basicdmp.h"
#include "dmp/representation/dmp/dmp3rdorder.h"
#include "dmp/representation/dmp/quaterniondmp.h"
#include "dmp/representation/dmp/endvelodmp.h"
#include "dmp/representation/dmp/dmp3rdorderforcefield.h"
#include "dmp/representation/dmp/forcefielddmp.h"
#include "dmp/representation/dmp/adaptive3rdorderdmp.h"
#include "dmp/representation/dmp/simpleendvelodmp.h"
//#include "dmp/representation/dmp/endveloforcefielddmp.h"
//#include "dmp/representation/dmp/endveloforcefieldwithobjrepulsiondmp.h"
//#include "dmp/representation/dmp/periodictransientdmp.h"
namespace armarx
{
/**
* @class DMPComponentPropertyDefinitions
* @brief
*/
#define ARMARX_DMPTYPE_BASICDMP 10
#define ARMARX_DMPTYPE_ENDVELODMP 11
#define ARMARX_DMPTYPE_SIMPLEENDVELODMP 12
#define ARMARX_DMPTYPE_FORCEFIELDDMP 13
#define ARMARX_DMPTYPE_ENDVELFORCEFILELDDMP 14
#define ARMARX_DMPTYPE_PERIODICTRANSIENTDMP 15
#define ARMARX_DMPTYPE_DMP3RDORDER 20
#define ARMARX_DMPTYPE_DMP3RDORDERFORCEFIELD 21
#define ARMARX_DMPTYPE_ADAPTIVEGOAL3RDORDERDMP 22
#define ARMARX_DMPTYPE_QUATERNIONDMP 30
typedef boost::variant<double, DMP::DVec, Eigen::Quaternionf> paraType;
typedef std::map<int, paraType > configMap;
typedef std::pair<int, paraType > configPair;
class DMPComponentPropertyDefinitions:
public armarx::ComponentPropertyDefinitions
{
public:
DMPComponentPropertyDefinitions(std::string prefix):
armarx::ComponentPropertyDefinitions(prefix)
{
// defineRequiredProperty<std::string>("LongtermMemoryName", "Description");
defineOptionalProperty<std::string>("LongtermMemoryName", "LongtermMemory", "Name of the LongtermMemory component");
}
};
/**
* @class DMPComponent
* @ingroup Components
* @ingroup RobotAPI-Components
* @brief A brief description
*
*
* Detailed Description
*/
class DMPComponent :
virtual public Component,
virtual public DMPComponentBase
{
public:
typedef boost::shared_ptr<DMP::DMPInterface<DMP::DMPState> > BasicDMPInterfacePtr;
typedef boost::shared_ptr<DMP::DMPInterface<DMP::_3rdOrderDMP> > DMP3rdOrderInterfacePtr;
DMPComponent():
basicdmp(new DMP::BasicDMP()),
dmp3rdorder(new DMP::DMP3rdOrder()),
dmpType(ARMARX_DMPTYPE_BASICDMP),
tCurrent(0)
{}
/**
* @see armarx::ManagedIceObject::getDefaultName()
*/
virtual std::string getDefaultName() const
{
return "DMPComponent";
}
/**
* @see PropertyUser::createPropertyDefinitions()
*/
virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions()
{
return armarx::PropertyDefinitionsPtr
{
new DMPComponentPropertyDefinitions{getConfigIdentifier()}
};
}
virtual void calculateWholeTrajectory();
virtual DMP::Vec<DMP::DMPState> calculateNextState(DMP::Vec<DMP::DMPState> ¤tStates, double t, double tInit, DMP::DVec &canonicalValues);
virtual DMP::Vec<DMP::_3rdOrderDMP> calculateNextState(DMP::Vec<DMP::_3rdOrderDMP> ¤tStates, double t, double tInit, DMP::DVec &canonicalValues);
//transmit data from client to server (using ice)
virtual void instantiateDMP(int DMPType, const ::Ice::Current& = ::Ice::Current());
virtual void usingBasicDMP(const ::Ice::Current& = ::Ice::Current()){is3rdOrder = false;}
virtual void using3rdOrderDMP(const ::Ice::Current& = ::Ice::Current()){is3rdOrder = true;}
virtual void setDMPState(const ::armarx::cStateVec &state, const ::Ice::Current& = ::Ice::Current());
virtual void setParameter(const int paraID, double value, const ::Ice::Current& = ::Ice::Current());
virtual void setGoal(const DVector &value, const Ice::Current & = ::Ice::Current());
virtual void setStartPosition(const DVector &value, const Ice::Current & = ::Ice::Current());
virtual void setTimeStamps(const DVector &value, const Ice::Current & = ::Ice::Current());
virtual void setCanonicalValues(const DVector &value, const Ice::Current & = ::Ice::Current());
virtual void getDMPFromDatabase(const std::string &dmpName, const Ice::Current & = ::Ice::Current());
virtual void readTrajectoryFromFile(const std::string &file, const Ice::Current & = ::Ice::Current());
virtual void trainDMP(const ::Ice::Current& = ::Ice::Current());
virtual void storeDMPInDatabase(const std::string &name, const ::Ice::Current& = ::Ice::Current());
virtual void removeDMPFromDatabase(const std::string &name, const ::Ice::Current& = ::Ice::Current());
//transmit data from server to client (using ice)
virtual ::armarx::cStateVec getNextState(const ::Ice::Current& = ::Ice::Current());
virtual int getBasicDMPType(){
int basictype = (dmpType/10) * 10;
if(basictype == 2){
is3rdOrder = true;
}
return basictype;
}
virtual std::string getDMPTypeName();
virtual void setConfigurationMap(configMap conf){configs = conf;setDMPConfiguration();}
virtual void setDMPConfiguration();
configMap constructConfigMap(DMP::Vec<int> paraIDs , DMP::Vec<paraType> paraVals);
configMap configs;
DMP::DVec timestamps;
DMP::DVec canonicalValues;
int tCurrent;
protected:
BasicDMPInterfacePtr basicdmp;
DMP3rdOrderInterfacePtr dmp3rdorder;
DMP::Vec<DMP::SampledTrajectoryV2> trajs;
int dmpType;
DMP::Vec<DMP::DMPState> currentDMPState;
DMP::Vec<DMP::_3rdOrderDMP> currentDMP3rdOrder;
memoryx::LongtermMemoryInterfacePrx longtermMemoryPrx;
memoryx::PersistentDMPDataSegmentBasePrx dmpDataMemoryPrx;
/**
* @see armarx::ManagedIceObject::onInitComponent()
*/
virtual void onInitComponent();
/**
* @see armarx::ManagedIceObject::onConnectComponent()
*/
virtual void onConnectComponent();
/**
* @see armarx::ManagedIceObject::onDisconnectComponent()
*/
virtual void onDisconnectComponent();
/**
* @see armarx::ManagedIceObject::onExitComponent()
*/
virtual void onExitComponent();
/**
* @see PropertyUser::createPropertyDefinitions()
*/
// virtual armarx::PropertyDefinitionsPtr createPropertyDefinitions();
};
typedef ::IceInternal::Handle< ::armarx::DMPComponent> DMPComponentPtr;
}
#endif