Robowflex  v0.1
Making MoveIt Easy
Functions
fetch_lift.cpp File Reference
#include <chrono>
#include <thread>
#include <robowflex_library/log.h>
#include <robowflex_dart/gui.h>
#include <robowflex_dart/io.h>
#include <robowflex_dart/planning.h>
#include <robowflex_dart/robot.h>
#include <robowflex_dart/space.h>
#include <robowflex_dart/tsr.h>
#include <robowflex_dart/world.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>

Go to the source code of this file.

Functions

int main (int, char **)
 

Function Documentation

◆ main()

int main ( int  ,
char **   
)

Definition at line 21 of file fetch_lift.cpp.

22 {
23  double height = 0.6;
24  double raise = 1.2;
25  double radius = 0.8;
26 
27  //
28  // Load and setup the fetches
29  //
30  auto world = std::make_shared<darts::World>();
31  auto fetch1 = darts::loadMoveItRobot("fetch1", //
32  "package://fetch_description/robots/fetch.urdf", //
33  "package://fetch_moveit_config/config/fetch.srdf");
34  auto fetch2 = fetch1->cloneRobot("fetch2");
35  fetch2->setDof(2, -1.57);
36  fetch2->setDof(3, radius);
37  fetch2->setDof(4, radius);
38 
39  //
40  // make box to grab
41  //
42  auto scene = std::make_shared<darts::Structure>("object");
43 
44  dart::dynamics::FreeJoint::Properties joint;
45  joint.mName = "box";
46  joint.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(radius, 0, height);
47 
48  scene->addFreeFrame(joint, darts::makeBox(0.50, 0.50, 0.1));
49 
50  world->addRobot(fetch1);
51  world->addRobot(fetch2);
52  world->addStructure(scene);
53 
54  darts::Window window(world);
55 
56  //
57  // Touch the box
58  //
59  const auto &touch = [&] {
60  darts::PlanBuilder builder(world);
61  builder.addGroup("fetch1", "arm_with_torso");
62  builder.addGroup("fetch2", "arm_with_torso");
63 
64  builder.setStartConfiguration({
65  0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, //
66  0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, //
67  });
68 
69  builder.initialize();
70 
71  darts::TSR::Specification goal1_spec;
72  goal1_spec.setFrame("fetch1", "wrist_roll_link", "base_link");
73  goal1_spec.setPose(0.5, 0, height - 0.1, //
74  0.707, 0, -0.707, 0);
75 
76  darts::TSR::Specification goal2_spec;
77  goal2_spec.setFrame("fetch2", "wrist_roll_link", "base_link");
78  goal2_spec.setPose(0.5, 0, height - 0.1, //
79  0.707, 0, -0.707, 0);
80 
81  auto goal1_tsr = std::make_shared<darts::TSR>(world, goal1_spec);
82  auto goal2_tsr = std::make_shared<darts::TSR>(world, goal2_spec);
83  auto goal = builder.getGoalTSR({goal1_tsr, goal2_tsr});
84  builder.setGoal(goal);
85 
86  auto rrt = std::make_shared<ompl::geometric::RRTConnect>(builder.info, true);
87  rrt->setRange(1.);
88  builder.ss->setPlanner(rrt);
89 
90  builder.setup();
91 
92  goal->options.max_samples = 1;
93  goal->startSampling();
94  ompl::base::PlannerStatus solved = builder.ss->solve(60.0);
95  goal->stopSampling();
96 
97  if (solved)
98  {
99  RBX_INFO("Found solution!");
100  window.animatePath(builder, builder.getSolutionPath());
101  }
102  else
103  RBX_WARN("No solution found");
104 
105  return solved == ompl::base::PlannerStatus::EXACT_SOLUTION;
106  };
107 
108  //
109  // Lift the box
110  //
111  const auto &lift = [&] {
112  darts::PlanBuilder builder(world);
113  builder.addGroup("fetch1", "arm_with_torso");
114  builder.addGroup("fetch2", "arm_with_torso");
115 
116  builder.setStartConfigurationFromWorld();
117 
118  darts::TSR::Specification con1_spec;
119  con1_spec.setTarget("fetch2", "wrist_roll_link");
120  con1_spec.setBase("fetch1", "wrist_roll_link");
121  con1_spec.setPoseFromWorld(world);
122  auto con1_tsr = std::make_shared<darts::TSR>(world, con1_spec);
123  builder.addConstraint(con1_tsr);
124 
125  darts::TSR::Specification con2_spec;
126  con2_spec.setFrame("fetch1", "wrist_roll_link", "base_link");
127  con2_spec.setPoseFromWorld(world);
128  con2_spec.setNoPosTolerance();
129  con2_spec.setXRotTolerance(0.05);
130  con2_spec.setYRotTolerance(0.05);
131  con2_spec.setNoZRotTolerance();
132 
133  auto con2_tsr = std::make_shared<darts::TSR>(world, con2_spec);
134  builder.addConstraint(con2_tsr);
135 
136  builder.initialize();
137 
138  darts::TSR::Specification goal_spec;
139  goal_spec.setFrame("fetch1", "wrist_roll_link", "base_link");
140  goal_spec.setPose(0.5, 0, height + raise, //
141  0.707, 0, -0.707, 0);
142  auto goal_tsr = std::make_shared<darts::TSR>(world, goal_spec);
143  auto goal = builder.getGoalTSR(goal_tsr);
144  builder.setGoal(goal);
145 
146  auto rrt = std::make_shared<ompl::geometric::RRTConnect>(builder.info, true);
147  rrt->setRange(100.);
148  builder.ss->setPlanner(rrt);
149 
150  builder.setup();
151 
152  goal->options.max_samples = 100;
153  goal->startSampling();
154  ompl::base::PlannerStatus solved = builder.ss->solve(240.0);
155  goal->stopSampling();
156 
157  if (solved)
158  {
159  RBX_INFO("Found solution!");
160  window.animatePath(builder, builder.getSolutionPath());
161  }
162  else
163  RBX_WARN("No solution found");
164 
165  return solved == ompl::base::PlannerStatus::EXACT_SOLUTION;
166  };
167 
168  //
169  // Lower the box
170  //
171  const auto &lower = [&] {
172  darts::PlanBuilder builder(world);
173  builder.addGroup("fetch1", "arm_with_torso");
174  builder.addGroup("fetch2", "arm_with_torso");
175 
176  builder.setStartConfigurationFromWorld();
177 
178  darts::TSR::Specification con1_spec;
179  con1_spec.setTarget("fetch2", "wrist_roll_link");
180  con1_spec.setBase("fetch1", "wrist_roll_link");
181  con1_spec.setPoseFromWorld(world);
182  auto con1_tsr = std::make_shared<darts::TSR>(world, con1_spec);
183  builder.addConstraint(con1_tsr);
184 
185  darts::TSR::Specification con2_spec;
186  con2_spec.setFrame("fetch1", "wrist_roll_link", "base_link");
187  con2_spec.setPoseFromWorld(world);
188  con2_spec.setNoPosTolerance();
189  con2_spec.setXRotTolerance(0.05);
190  con2_spec.setYRotTolerance(0.05);
191  con2_spec.setNoZRotTolerance();
192 
193  auto con2_tsr = std::make_shared<darts::TSR>(world, con2_spec);
194  builder.addConstraint(con2_tsr);
195 
196  builder.initialize();
197 
198  darts::TSR::Specification goal_spec;
199  goal_spec.setFrame("fetch1", "wrist_roll_link", "base_link");
200  goal_spec.setPose(0.5, 0, height - 0.1, //
201  0.707, 0, -0.707, 0);
202  auto goal_tsr = std::make_shared<darts::TSR>(world, goal_spec);
203  auto goal = builder.getGoalTSR(goal_tsr);
204  builder.setGoal(goal);
205 
206  auto rrt = std::make_shared<ompl::geometric::RRTConnect>(builder.info, true);
207  rrt->setRange(100.);
208  builder.ss->setPlanner(rrt);
209 
210  builder.setup();
211 
212  goal->options.max_samples = 100;
213  goal->startSampling();
214  ompl::base::PlannerStatus solved = builder.ss->solve(240.0);
215  goal->stopSampling();
216 
217  if (solved)
218  {
219  RBX_INFO("Found solution!");
220  window.animatePath(builder, builder.getSolutionPath());
221  }
222  else
223  RBX_WARN("No solution found");
224 
225  return solved == ompl::base::PlannerStatus::EXACT_SOLUTION;
226  };
227 
228  //
229  // Tuck the Fetch's arms
230  //
231  const auto &tuck = [&] {
232  darts::PlanBuilder builder(world);
233  builder.addGroup("fetch1", "arm_with_torso");
234  builder.addGroup("fetch2", "arm_with_torso");
235 
236  builder.setStartConfigurationFromWorld();
237 
238  builder.initialize();
239 
240  auto goal = builder.getGoalConfiguration({
241  0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, //
242  0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0, //
243  });
244  builder.setGoal(goal);
245 
246  auto rrt = std::make_shared<ompl::geometric::RRTConnect>(builder.info, true);
247  rrt->setRange(1.);
248  builder.ss->setPlanner(rrt);
249 
250  builder.setup();
251 
252  ompl::base::PlannerStatus solved = builder.ss->solve(60.0);
253 
254  if (solved)
255  {
256  RBX_INFO("Found solution!");
257  window.animatePath(builder, builder.getSolutionPath());
258  }
259  else
260  RBX_WARN("No solution found");
261 
262  return solved == ompl::base::PlannerStatus::EXACT_SOLUTION;
263  };
264 
265  window.run([&]() {
266  RBX_INFO("Press enter");
267  std::cin.ignore();
268 
269  std::this_thread::sleep_for(std::chrono::milliseconds(2000));
270 
271  while (true)
272  {
273  if (touch())
274  {
275  auto *cube = scene->getFrame("box");
276  fetch1->reparentFreeFrame(cube, "wrist_roll_link");
277  if (lift())
278  {
279  if (lower())
280  {
281  scene->reparentFreeFrame(cube);
282  if (not tuck())
283  break;
284  }
285  }
286  }
287  }
288  });
289 
290  return 0;
291 }
robowflex::darts::PlanBuilder
A helper class to setup common OMPL structures for planning.
Definition: robowflex_dart/include/robowflex_dart/planning.h:222
robowflex::darts::TSR::Specification
The specification of a TSR.
Definition: tsr.h:70
robowflex::darts::TSR::Specification::setTarget
void setTarget(const std::string &structure, const std::string &frame)
Setting TSR Frame.
Definition: tsr.cpp:33
robowflex::darts::TSR::Specification::setBase
void setBase(const std::string &structure, const std::string &frame)
Set the base frame.
Definition: tsr.cpp:42
robowflex::darts::TSR::Specification::setPose
void setPose(const RobotPose &other)
Set the pose of the TSR.
Definition: tsr.cpp:93
robowflex::darts::TSR::Specification::setFrame
void setFrame(const std::string &structure, const std::string &target, const std::string &base=magic::ROOT_FRAME)
Set the base and target frame.
Definition: tsr.cpp:48
robowflex::darts::TSR::Specification::setNoPosTolerance
void setNoPosTolerance()
Set no position tolerance at all.
Definition: tsr.cpp:233
robowflex::darts::TSR::Specification::setYRotTolerance
void setYRotTolerance(double bound)
Set the Y orientation tolerance to (-bound, bound).
Definition: tsr.cpp:178
robowflex::darts::TSR::Specification::setNoZRotTolerance
void setNoZRotTolerance()
Set no orientation tolerance on the Z-axis.
Definition: tsr.cpp:250
robowflex::darts::TSR::Specification::setXRotTolerance
void setXRotTolerance(double bound)
Setting Orientation Tolerances.
Definition: tsr.cpp:173
robowflex::darts::TSR::Specification::setPoseFromWorld
void setPoseFromWorld(const WorldPtr &world)
Set the pose of the TSR for the desired frame in a provided world. Uses world's current configuration...
Definition: tsr.cpp:110
robowflex::darts::Window
Open Scene Graph GUI for DART visualization.
Definition: gui.h:67
RBX_WARN
#define RBX_WARN(fmt,...)
Output a warning logging message.
Definition: log.h:110
RBX_INFO
#define RBX_INFO(fmt,...)
Output a info logging message.
Definition: log.h:118
robowflex::darts::loadMoveItRobot
RobotPtr loadMoveItRobot(const std::string &name, const std::string &urdf, const std::string &srdf)
Load a robot from a URDF and SRDF (i.e., a MoveIt enabled robot).
Definition: robowflex_dart/src/robot.cpp:586
robowflex::darts::makeBox
std::shared_ptr< dart::dynamics::BoxShape > makeBox(const Eigen::Ref< const Eigen::Vector3d > &v)
Create a box.
Definition: structure.cpp:370
scene
Functions for loading and animating scenes in Blender.
std::this_thread::sleep_for
T sleep_for(T... args)