Skip to content

Commit

Permalink
Fix demos (#493)
Browse files Browse the repository at this point in the history 
* improve demo rviz config
  - orbit view centered at workspace instead of 2d orbit
  - REALTIME trajectory display
* FixedState: setter for ignore_collisions property
* Fix clearance_cost demo
  - verify default state
  - use model frame over virtual "world" which is not known in fresh PlanningScene
  - ignore initial collisions with the IK seed
* improve documentation in Fallbacks demo
  • Loading branch information
@v4hn
v4hn authored Oct 11, 2023
1 parent 64fd3df commit ab4bb40
Show file tree
Hide file tree
Showing 4 changed files with 162 additions and 42 deletions.
2 changes: 2 additions & 0 deletions core/include/moveit/task_constructor/stages/fixed_state.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,8 @@ class FixedState : public Generator
FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr scene = nullptr);
void setState(const planning_scene::PlanningScenePtr& scene);

void setIgnoreCollisions(bool ignore) { setProperty("ignore_collisions", ignore); }

void reset() override;
bool canCompute() const override;
void compute() override;
Expand Down
163 changes: 140 additions & 23 deletions demo/config/mtc.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@ Panels:
Expanded:
- /Motion Planning Tasks1
Splitter Ratio: 0.5393258333206177
Tree Height: 533
Tree Height: 597
- Class: rviz/Help
Name: Help
- Class: rviz/Views
Expand All @@ -17,25 +17,24 @@ Panels:
- Class: moveit_task_constructor/Motion Planning Tasks
Global Settings:
Task View Settings:
Old task handling: Keep
Show Computation Times: true
Task Expansion: All Expanded
Name: Motion Planning Tasks
Tasks View:
property_splitter:
- 540
- 600
- 0
solution_sorting:
column: 1
order: 0
solutions_splitter:
- 306
- 98
solutions_view_columns:
- 38
- 52
- 0
- 223
- 72
solutions_view_columns: ~
tasks_view_columns:
- 226
- 107
- 38
- 38
- 38
Preferences:
Expand Down Expand Up @@ -84,6 +83,65 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Expand All @@ -104,6 +162,65 @@ Visualization Manager:
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
panda_hand:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_leftfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link0:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link1:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link2:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link3:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link4:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link5:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link6:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link7:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
panda_link8:
Alpha: 1
Show Axes: false
Show Trail: false
panda_rightfinger:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 1
Show Robot Collision: false
Show Robot Visual: true
Expand All @@ -118,10 +235,10 @@ Visualization Manager:
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Show Trail: false
State Display Time: 0.05 s
State Display Time: REALTIME
Task Solution Topic: /mtc_tutorial/solution
Tasks:
Cartesian Path: 1
{}
Trail Step Size: 1
Value: true
Enabled: true
Expand All @@ -139,31 +256,31 @@ Visualization Manager:
Value: true
Views:
Current:
Class: rviz/XYOrbit
Distance: 1.3878222703933716
Class: rviz/Orbit
Distance: 1.7952697277069092
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.30880630016326904
Y: -0.1259305477142334
Z: 1.3560062370743253e-6
X: 0.18215136229991913
Y: 0.0822998434305191
Z: 0.374462366104126
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.264797568321228
Pitch: 0.48479723930358887
Target Frame: panda_link0
Yaw: 4.939944744110107
Yaw: 5.5999436378479
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 768
Height: 861
Help:
collapsed: false
Hide Left Dock: false
Expand All @@ -172,9 +289,9 @@ Window Geometry:
collapsed: false
Motion Planning Tasks - Slider:
collapsed: false
QMainWindow State: 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
QMainWindow State: 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
Views:
collapsed: false
Width: 1621
X: 249
Y: 25
Width: 1450
X: 1920
Y: 28
32 changes: 15 additions & 17 deletions demo/src/fallbacks_move_to.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,12 @@ constexpr double TAU = 2 * M_PI;

using namespace moveit::task_constructor;

/** CurrentState -> Fallbacks( MoveTo<CartesianPath>, MoveTo<PTP>, MoveTo<OMPL> )*/
/** Alternatives (3x FixedState with different states) -> Fallbacks(MoveTo<CartesianPath>, MoveTo<PTP>, MoveTo<OMPL>)
*
* This task demonstrates how to use the Fallbacks stage to try different planning approaches in propagator.
* Note that the initial states are all different, so this task does not describe any real-world scenario
* (where all plans should start from the same initial state for execution).
*/
int main(int argc, char** argv) {
ros::init(argc, argv, "mtc_tutorial");

Expand All @@ -22,6 +27,7 @@ int main(int argc, char** argv) {

// setup Task
Task t;
t.setName("fallback strategies in MoveTo");
t.loadRobotModel();
const moveit::core::RobotModelConstPtr robot{ t.getRobotModel() };

Expand All @@ -31,19 +37,13 @@ int main(int argc, char** argv) {
auto cartesian = std::make_shared<solvers::CartesianPath>();
cartesian->setJumpThreshold(2.0);

const auto ptp = []() {
auto pp{ std::make_shared<solvers::PipelinePlanner>("pilz_industrial_motion_planner") };
pp->setPlannerId("PTP");
return pp;
}();
auto ptp = std::make_shared<solvers::PipelinePlanner>("pilz_industrial_motion_planner");
ptp->setPlannerId("PTP");

const auto rrtconnect = []() {
auto pp{ std::make_shared<solvers::PipelinePlanner>("ompl") };
pp->setPlannerId("RRTConnectkConfigDefault");
return pp;
}();
auto rrtconnect = std::make_shared<solvers::PipelinePlanner>("ompl");
rrtconnect->setPlannerId("RRTConnect");

// target state for Task
// target end state for all Task plans
std::map<std::string, double> target_state;
robot->getJointModelGroup("panda_arm")->getVariableDefaultPositions("ready", target_state);
target_state["panda_joint1"] = +TAU / 8;
Expand All @@ -56,15 +56,15 @@ int main(int argc, char** argv) {

{
// can reach target with Cartesian motion
auto fixed{ std::make_unique<stages::FixedState>("current state") };
auto fixed{ std::make_unique<stages::FixedState>("close to target state in workspace") };
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } });
fixed->setState(scene);
initial_alternatives->add(std::move(fixed));
}
{
// Cartesian motion to target is impossible, but PTP is collision-free
auto fixed{ std::make_unique<stages::FixedState>("current state") };
auto fixed{ std::make_unique<stages::FixedState>("directly reachable without collision") };
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({
{ "panda_joint1", +TAU / 8 },
Expand All @@ -75,7 +75,7 @@ int main(int argc, char** argv) {
}
{
// Cartesian and PTP motion to target would be in collision
auto fixed = std::make_unique<stages::FixedState>("current state");
auto fixed{ std::make_unique<stages::FixedState>("getting to target requires collision avoidance") };
auto scene{ initial_scene->diff() };
scene->getCurrentStateNonConst().setVariablePositions({ { "panda_joint1", -TAU / 8 } });
scene->processCollisionObjectMsg([]() {
Expand Down Expand Up @@ -122,8 +122,6 @@ int main(int argc, char** argv) {
t.add(std::move(fallbacks));

try {
t.init();
std::cout << t << std::endl;
t.plan();
} catch (const InitStageException& e) {
std::cout << e << std::endl;
Expand Down
7 changes: 5 additions & 2 deletions demo/src/ik_clearance_cost.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,22 +31,25 @@ int main(int argc, char** argv) {
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
auto& robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues();
robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended");
[[maybe_unused]] bool found =
robot_state.setToDefaultValues(robot_state.getJointModelGroup("panda_arm"), "extended");
assert(found);

moveit_msgs::CollisionObject co;
co.id = "obstacle";
co.primitives.emplace_back();
co.primitives[0].type = shape_msgs::SolidPrimitive::SPHERE;
co.primitives[0].dimensions.resize(1);
co.primitives[0].dimensions[0] = 0.1;
co.header.frame_id = "world";
co.header.frame_id = t.getRobotModel()->getModelFrame();
co.primitive_poses.emplace_back();
co.primitive_poses[0].orientation.w = 1.0;
co.primitive_poses[0].position.z = 0.85;
scene->processCollisionObjectMsg(co);

auto initial = std::make_unique<stages::FixedState>();
initial->setState(scene);
initial->setIgnoreCollisions(true);

auto ik = std::make_unique<stages::ComputeIK>();
ik->insert(std::move(initial));
Expand Down

0 comments on commit ab4bb40

Please sign in to comment.