RobotStatePredictionClient.cpp
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  * @package RobotAPI::ArmarXObjects::RobotStatePredictionClientExample
17  * @author Rainer Kartmann ( rainer dot kartmann at kit dot edu )
18  * @date 2022
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
24 
26 
32 #include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
33 #include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
38 
39 #include "simox_alg.hpp"
40 
41 
43 {
44 
46  {
47  }
48 
49 
50  std::vector<armem::MemoryID>
52  {
54  }
55 
56 
57  std::vector<MemoryID>
59  {
61  }
62 
63 
64  std::vector<MemoryID>
65  RobotStatePredictionClient::_queryEntityIDs(const std::string& coreSegmentName)
66  {
68  {
69  namespace qf = armarx::armem::client::query_fns;
70  qb.coreSegments(qf::withName(coreSegmentName))
72  .entities(qf::all())
74  }
76  if (result.success)
77  {
78  return armem::getEntityIDs(result.memory);
79  }
80  else
81  {
82  // ToDo: Use exceptions to escalate error.
83  ARMARX_VERBOSE << "Query failed: " << result.errorMessage;
84  return {};
85  }
86  }
87 
88 
89  std::vector<armem::PredictionRequest>
91  const std::vector<armem::MemoryID>& entityIDs,
92  armem::Time predictedTime,
93  const std::string& engineID)
94  {
95  std::vector<armem::PredictionRequest> requests;
96  requests.reserve(entityIDs.size());
97  for (const armem::MemoryID& entityID : entityIDs)
98  {
99  armem::PredictionRequest& request = requests.emplace_back();
100  request.snapshotID = entityID.withTimestamp(predictedTime);
101  request.predictionSettings.predictionEngineID = engineID;
102  }
103  ARMARX_CHECK_EQUAL(requests.size(), entityIDs.size());
104  return requests;
105  }
106 
107 
108  std::vector<PredictionResult>
109  RobotStatePredictionClient::predict(const std::vector<MemoryID>& entityIDs,
110  Time predictedTime,
111  const std::string& engineID)
112  {
113  const std::vector<PredictionRequest> requests =
114  makePredictionRequests(entityIDs, predictedTime, engineID);
115  return predict(requests);
116  }
117 
118 
119  std::vector<PredictionResult>
120  RobotStatePredictionClient::predict(const std::vector<PredictionRequest>& requests)
121  {
122  const std::vector<PredictionResult> results = remote.reader.predict(requests);
123  ARMARX_CHECK_EQUAL(results.size(), requests.size());
124  return results;
125  }
126 
127 
128  std::optional<Eigen::Affine3f>
130  const std::vector<armem::PredictionResult>& localizationPredictionResults,
131  const std::vector<armem::MemoryID>& localizationEntityIDs,
132  armem::Time predictedTime)
133  {
134  ARMARX_CHECK_EQUAL(localizationPredictionResults.size(), localizationEntityIDs.size());
135  std::stringstream errorMessages;
136 
137  namespace loc = armem::robot_state::localization;
138 
139  std::optional<armem::wm::CoreSegment> coreSegment;
140 
141  loc::TransformQuery query;
142  query.header.parentFrame = armarx::GlobalFrame;
144  query.header.agent = "";
145  query.header.timestamp = predictedTime;
146 
147  for (size_t i = 0; i < localizationPredictionResults.size(); ++i)
148  {
149  const armem::PredictionResult& result = localizationPredictionResults[i];
150  const armem::MemoryID& entityID = localizationEntityIDs.at(i);
151 
152  if (result.success)
153  {
154  if (query.header.agent.empty())
155  {
156  const arondto::Transform tf = arondto::Transform::FromAron(result.prediction);
157  query.header.agent = tf.header.agent;
158  }
159  if (not coreSegment)
160  {
161  coreSegment.emplace(entityID.getCoreSegmentID());
162  }
163  {
165  update.entityID = entityID;
166  update.referencedTime = predictedTime;
167  update.instancesData = {result.prediction};
168  coreSegment->update(update);
169  }
170  }
171  else
172  {
173  errorMessages << "Prediction of '" << entityID << "'"
174  << " failed: " << result.errorMessage << "\n";
175  }
176  }
177 
178  std::optional<Eigen::Affine3f> result;
179  if (coreSegment.has_value())
180  {
181  loc::TransformResult tf =
182  loc::TransformHelper::lookupTransform(coreSegment.value(), query);
183 
184  if (tf)
185  {
186  result = tf.transform.transform;
187  }
188  else
189  {
190  errorMessages << "\nFailed to lookup transform: " << tf.errorMessage << "\n";
191  }
192  }
193 
194  if (not errorMessages.str().empty())
195  {
196  // ToDo: Introduce an exception here?
197  ARMARX_VERBOSE << errorMessages.str();
198  }
199  return result;
200  }
201 
202  std::optional<std::map<std::string, float>>
204  const std::vector<PredictionResult>& proprioceptionPredictionResults,
205  const std::string& robotName)
206  {
207  auto it = std::find_if(proprioceptionPredictionResults.begin(),
208  proprioceptionPredictionResults.end(),
209  [&robotName](const PredictionResult& result)
210  { return result.snapshotID.entityName == robotName; });
211  if (it != proprioceptionPredictionResults.end())
212  {
213  auto result = *it;
214  auto prop = armem::arondto::Proprioception::FromAron(result.prediction);
215  return prop.joints.position;
216  }
217  else
218  {
219  return std::nullopt;
220  }
221  }
222 
223 
224  std::optional<Eigen::Affine3f>
225  RobotStatePredictionClient::predictGlobalPose(const std::vector<MemoryID>& entityIDs,
226  Time predictedTime,
227  const std::string& engineID)
228  {
229  const std::vector<PredictionResult> results = predict(entityIDs, predictedTime, engineID);
230  std::optional<Eigen::Affine3f> pose = lookupGlobalPose(results, entityIDs, predictedTime);
231  return pose;
232  }
233 
234 
235  std::optional<std::map<std::string, float>>
236  RobotStatePredictionClient::predictJointPositions(const std::vector<MemoryID>& entityIDs,
237  Time predictedTime,
238  const std::string& robotName,
239  const std::string& engineID)
240  {
241  const std::vector<PredictionResult> results = predict(entityIDs, predictedTime, engineID);
242  return lookupJointPositions(results, robotName);
243  }
244 
245 
247  RobotStatePredictionClient::predictWholeBody(const std::vector<armem::MemoryID>& locEntityIDs,
248  const std::vector<armem::MemoryID>& propEntityIDs,
249  Time predictedTime,
250  const std::string& robotName,
251  const std::string& engineID)
252  {
254 
255  // Predict.
256  const std::vector<armem::MemoryID> entityIDs =
257  simox::alg::concatenate(locEntityIDs, propEntityIDs);
258 
259  if (entityIDs.empty())
260  {
261  return result;
262  }
263 
264  auto _results = predict(entityIDs, predictedTime, "Linear");
265 
266  // Gather results.
267  std::vector<armem::PredictionResult> locResults, propResults;
268  locResults = simox::alg::slice(_results, 0, locEntityIDs.size());
269  propResults = simox::alg::slice(_results, locEntityIDs.size());
270  ARMARX_CHECK_EQUAL(locResults.size(), locEntityIDs.size());
271  ARMARX_CHECK_EQUAL(propResults.size(), propEntityIDs.size());
272 
273 
274  result.globalPose = lookupGlobalPose(locResults, locEntityIDs, predictedTime);
275  result.jointPositions = lookupJointPositions(propResults, robotName);
276 
277  return result;
278  }
279 
280 } // namespace armarx::armem::robot_state
armarx::armem::detail::SuccessHeader::success
bool success
Definition: SuccessHeader.h:20
simox::alg::slice
std::vector< Args... > slice(const std::vector< Args... > &vector, size_t start=0, std::optional< size_t > end=std::nullopt)
Definition: Impl.cpp:95
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::armem::robot_state::RobotStatePredictionClient::lookupJointPositions
std::optional< std::map< std::string, float > > lookupJointPositions(const std::vector< armem::PredictionResult > &proprioceptionPredictionResults, const std::string &robotName)
Definition: RobotStatePredictionClient.cpp:203
armarx::armem::PredictionRequest
Definition: Prediction.h:49
armarx::armem::robot_state::RobotStatePredictionClient::remote
Remote remote
Definition: RobotStatePredictionClient.h:103
armarx::armem::client::query::ProviderSegmentSelector::entities
EntitySelector & entities()
Start specifying entities.
Definition: selectors.cpp:123
armarx::armem::client::QueryResult::memory
wm::Memory memory
The slice of the memory that matched the query.
Definition: Query.h:58
query.h
armarx::armem::robot_state::RobotStatePredictionClient::Remote::reader
armem::client::Reader reader
Definition: RobotStatePredictionClient.h:100
armarx::armem::robot_state::RobotStatePredictionClient::WholeBodyPrediction
Definition: RobotStatePredictionClient.h:45
armarx::armem::client::query::EntitySelector::snapshots
SnapshotSelector & snapshots()
Start specifying entity snapshots.
Definition: selectors.cpp:86
armarx::GlobalFrame
const std::string GlobalFrame
Definition: FramedPose.h:62
armarx::armem::PredictionRequest::predictionSettings
PredictionSettings predictionSettings
Definition: Prediction.h:52
constants.h
armarx::armem::robot_state::RobotStatePredictionClient::RobotStatePredictionClient
RobotStatePredictionClient()
Definition: RobotStatePredictionClient.cpp:45
armarx::armem::robot_state::constants::proprioceptionCoreSegment
const std::string proprioceptionCoreSegment
Definition: constants.h:30
armarx::armem::robot_state
Definition: RobotStatePredictionClient.cpp:42
armarx::armem::robot_state::RobotStatePredictionClient::queryProprioceptionEntityIDs
std::vector< armem::MemoryID > queryProprioceptionEntityIDs()
Definition: RobotStatePredictionClient.cpp:58
RobotStatePredictionClient.h
armarx::armem::robot_state::RobotStatePredictionClient::WholeBodyPrediction::globalPose
std::optional< Eigen::Affine3f > globalPose
Definition: RobotStatePredictionClient.h:47
armarx::armem::client::QueryResult
Result of a QueryInput.
Definition: Query.h:50
armarx::armem::robot_state::constants::robotRootNodeName
const std::string robotRootNodeName
Definition: constants.h:36
armarx::armem::client::query_fns::withName
auto withName(const std::string &name)
Definition: query_fns.h:32
armarx::armem::PredictionResult
Definition: Prediction.h:58
armarx::armem::robot_state::RobotStatePredictionClient::predict
std::vector< armem::PredictionResult > predict(const std::vector< armem::MemoryID > &entityIDs, armem::Time predictedTime, const std::string &engineID="Linear")
Definition: RobotStatePredictionClient.cpp:109
TransformHelper.h
armarx::armem::client::Reader::predict
std::vector< PredictionResult > predict(const std::vector< PredictionRequest > &requests) const
Get a prediction for the state of multiple entity instances in the future.
Definition: Reader.cpp:482
armarx::armem::client::query_fns::all
auto all()
Definition: query_fns.h:10
armarx::armem::detail::SuccessHeader::errorMessage
std::string errorMessage
Definition: SuccessHeader.h:21
armarx::armem::MemoryID
A memory ID.
Definition: MemoryID.h:47
armarx::armem::PredictionResult::prediction
aron::data::DictPtr prediction
Definition: Prediction.h:64
FramedPose.h
armarx::armem::client::query::Builder::coreSegments
CoreSegmentSelector & coreSegments()
Start specifying core segments.
Definition: Builder.cpp:38
error.h
armarx::armem::EntityUpdate
An update of an entity for a specific point in time.
Definition: Commit.h:27
aron_conversions.h
operations.h
armarx::armem::server::ltm::util::mongodb::detail::update
bool update(mongocxx::collection &coll, const nlohmann::json &query, const nlohmann::json &update)
Definition: mongodb.cpp:67
memory_definitions.h
armarx::armem::robot_state::RobotStatePredictionClient::predictJointPositions
std::optional< std::map< std::string, float > > predictJointPositions(const std::vector< armem::MemoryID > &entityIDs, armem::Time predictedTime, const std::string &robotName, const std::string &engineID="Linear")
Definition: RobotStatePredictionClient.cpp:236
ExpressionException.h
armarx::armem::client::query_fns::latest
std::function< void(query::SnapshotSelector &)> latest()
Definition: query_fns.h:109
armarx::armem::robot_state::RobotStatePredictionClient::queryLocalizationEntityIDs
std::vector< armem::MemoryID > queryLocalizationEntityIDs()
Definition: RobotStatePredictionClient.cpp:51
armarx::core::time::DateTime
Represents a point in time.
Definition: DateTime.h:24
armarx::armem::MemoryID::getCoreSegmentID
MemoryID getCoreSegmentID() const
Definition: MemoryID.cpp:289
armarx::armem::robot_state::RobotStatePredictionClient::predictGlobalPose
std::optional< Eigen::Affine3f > predictGlobalPose(const std::vector< armem::MemoryID > &entityIDs, armem::Time predictedTime, const std::string &engineID="Linear")
Definition: RobotStatePredictionClient.cpp:225
simox_alg.hpp
armarx::armem::robot_state::RobotStatePredictionClient::predictWholeBody
WholeBodyPrediction predictWholeBody(const std::vector< armem::MemoryID > &localizationEntityIDs, const std::vector< armem::MemoryID > &proprioceptionEntityIDs, armem::Time predictedTime, const std::string &robotName, const std::string &engineID="Linear")
Definition: RobotStatePredictionClient.cpp:247
armarx::armem::robot_state::constants::localizationCoreSegment
const std::string localizationCoreSegment
Definition: constants.h:29
armarx::armem::PredictionResult::errorMessage
std::string errorMessage
Definition: Prediction.h:61
armarx::armem::robot_state::RobotStatePredictionClient::makePredictionRequests
std::vector< armem::PredictionRequest > makePredictionRequests(const std::vector< armem::MemoryID > &entityIDs, armem::Time predictedTime, const std::string &engineID="Linear")
Definition: RobotStatePredictionClient.cpp:90
armarx::armem::getEntityIDs
std::vector< MemoryID > getEntityIDs(const ContainerT &container)
Definition: operations.h:54
simox::alg::concatenate
std::vector< Args... > concatenate(const std::vector< Args... > &lhs, const std::vector< Args... > &rhs)
Definition: Impl.cpp:44
armarx::armem::robot_state::RobotStatePredictionClient::lookupGlobalPose
std::optional< Eigen::Affine3f > lookupGlobalPose(const std::vector< armem::PredictionResult > &localizationPredictionResults, const std::vector< armem::MemoryID > &localizationEntityIDs, armem::Time predictedTime)
Definition: RobotStatePredictionClient.cpp:129
armarx::armem::MemoryID::withTimestamp
MemoryID withTimestamp(Time time) const
Definition: MemoryID.cpp:428
armarx::armem::client::query::Builder
The query::Builder class provides a fluent-style specification of hierarchical queries.
Definition: Builder.h:22
armarx::armem::client::query::CoreSegmentSelector::providerSegments
ProviderSegmentSelector & providerSegments()
Start specifying provider segments.
Definition: selectors.cpp:160
MemoryNameSystem.h
armarx::armem::PredictionResult::success
bool success
Definition: Prediction.h:60
ARMARX_CHECK_EQUAL
#define ARMARX_CHECK_EQUAL(lhs, rhs)
This macro evaluates whether lhs is equal (==) rhs and if it turns out to be false it will throw an E...
Definition: ExpressionException.h:130
armarx::armem::PredictionRequest::snapshotID
armem::MemoryID snapshotID
Definition: Prediction.h:51
armarx::armem::client::query_fns
Definition: query_fns.h:6
armarx::armem::client::Reader::query
QueryResult query(const QueryInput &input) const
Perform a query.
Definition: Reader.cpp:33
armarx::armem::robot_state::RobotStatePredictionClient::WholeBodyPrediction::jointPositions
std::optional< std::map< std::string, float > > jointPositions
Definition: RobotStatePredictionClient.h:48
armarx::armem::PredictionSettings::predictionEngineID
std::string predictionEngineID
Definition: Prediction.h:43