PlannedMotionProvider.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 RobotComponents::ArmarXObjects::PlannedMotionProvider
17  * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
18  * @date 2017
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #include "PlannedMotionProvider.h"
24 
25 #include <VirtualRobot/MathTools.h>
26 #include <VirtualRobot/Nodes/RobotNode.h>
27 #include <VirtualRobot/RobotNodeSet.h>
28 
29 #include <RobotAPI/libraries/core/FramedPose.h>
30 
31 #include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
32 //#include <RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h>
33 #include <RobotAPI/libraries/core/math/MathUtils.h>
34 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
35 
36 #include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
37 #include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
38 #include <RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h>
39 
40 namespace armarx
41 {
42  GraspingTrajectory
43  PlannedMotionProvider::planMotion(const SimoxCSpaceBasePtr& cSpaceBase,
44  const SimoxCSpaceBasePtr& cSpacePlatformBase,
45  const MotionPlanningData& mpd,
46  const Ice::Current& c)
47  {
48  TrajectoryPtr jointTrajectory = calculateJointTrajectory(cSpaceBase, mpd, c);
49  TrajectoryPtr platformTrajectory = calculatePlatformTrajectory(cSpacePlatformBase, mpd, c);
50  return {platformTrajectory, jointTrajectory, mpd.rnsToUse, mpd.endeffector, mpd.grasp};
51  }
52 
53  GraspingTrajectory
54  PlannedMotionProvider::planMotionParallel(const SimoxCSpaceBasePtr& cSpaceBase,
55  const SimoxCSpaceBasePtr& cSpacePlatformBase,
56  const MotionPlanningData& mpd,
57  const Ice::Current& c)
58  {
59  VectorXf startCfg, goalCfg, startPos, goalPos;
60  ScaledCSpacePtr scaledJointCSpace, scaledPlatformCSpace;
61  Eigen::Vector3f rpyAgent;
62  SimoxCSpacePtr cSpace = SimoxCSpacePtr::dynamicCast(cSpaceBase);
63  SimoxCSpacePtr cSpacePlatform = SimoxCSpacePtr::dynamicCast(cSpacePlatformBase);
64  prepareJointCSpace(cSpace, mpd, &startCfg, &goalCfg, &scaledJointCSpace);
65  preparePlatformCSpace(
66  cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
67  ARMARX_INFO << "Starting BiRRT for Joints and Platform";
68  // TODO: This is not async. Why?? Are the tasks not started when they are enqueued?
69  RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspJointHandle =
70  startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
71  RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspPlatformHandle =
72  startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
73  ARMARX_INFO << "Collecting the jointTask";
74  Path jointTrajectoryPath = finishBiRRT(rspJointHandle, scaledJointCSpace, "Joint");
75  ARMARX_INFO << "Collecting the platformTask";
76  Path posTrajectoryPath = finishBiRRT(rspPlatformHandle, scaledPlatformCSpace, "Platform");
77  // BiRRTTask;
78 
79  auto transformToGlobal = [&](armarx::VectorXf& pos2D)
80  {
81  Eigen::Vector2f globalPos =
82  Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
83  pos2D.at(0) = globalPos[0] + cSpacePlatform->getAgent().agentPose->position->x;
84  pos2D.at(1) = globalPos[1] + cSpacePlatform->getAgent().agentPose->position->y;
85  pos2D.at(2) = math::MathUtils::angleModPI(pos2D.at(2) + rpyAgent(2));
86  };
87  for (auto& e : posTrajectoryPath.nodes)
88  {
89  transformToGlobal(e);
90  }
91  TrajectoryPtr jointTrajectory, platformTrajectory;
92  jointTrajectory = cSpace->pathToTrajectory(jointTrajectoryPath);
93  platformTrajectory = cSpacePlatform->pathToTrajectory(posTrajectoryPath);
94  return {platformTrajectory, jointTrajectory, mpd.rnsToUse, mpd.endeffector, mpd.grasp};
95  }
96 
97  void
98  PlannedMotionProvider::onInitComponent()
99  {
100  usingProxy("MotionPlanningServer");
101  usingProxy("RobotStateComponent");
102  maxPostProcessingSteps = 50;
103  }
104 
105  void
106  PlannedMotionProvider::onConnectComponent()
107  {
108  getProxy(mps, "MotionPlanningServer");
109  getProxy(robotStateComponent, "RobotStateComponent");
110  localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent,
111  VirtualRobot::RobotIO::eStructure);
112  }
113 
114  void
115  PlannedMotionProvider::onDisconnectComponent()
116  {
117  planningTasks.clear();
118  }
119 
120  void
121  PlannedMotionProvider::onExitComponent()
122  {
123  }
124 
125  armarx::PropertyDefinitionsPtr
126  PlannedMotionProvider::createPropertyDefinitions()
127  {
128  return armarx::PropertyDefinitionsPtr(
129  new PlannedMotionProviderPropertyDefinitions(getConfigIdentifier()));
130  }
131 
132  TrajectoryBasePtr
133  PlannedMotionProvider::planJointMotion(const armarx::SimoxCSpaceBasePtr& cSpaceBase,
134  const armarx::MotionPlanningData& mpd,
135  const Ice::Current& c)
136  {
137  TrajectoryPtr trajectory = calculateJointTrajectory(cSpaceBase, mpd, c);
138  return TrajectoryBasePtr::dynamicCast(trajectory);
139  }
140 
141  TrajectoryBasePtr
142  PlannedMotionProvider::planPlatformMotion(const armarx::SimoxCSpaceBasePtr& cSpacePlatformBase,
143  const armarx::MotionPlanningData& mpd,
144  const Ice::Current& c)
145  {
146  TrajectoryPtr trajectory = calculatePlatformTrajectory(cSpacePlatformBase, mpd, c);
147  return TrajectoryBasePtr::dynamicCast(trajectory);
148  }
149 
150  TrajectoryPtr
151  PlannedMotionProvider::calculateJointTrajectory(const SimoxCSpaceBasePtr& cSpaceBase,
152  const MotionPlanningData& mpd,
153  const Ice::Current& c)
154  {
155  VectorXf startCfg;
156  VectorXf goalCfg;
157  ScaledCSpacePtr scaledJointCSpace;
158  SimoxCSpacePtr cSpace = SimoxCSpacePtr::dynamicCast(cSpaceBase);
159  prepareJointCSpace(cSpace, mpd, &startCfg, &goalCfg, &scaledJointCSpace);
160  RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle =
161  startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
162  Path jointTrajectoryPath = finishBiRRT(rspHandle, scaledJointCSpace, "Joint");
163  return cSpace->pathToTrajectory(jointTrajectoryPath);
164  }
165 
166  TrajectoryPtr
167  PlannedMotionProvider::calculatePlatformTrajectory(const SimoxCSpaceBasePtr& cSpacePlatformBase,
168  const MotionPlanningData& mpd,
169  const Ice::Current& c)
170  {
171  VectorXf startPos;
172  VectorXf goalPos;
173  ScaledCSpacePtr scaledPlatformCSpace;
174  Eigen::Vector3f rpyAgent;
175  SimoxCSpacePtr cSpacePlatform = SimoxCSpacePtr::dynamicCast(cSpacePlatformBase);
176  preparePlatformCSpace(
177  cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
178  // BiRRTTask;
179  RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle =
180  startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
181  Path posTrajectoryPath = finishBiRRT(rspHandle, scaledPlatformCSpace, "Platform");
182 
183  auto transformToGlobal = [&](armarx::VectorXf& pos2D)
184  {
185  Eigen::Vector2f globalPos =
186  Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
187  pos2D.at(0) = globalPos[0] + cSpacePlatform->getAgent().agentPose->position->x;
188  pos2D.at(1) = globalPos[1] + cSpacePlatform->getAgent().agentPose->position->y;
189  pos2D.at(2) = math::MathUtils::angleModPI(pos2D.at(2) + rpyAgent(2));
190  };
191  for (auto& e : posTrajectoryPath.nodes)
192  {
193  transformToGlobal(e);
194  }
195 
196  return cSpacePlatform->pathToTrajectory(posTrajectoryPath);
197  }
198 
199  RemoteHandle<MotionPlanningTaskControlInterfacePrx>
200  PlannedMotionProvider::startBiRRT(const ScaledCSpacePtr scaledCSpace,
201  const armarx::VectorXf startPos,
202  const armarx::VectorXf goalPos,
203  float dcdStep,
204  bool doRandomShortcutPostProcessing)
205  {
206  // BiRRTTask;
207  MotionPlanningTaskBasePtr taskRRT =
208  new BiRRTTask{scaledCSpace,
209  startPos,
210  goalPos,
211  getDefaultName() + ".birrt" + IceUtil::generateUUID(),
212  60,
213  dcdStep};
214  RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle;
215  if (!doRandomShortcutPostProcessing)
216  {
217  handle = mps->enqueueTask(taskRRT);
218  }
219  else
220  {
221  handle = mps->enqueueTask(
222  new RandomShortcutPostprocessorTask(taskRRT,
223  "RRTSmoothing" + IceUtil::generateUUID(),
224  1,
225  dcdStep,
226  maxPostProcessingSteps));
227  }
228  planningTasks.push_back(handle);
229  return handle;
230  }
231 
232  Path
233  PlannedMotionProvider::finishBiRRT(RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle,
234  const ScaledCSpacePtr scaledCSpace,
235  const std::string roboPart)
236  {
237  ARMARX_CHECK_EXPRESSION(handle);
238  handle->waitForFinishedRunning();
239  ARMARX_IMPORTANT
240  << roboPart + " trajectory planning took "
241  << IceUtil::Time::microSeconds(handle->getRunningTime()).toMilliSecondsDouble()
242  << " ms";
243 
244  if (handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed)
245  {
246  throw RuntimeError(" Motion Planning failed!");
247  }
248 
249  ARMARX_INFO << "RRTConnectTask Planning "
250  << ((bool)(handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed)
251  ? "failed"
252  : "succeeded");
253  ARMARX_INFO << "RRTConnectTask Planning status: " << handle->getTaskStatus();
254 
255  auto posTrajectoryPath = handle->getPath();
256  ARMARX_CHECK_EXPRESSION(scaledCSpace);
257  scaledCSpace->unscalePath(posTrajectoryPath);
258  return posTrajectoryPath;
259  }
260 
261  void
262  PlannedMotionProvider::preparePlatformCSpace(SimoxCSpacePtr cSpacePlatform,
263  const MotionPlanningData& mpd,
264  VectorXf* storeStart,
265  VectorXf* storeGoal,
266  ScaledCSpacePtr* storeScaledCSpace,
267  Eigen::Vector3f* storeRpyAgent)
268  {
269  cSpacePlatform->setStationaryObjectMargin(
270  getProperty<float>("PlatformMotionSafetyMargin").getValue());
271  ARMARX_CHECK_EXPRESSION(cSpacePlatform);
272 
273  Eigen::Vector3f rpy, rpyAgent;
274  VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseStart)->toEigen(),
275  rpy);
276  VirtualRobot::MathTools::eigen4f2rpy(
277  PosePtr::dynamicCast(cSpacePlatform->getAgent().agentPose)->toEigen(), rpyAgent);
278  armarx::VectorXf startPos{
279  mpd.globalPoseStart->position->x - cSpacePlatform->getAgent().agentPose->position->x,
280  mpd.globalPoseStart->position->y - cSpacePlatform->getAgent().agentPose->position->y,
281  math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))};
282 
283  Eigen::Vector2f localStartPos =
284  Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(startPos.at(0), startPos.at(1));
285  startPos.at(0) = localStartPos(0);
286  startPos.at(1) = localStartPos(1);
287  VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseGoal)->toEigen(),
288  rpy);
289  armarx::VectorXf goalPos{
290  mpd.globalPoseGoal->position->x - cSpacePlatform->getAgent().agentPose->position->x,
291  mpd.globalPoseGoal->position->y - cSpacePlatform->getAgent().agentPose->position->y,
292  math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))};
293  Eigen::Vector2f localGoalPos =
294  Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(goalPos.at(0), goalPos.at(1));
295  goalPos.at(0) = localGoalPos(0);
296  goalPos.at(1) = localGoalPos(1);
297 
298  ARMARX_INFO << VAROUT(startPos) << VAROUT(goalPos);
299 
300  ScaledCSpacePtr scaledPlatformCSpace = new ScaledCSpace(cSpacePlatform, {0.001, 0.001, 1});
301  scaledPlatformCSpace->scaleConfig(startPos);
302  scaledPlatformCSpace->scaleConfig(goalPos);
303  *storeStart = startPos;
304  *storeGoal = goalPos;
305  *storeScaledCSpace = scaledPlatformCSpace;
306  *storeRpyAgent = rpyAgent;
307  }
308 
309  void
310  PlannedMotionProvider::prepareJointCSpace(SimoxCSpacePtr cSpace,
311  const MotionPlanningData& mpd,
312  VectorXf* storeStart,
313  VectorXf* storeGoal,
314  ScaledCSpacePtr* storeScaledCSpace)
315  {
316  cSpace->setStationaryObjectMargin(getProperty<float>("JointMotionSafetyMargin").getValue());
317  ARMARX_CHECK_EXPRESSION(cSpace);
318  ARMARX_INFO << "Entered prepareJointCSpace";
319  Ice::FloatSeq cSpaceScaling;
320  for (VirtualRobot::RobotNodePtr node :
321  localRobot->getRobotNodeSet(mpd.rnsToUse)->getAllRobotNodes())
322  {
323  cSpaceScaling.push_back(node->isTranslationalJoint() ? 0.001f : 1.0f);
324  }
325  ARMARX_IMPORTANT << "Robot position: " << VAROUT(mpd.globalPoseStart->output())
326  << VAROUT(mpd.globalPoseGoal->output());
327 
328  armarx::VectorXf startCfg = cSpace->jointMapToVector(mpd.configStart);
329  armarx::VectorXf goalCfg = cSpace->jointMapToVector(mpd.configGoal);
330 
331  ARMARX_VERBOSE << VAROUT(startCfg) << VAROUT(goalCfg);
332  ScaledCSpacePtr scaledJointCSpace = new ScaledCSpace(cSpace, cSpaceScaling);
333  scaledJointCSpace->scaleConfig(startCfg);
334  scaledJointCSpace->scaleConfig(goalCfg);
335  *storeStart = startCfg;
336  *storeGoal = goalCfg;
337  *storeScaledCSpace = scaledJointCSpace;
338  }
339 } // namespace armarx
RemoteRobot.h
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
MathUtils.h
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::PlannedMotionProvider::onExitComponent
void onExitComponent() override
Definition: PlannedMotionProvider.cpp:121
Task.h
armarx::RemoteRobot::createLocalCloneFromFile
static VirtualRobot::RobotPtr createLocalCloneFromFile(RobotStateComponentInterfacePrx robotStatePrx, VirtualRobot::RobotIO::RobotDescription loadMode=VirtualRobot::RobotIO::eFull)
This is a convenience function for createLocalClone, which automatically gets the filename from the R...
Definition: RemoteRobot.cpp:512
armarx::PlannedMotionProvider::onConnectComponent
void onConnectComponent() override
Definition: PlannedMotionProvider.cpp:106
ScaledCSpace.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::BiRRTTask
birrt::Task BiRRTTask
Definition: Task.h:72
IceInternal::Handle< Trajectory >
armarx::ScaledCSpacePtr
IceInternal::Handle< ScaledCSpace > ScaledCSpacePtr
An ice handle to a ScaledCSpace.
Definition: ScaledCSpace.h:38
armarx::PlannedMotionProvider::planPlatformMotion
TrajectoryBasePtr planPlatformMotion(const SimoxCSpaceBasePtr &cSpacePlatformBase, const MotionPlanningData &mpd, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PlannedMotionProvider.cpp:142
armarx::PlannedMotionProvider::onDisconnectComponent
void onDisconnectComponent() override
Definition: PlannedMotionProvider.cpp:115
FramedPose.h
armarx::RandomShortcutPostprocessorTask
rngshortcut::Task RandomShortcutPostprocessorTask
Definition: Task.h:128
Task.h
armarx::math::MathUtils::angleModPI
static float angleModPI(float value)
Definition: MathUtils.h:173
armarx::TrajectoryPtr
IceInternal::Handle< Trajectory > TrajectoryPtr
Definition: Trajectory.h:52
armarx::control::common::getValue
T getValue(nlohmann::json &userConfig, nlohmann::json &defaultConfig, const std::string &entryName)
Definition: utils.h:71
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:79
PlannedMotionProvider.h
armarx::PlannedMotionProvider::planJointMotion
TrajectoryBasePtr planJointMotion(const SimoxCSpaceBasePtr &cSpaceBase, const MotionPlanningData &mpd, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PlannedMotionProvider.cpp:133
SimoxCSpace.h
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
armarx::SimoxCSpacePtr
IceInternal::Handle< SimoxCSpace > SimoxCSpacePtr
An ice handle for a SimoxCSpace.
Definition: SimoxCSpace.h:56
armarx::PlannedMotionProvider::getDefaultName
std::string getDefaultName() const override
Definition: PlannedMotionProvider.h:73
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::PlannedMotionProviderPropertyDefinitions
Definition: PlannedMotionProvider.h:39
armarx::PlannedMotionProvider::onInitComponent
void onInitComponent() override
Definition: PlannedMotionProvider.cpp:98
armarx::RemoteHandle
The RemoteHandle class wrapps a ClientSideRemoteHandleControlBlock and can be used just as a Ice prox...
Definition: RemoteHandle.h:46
armarx::armem::server::ltm::detail::mixin::Path
std::filesystem::path Path
Definition: DiskStorageMixin.h:17
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:35
armarx::ManagedIceObject::getProxy
Ice::ObjectPrx getProxy(long timeoutMs=0, bool waitForScheduler=true) const
Returns the proxy of this object (optionally it waits for the proxy)
Definition: ManagedIceObject.cpp:407
armarx::PlannedMotionProvider::planMotion
GraspingTrajectory planMotion(const SimoxCSpaceBasePtr &cSpaceBase, const SimoxCSpaceBasePtr &cSpacePlatformBase, const MotionPlanningData &mpd, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PlannedMotionProvider.cpp:43
armarx::PlannedMotionProvider::planMotionParallel
GraspingTrajectory planMotionParallel(const SimoxCSpaceBasePtr &cSpaceBase, const SimoxCSpaceBasePtr &cSpacePlatformBase, const MotionPlanningData &mpd, const Ice::Current &c) override
Definition: PlannedMotionProvider.cpp:54
armarx::ManagedIceObject::usingProxy
bool usingProxy(const std::string &name, const std::string &endpoints="")
Registers a proxy for retrieval after initialization and adds it to the dependency list.
Definition: ManagedIceObject.cpp:154
armarx::PlannedMotionProvider::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: PlannedMotionProvider.cpp:126
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27