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 #include <VirtualRobot/Nodes/RobotNode.h>
25 #include <RobotAPI/libraries/core/FramedPose.h>
26 
27 #include <RobotComponents/components/MotionPlanning/Tasks/RRTConnect/Task.h>
28 //#include <RobotComponents/components/MotionPlanning/Tasks/BiRRT/Task.h>
29 #include <RobotComponents/components/MotionPlanning/Tasks/RandomShortcutPostprocessor/Task.h>
30 #include <RobotComponents/components/MotionPlanning/CSpace/SimoxCSpace.h>
31 #include <RobotComponents/components/MotionPlanning/CSpace/ScaledCSpace.h>
32 #include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
33 #include <RobotAPI/libraries/core/math/MathUtils.h>
34 
35 
36 namespace armarx
37 {
38  GraspingTrajectory PlannedMotionProvider::planMotion(const SimoxCSpaceBasePtr& cSpaceBase,
39  const SimoxCSpaceBasePtr& cSpacePlatformBase,
40  const MotionPlanningData& mpd, const Ice::Current& c)
41  {
42  TrajectoryPtr jointTrajectory = calculateJointTrajectory(cSpaceBase, mpd, c);
43  TrajectoryPtr platformTrajectory = calculatePlatformTrajectory(cSpacePlatformBase, mpd, c);
44  return {platformTrajectory,
45  jointTrajectory, mpd.rnsToUse, mpd.endeffector,
46  mpd.grasp
47  };
48  }
49 
50  GraspingTrajectory PlannedMotionProvider::planMotionParallel(const SimoxCSpaceBasePtr& cSpaceBase,
51  const SimoxCSpaceBasePtr& cSpacePlatformBase,
52  const MotionPlanningData& mpd, const Ice::Current& c)
53  {
54  VectorXf startCfg, goalCfg, startPos, goalPos;
55  ScaledCSpacePtr scaledJointCSpace, scaledPlatformCSpace;
56  Eigen::Vector3f rpyAgent;
57  SimoxCSpacePtr cSpace = SimoxCSpacePtr::dynamicCast(cSpaceBase);
58  SimoxCSpacePtr cSpacePlatform = SimoxCSpacePtr::dynamicCast(cSpacePlatformBase);
59  prepareJointCSpace(cSpace, mpd, &startCfg, &goalCfg, &scaledJointCSpace);
60  preparePlatformCSpace(cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
61  ARMARX_INFO << "Starting BiRRT for Joints and Platform";
62  // TODO: This is not async. Why?? Are the tasks not started when they are enqueued?
63  RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspJointHandle = startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
64  RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspPlatformHandle = startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
65  ARMARX_INFO << "Collecting the jointTask";
66  Path jointTrajectoryPath = finishBiRRT(rspJointHandle, scaledJointCSpace, "Joint");
67  ARMARX_INFO << "Collecting the platformTask";
68  Path posTrajectoryPath = finishBiRRT(rspPlatformHandle, scaledPlatformCSpace, "Platform");
69  // BiRRTTask;
70 
71  auto transformToGlobal = [&](armarx::VectorXf & pos2D)
72  {
73  Eigen::Vector2f globalPos = Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
74  pos2D.at(0) = globalPos[0] + cSpacePlatform->getAgent().agentPose->position->x;
75  pos2D.at(1) = globalPos[1] + cSpacePlatform->getAgent().agentPose->position->y;
76  pos2D.at(2) = math::MathUtils::angleModPI(pos2D.at(2) + rpyAgent(2));
77 
78  };
79  for (auto& e : posTrajectoryPath.nodes)
80  {
81  transformToGlobal(e);
82  }
83  TrajectoryPtr jointTrajectory, platformTrajectory;
84  jointTrajectory = cSpace->pathToTrajectory(jointTrajectoryPath);
85  platformTrajectory = cSpacePlatform->pathToTrajectory(posTrajectoryPath);
86  return {platformTrajectory, jointTrajectory, mpd.rnsToUse, mpd.endeffector, mpd.grasp};
87  }
88 
89  void PlannedMotionProvider::onInitComponent()
90  {
91  usingProxy("MotionPlanningServer");
92  usingProxy("RobotStateComponent");
93  maxPostProcessingSteps = 50;
94 
95  }
96 
97 
98  void PlannedMotionProvider::onConnectComponent()
99  {
100  getProxy(mps, "MotionPlanningServer");
101  getProxy(robotStateComponent, "RobotStateComponent");
102  localRobot = RemoteRobot::createLocalCloneFromFile(robotStateComponent, VirtualRobot::RobotIO::eStructure);
103  }
104 
105 
106  void PlannedMotionProvider::onDisconnectComponent()
107  {
108  planningTasks.clear();
109  }
110 
111 
112  void PlannedMotionProvider::onExitComponent()
113  {
114 
115  }
116 
117  armarx::PropertyDefinitionsPtr PlannedMotionProvider::createPropertyDefinitions()
118  {
119  return armarx::PropertyDefinitionsPtr(new PlannedMotionProviderPropertyDefinitions(
120  getConfigIdentifier()));
121  }
122 
123  TrajectoryBasePtr PlannedMotionProvider::planJointMotion(const armarx::SimoxCSpaceBasePtr& cSpaceBase,
124  const armarx::MotionPlanningData& mpd,
125  const Ice::Current& c)
126  {
127  TrajectoryPtr trajectory = calculateJointTrajectory(cSpaceBase, mpd, c);
128  return TrajectoryBasePtr::dynamicCast(trajectory);
129  }
130 
131  TrajectoryBasePtr PlannedMotionProvider::planPlatformMotion(const armarx::SimoxCSpaceBasePtr& cSpacePlatformBase,
132  const armarx::MotionPlanningData& mpd,
133  const Ice::Current& c)
134  {
135  TrajectoryPtr trajectory = calculatePlatformTrajectory(cSpacePlatformBase, mpd, c);
136  return TrajectoryBasePtr::dynamicCast(trajectory);
137  }
138 
139  TrajectoryPtr PlannedMotionProvider::calculateJointTrajectory(const SimoxCSpaceBasePtr& cSpaceBase,
140  const MotionPlanningData& mpd, const Ice::Current& c)
141  {
142  VectorXf startCfg;
143  VectorXf goalCfg;
144  ScaledCSpacePtr scaledJointCSpace;
145  SimoxCSpacePtr cSpace = SimoxCSpacePtr::dynamicCast(cSpaceBase);
146  prepareJointCSpace(cSpace, mpd, &startCfg, &goalCfg, &scaledJointCSpace);
147  RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle = startBiRRT(scaledJointCSpace, startCfg, goalCfg, 0.01);
148  Path jointTrajectoryPath = finishBiRRT(rspHandle, scaledJointCSpace, "Joint");
149  return cSpace->pathToTrajectory(jointTrajectoryPath);
150  }
151 
152 
153  TrajectoryPtr PlannedMotionProvider::calculatePlatformTrajectory(const SimoxCSpaceBasePtr& cSpacePlatformBase,
154  const MotionPlanningData& mpd,
155  const Ice::Current& c)
156  {
157  VectorXf startPos;
158  VectorXf goalPos;
159  ScaledCSpacePtr scaledPlatformCSpace;
160  Eigen::Vector3f rpyAgent;
161  SimoxCSpacePtr cSpacePlatform = SimoxCSpacePtr::dynamicCast(cSpacePlatformBase);
162  preparePlatformCSpace(cSpacePlatform, mpd, &startPos, &goalPos, &scaledPlatformCSpace, &rpyAgent);
163  // BiRRTTask;
164  RemoteHandle<MotionPlanningTaskControlInterfacePrx> rspHandle = startBiRRT(scaledPlatformCSpace, startPos, goalPos, 0.1);
165  Path posTrajectoryPath = finishBiRRT(rspHandle, scaledPlatformCSpace, "Platform");
166 
167  auto transformToGlobal = [&](armarx::VectorXf & pos2D)
168  {
169  Eigen::Vector2f globalPos = Eigen::Rotation2Df(rpyAgent(2)) * Eigen::Vector2f(pos2D.at(0), pos2D.at(1));
170  pos2D.at(0) = globalPos[0] + cSpacePlatform->getAgent().agentPose->position->x;
171  pos2D.at(1) = globalPos[1] + cSpacePlatform->getAgent().agentPose->position->y;
172  pos2D.at(2) = math::MathUtils::angleModPI(pos2D.at(2) + rpyAgent(2));
173 
174  };
175  for (auto& e : posTrajectoryPath.nodes)
176  {
177  transformToGlobal(e);
178  }
179 
180  return cSpacePlatform->pathToTrajectory(posTrajectoryPath);
181  }
182 
183  RemoteHandle<MotionPlanningTaskControlInterfacePrx> PlannedMotionProvider::startBiRRT(const ScaledCSpacePtr scaledCSpace,
184  const armarx::VectorXf startPos,
185  const armarx::VectorXf goalPos, float dcdStep, bool doRandomShortcutPostProcessing)
186  {
187  // BiRRTTask;
188  MotionPlanningTaskBasePtr taskRRT = new BiRRTTask {scaledCSpace, startPos, goalPos,
189  getDefaultName() + ".birrt" + IceUtil::generateUUID(),
190  60, dcdStep
191  };
192  RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle;
193  if (!doRandomShortcutPostProcessing)
194  {
195  handle = mps->enqueueTask(taskRRT);
196  }
197  else
198  {
199  handle = mps->enqueueTask(
200  new RandomShortcutPostprocessorTask(taskRRT, "RRTSmoothing" + IceUtil::generateUUID(),
201  1, dcdStep, maxPostProcessingSteps));
202  }
203  planningTasks.push_back(handle);
204  return handle;
205  }
206 
207  Path PlannedMotionProvider::finishBiRRT(RemoteHandle<MotionPlanningTaskControlInterfacePrx> handle,
208  const ScaledCSpacePtr scaledCSpace,
209  const std::string roboPart)
210  {
211  ARMARX_CHECK_EXPRESSION(handle);
212  handle->waitForFinishedRunning();
213  ARMARX_IMPORTANT << roboPart + " trajectory planning took "
214  << IceUtil::Time::microSeconds(handle->getRunningTime()).toMilliSecondsDouble()
215  << " ms";
216 
217  if (handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed)
218  {
219  throw RuntimeError(" Motion Planning failed!");
220  }
221 
222  ARMARX_INFO << "RRTConnectTask Planning "
223  << ((bool)(handle->getTaskStatus() == armarx::TaskStatus::ePlanningFailed) ? "failed"
224  : "succeeded");
225  ARMARX_INFO << "RRTConnectTask Planning status: " << handle->getTaskStatus();
226 
227  auto posTrajectoryPath = handle->getPath();
228  ARMARX_CHECK_EXPRESSION(scaledCSpace);
229  scaledCSpace->unscalePath(posTrajectoryPath);
230  return posTrajectoryPath;
231  }
232 
233  void PlannedMotionProvider::preparePlatformCSpace(SimoxCSpacePtr cSpacePlatform, const MotionPlanningData& mpd,
234  VectorXf* storeStart, VectorXf* storeGoal,
235  ScaledCSpacePtr* storeScaledCSpace, Eigen::Vector3f* storeRpyAgent)
236  {
237  cSpacePlatform->setStationaryObjectMargin(getProperty<float>("PlatformMotionSafetyMargin").getValue());
238  ARMARX_CHECK_EXPRESSION(cSpacePlatform);
239 
240  Eigen::Vector3f rpy, rpyAgent;
241  VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseStart)->toEigen(), rpy);
242  VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(cSpacePlatform->getAgent().agentPose)->toEigen(),
243  rpyAgent);
244  armarx::VectorXf startPos {mpd.globalPoseStart->position->x - cSpacePlatform->getAgent().agentPose->position->x,
245  mpd.globalPoseStart->position->y - cSpacePlatform->getAgent().agentPose->position->y,
246  math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))
247  };
248 
249  Eigen::Vector2f localStartPos = Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(startPos.at(0), startPos.at(1));
250  startPos.at(0) = localStartPos(0);
251  startPos.at(1) = localStartPos(1);
252  VirtualRobot::MathTools::eigen4f2rpy(PosePtr::dynamicCast(mpd.globalPoseGoal)->toEigen(), rpy);
253  armarx::VectorXf goalPos {mpd.globalPoseGoal->position->x - cSpacePlatform->getAgent().agentPose->position->x,
254  mpd.globalPoseGoal->position->y - cSpacePlatform->getAgent().agentPose->position->y,
255  math::MathUtils::angleModPI(rpy(2) - rpyAgent(2))
256  };
257  Eigen::Vector2f localGoalPos = Eigen::Rotation2Df(-rpyAgent(2)) * Eigen::Vector2f(goalPos.at(0), goalPos.at(1));
258  goalPos.at(0) = localGoalPos(0);
259  goalPos.at(1) = localGoalPos(1);
260 
261  ARMARX_INFO << VAROUT(startPos) << VAROUT(goalPos);
262 
263  ScaledCSpacePtr scaledPlatformCSpace = new ScaledCSpace(cSpacePlatform, {0.001, 0.001, 1});
264  scaledPlatformCSpace->scaleConfig(startPos);
265  scaledPlatformCSpace->scaleConfig(goalPos);
266  *storeStart = startPos;
267  *storeGoal = goalPos;
268  *storeScaledCSpace = scaledPlatformCSpace;
269  *storeRpyAgent = rpyAgent;
270  }
271 
272  void PlannedMotionProvider::prepareJointCSpace(SimoxCSpacePtr cSpace, const MotionPlanningData& mpd,
273  VectorXf* storeStart, VectorXf* storeGoal,
274  ScaledCSpacePtr* storeScaledCSpace)
275  {
276  cSpace->setStationaryObjectMargin(getProperty<float>("JointMotionSafetyMargin").getValue());
277  ARMARX_CHECK_EXPRESSION(cSpace);
278  ARMARX_INFO << "Entered prepareJointCSpace";
279  Ice::FloatSeq cSpaceScaling;
280  for (VirtualRobot::RobotNodePtr node : localRobot->getRobotNodeSet(mpd.rnsToUse)->getAllRobotNodes())
281  {
282  cSpaceScaling.push_back(node->isTranslationalJoint() ? 0.001f : 1.0f);
283  }
284  ARMARX_IMPORTANT << "Robot position: " << VAROUT(mpd.globalPoseStart->output())
285  << VAROUT(mpd.globalPoseGoal->output());
286 
287  armarx::VectorXf startCfg = cSpace->jointMapToVector(mpd.configStart);
288  armarx::VectorXf goalCfg = cSpace->jointMapToVector(mpd.configGoal);
289 
290  ARMARX_VERBOSE << VAROUT(startCfg) << VAROUT(goalCfg);
291  ScaledCSpacePtr scaledJointCSpace = new ScaledCSpace(cSpace, cSpaceScaling);
292  scaledJointCSpace->scaleConfig(startCfg);
293  scaledJointCSpace->scaleConfig(goalCfg);
294  *storeStart = startCfg;
295  *storeGoal = goalCfg;
296  *storeScaledCSpace = scaledJointCSpace;
297 
298  }
299 }
300 
RemoteRobot.h
armarx::TrajectoryPtr
IceInternal::Handle< Trajectory > TrajectoryPtr
Definition: Trajectory.h:52
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
MathUtils.h
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:183
armarx::PlannedMotionProvider::onExitComponent
void onExitComponent() override
Definition: PlannedMotionProvider.cpp:112
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:441
armarx::PlannedMotionProvider::onConnectComponent
void onConnectComponent() override
Definition: PlannedMotionProvider.cpp:98
ScaledCSpace.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::BiRRTTask
birrt::Task BiRRTTask
Definition: Task.h:73
IceInternal::Handle< Trajectory >
armarx::ScaledCSpacePtr
IceInternal::Handle< ScaledCSpace > ScaledCSpacePtr
An ice handle to a ScaledCSpace.
Definition: ScaledCSpace.h:37
armarx::PlannedMotionProvider::planPlatformMotion
TrajectoryBasePtr planPlatformMotion(const SimoxCSpaceBasePtr &cSpacePlatformBase, const MotionPlanningData &mpd, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PlannedMotionProvider.cpp:131
armarx::PlannedMotionProvider::onDisconnectComponent
void onDisconnectComponent() override
Definition: PlannedMotionProvider.cpp:106
FramedPose.h
armarx::RandomShortcutPostprocessorTask
rngshortcut::Task RandomShortcutPostprocessorTask
Definition: Task.h:121
Task.h
armarx::math::MathUtils::angleModPI
static float angleModPI(float value)
Definition: MathUtils.h:140
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:74
PlannedMotionProvider.h
armarx::PlannedMotionProvider::planJointMotion
TrajectoryBasePtr planJointMotion(const SimoxCSpaceBasePtr &cSpaceBase, const MotionPlanningData &mpd, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PlannedMotionProvider.cpp:123
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:174
armarx::SimoxCSpacePtr
IceInternal::Handle< SimoxCSpace > SimoxCSpacePtr
An ice handle for a SimoxCSpace.
Definition: SimoxCSpace.h:55
armarx::PlannedMotionProvider::getDefaultName
std::string getDefaultName() const override
Definition: PlannedMotionProvider.h:71
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:182
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::PlannedMotionProviderPropertyDefinitions
Definition: PlannedMotionProvider.h:39
armarx::PlannedMotionProvider::onInitComponent
void onInitComponent() override
Definition: PlannedMotionProvider.cpp:89
armarx::RemoteHandle
The RemoteHandle class wrapps a ClientSideRemoteHandleControlBlock and can be used just as a Ice prox...
Definition: RemoteHandle.h:45
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:34
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:393
armarx::PlannedMotionProvider::planMotion
GraspingTrajectory planMotion(const SimoxCSpaceBasePtr &cSpaceBase, const SimoxCSpaceBasePtr &cSpacePlatformBase, const MotionPlanningData &mpd, const Ice::Current &c=Ice::emptyCurrent) override
Definition: PlannedMotionProvider.cpp:38
armarx::PlannedMotionProvider::planMotionParallel
GraspingTrajectory planMotionParallel(const SimoxCSpaceBasePtr &cSpaceBase, const SimoxCSpaceBasePtr &cSpacePlatformBase, const MotionPlanningData &mpd, const Ice::Current &c) override
Definition: PlannedMotionProvider.cpp:50
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:151
armarx::PlannedMotionProvider::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: PlannedMotionProvider.cpp:117
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28