MP.h
Go to the documentation of this file.
1 /**
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @author Jianfeng Gao ( jianfeng dot gao at kit dot edu )
17  * @date 2022
18  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
19  * GNU General Public License
20  */
21 
22 #pragma once
23 
24 #include <SimoxUtility/json.h>
25 
27 #include <ArmarXCore/interface/core/BasicTypes.h>
28 #include <ArmarXCore/interface/serialization/Eigen.h>
29 
30 #include <armarx/control/common/mp/aron/MPConfig.aron.generated.h>
31 
32 #include <mplib/factories/AbstractMPFactory.h>
33 #include <mplib/factories/MPFactoryCreator.h>
34 #include <mplib/factories/VMPFactory.h>
35 #include <mplib/representation/vmp/PrincipalComponentVMP.h>
36 #include <mplib/representation/vmp/TaskSpacePrincipalComponentVMP.h>
37 #include <mplib/representation/vmp/VMP.h>
38 
40 {
41  using namespace arondto;
42  using VMPPtr = std::shared_ptr<mplib::representation::vmp::PrincipalComponentVMP>;
43  using MutexType = std::recursive_mutex;
44  using LockGuardType = std::lock_guard<std::recursive_mutex>;
45 
46  using DVec = Ice::DoubleSeq;
47  using DVecVec = DoubleSeqSeq;
48 
49  struct MPInput
50  {
51  virtual ~MPInput()
52  {
53  }
54  };
55 
56  struct MPOutput
57  {
58  virtual ~MPOutput()
59  {
60  }
61  };
62 
63  using MPInputPtr = std::shared_ptr<MPInput>;
64  using MPOutputPtr = std::shared_ptr<MPOutput>;
65  using MPTrajs = std::vector<MPTraj>;
66 
67  class MP
68  {
69  public:
70  MP(const MPConfig& c);
71  virtual ~MP() = default;
72 
73  std::string getMPName();
74  std::string getClassName();
75  std::string getNodeSetName();
76 
77  /// control
78  void start();
79  void start(const DVec& goals);
80  void start(const DVec& goals, Ice::Double timeDuration);
81  void start(const DVec& goals, const DVec& starts, Ice::Double timeDuration);
82  void start(const DVec& goals, const DVec& starts);
83  void stop();
84  void pause();
85  void resume();
86  void reset();
87  bool isFinished();
88 
89  MPConfig getConfig();
90 
91  /// setting
92  void learnFromCSV(const Ice::StringSeq& fileNames = std::vector<std::string>());
93  void learnFromTraj(const MPTrajs& trajs = MPTrajs());
94  void trainMPFromTraj(std::vector<mplib::core::SampledTrajectory>& trajs);
95  void trainMP();
96 
97  void setDurationSec(Ice::Double timeDuration);
98  void setGoal(const DVec& goals);
99  void setStart(const DVec& starts);
100  void setStartAndGoal(const DVec& starts, const DVec& goals);
101  void setViaPoint(Ice::Double u, const DVec& viapoint);
102  void removeAllViaPoint();
103 
104  /// serialze
105  void setWeight(const DVecVec& weights);
106  void setTranslationWeights(const DVecVec& weights);
107  void setRotationWeights(const DVecVec& weights);
108  DVecVec getWeight();
109  std::string serialize(const std::string& mode = "string");
110  DVec deserialize(const std::string&, const std::string& mode = "string");
111 
112  ///
113  double getCanonicalValue();
114  virtual void run(MPInputPtr, MPOutputPtr) = 0;
115 
116  private:
117  void
118  resetVMPType(const std::string& mpTypeString)
119  {
120  if (mpTypeString != "")
121  {
122  vmpType = mplib::representation::VMPType::_from_string_nocase(mpTypeString.c_str());
123  }
124  }
125 
126  protected:
127  MPConfig cfg;
128 
129  bool isDisturbance = false;
130 
133  mplib::representation::VMPType vmpType = mplib::representation::VMPType::PrincipalComponent;
134 
135  std::atomic_bool running = false;
136  std::atomic_bool finished = false;
137  std::atomic_bool paused = false;
138  double canonicalValue = 1.0;
139 
140  std::atomic_bool goalSetByUser{false};
141  /// Note: if mp is not trained, the dimension is not initialized, thus mp->getDim() is undefined, if you set
142  /// viapoints you will have memory allocation error. So make sure mp is trained before setting any viapoint.
143  std::atomic_bool mpTrained{false};
146  std::vector<mplib::representation::MPState> currentState;
147  std::vector<std::pair<double, DVec>> userDefinedViaPoints;
148  };
149 
150  using MPPtr = std::shared_ptr<MP>;
151 
152 } // namespace armarx::control::common::mp
armarx::control::common::mp::MP
Definition: MP.h:67
armarx::control::common::mp::DVecVec
DoubleSeqSeq DVecVec
Definition: MP.h:47
armarx::control::common::mp::MutexType
std::recursive_mutex MutexType
Definition: MP.h:43
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::control::common::mp::MP::targetPoseVecInTraj
DVec targetPoseVecInTraj
Definition: MP.h:145
armarx::control::common::mp
This file is part of ArmarX.
Definition: aron_conversions.cpp:331
armarx::control::common::mp::VMPPtr
std::shared_ptr< mplib::representation::vmp::PrincipalComponentVMP > VMPPtr
Definition: MP.h:42
armarx::control::common::mp::MP::cfg
MPConfig cfg
Definition: MP.h:127
armarx::control::common::mp::LockGuardType
std::lock_guard< std::recursive_mutex > LockGuardType
Definition: MP.h:44
armarx::control::common::mp::MP::userDefinedViaPoints
std::vector< std::pair< double, DVec > > userDefinedViaPoints
Definition: MP.h:147
armarx::VariantType::Double
const VariantTypeId Double
Definition: Variant.h:919
armarx::control::common::mp::MPPtr
std::shared_ptr< MP > MPPtr
Definition: MP.h:150
armarx::control::common::mp::DVec
Ice::DoubleSeq DVec
Definition: MP.h:46
armarx::control::common::mp::MPInputPtr
std::shared_ptr< MPInput > MPInputPtr
Definition: MP.h:63
armarx::control::common::mp::MP::vmp
VMPPtr vmp
Definition: MP.h:132
armarx::control::common::mp::MP::currentState
std::vector< mplib::representation::MPState > currentState
Definition: MP.h:146
armarx::control::common::mp::MPInput
Definition: MP.h:49
armarx::control::common::mp::MPOutput::~MPOutput
virtual ~MPOutput()
Definition: MP.h:58
armarx::control::common::mp::MPOutputPtr
std::shared_ptr< MPOutput > MPOutputPtr
Definition: MP.h:64
armarx::control::common::mp::MPTrajs
std::vector< MPTraj > MPTrajs
Definition: MP.h:65
Logging.h
armarx::control::common::mp::MP::mpMutex
MutexType mpMutex
Definition: MP.h:131
armarx::control::common::mp::MPInput::~MPInput
virtual ~MPInput()
Definition: MP.h:51
armarx::control::common::mp::MP::targetPoseVec
DVec targetPoseVec
Definition: MP.h:144
armarx::control::common::mp::MPOutput
Definition: MP.h:56