Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Merge upstream #2

Merged
merged 3 commits into from
Oct 26, 2023
Merged

Merge upstream #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ab4bb40
Fix demos (#493)
v4hn Oct 11, 2023
dcde89d
Skip Fallbacks::replaceImpl() when already correctly initialized (#494)
v4hn Oct 11, 2023
a905572
Hide button to show rviz-based task construction (#492)
v4hn Oct 11, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions core/include/moveit/task_constructor/stages/fixed_state.h
View file
Open in desktop
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
2 changes: 2 additions & 0 deletions core/src/container.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -876,6 +876,8 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {

inline void Fallbacks::replaceImpl() {
FallbacksPrivate *impl = pimpl();
if (pimpl()->interfaceFlags() == pimpl()->requiredInterface())
return;
switch (pimpl()->requiredInterface()) {
case GENERATE:
impl = new FallbacksPrivateGenerator(std::move(*impl));
Expand Down
163 changes: 140 additions & 23 deletions demo/config/mtc.rviz
View file
Open in desktop
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: 000000ff00000000fd00000002000000000000015600000303fc020000000efc0000003d00000303000000e60100001cfa000000000100000002fb000000100044006900730070006c0061007900730100000000000001770000015600fffffffb0000000a005600690065007700730100000000ffffffff0000010000fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000027d0000004b0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000013d000001a60000000000000000000000010000012b00000303fc0200000002fb0000002a004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b0073010000003d00000292000000e800fffffffb0000003c004d006f00740069006f006e00200050006c0061006e006e0069006e00670020005400610073006b00730020002d00200053006c006900640065007201000002d50000006b0000004100ffffff0000031d0000030300000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
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
Open in desktop
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
Open in desktop
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
2 changes: 1 addition & 1 deletion visualization/motion_planning_tasks/src/task_panel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -177,7 +177,7 @@ TaskPanelPrivate::TaskPanelPrivate(TaskPanel* panel) : q_ptr(panel) {
tool_buttons_group = new QButtonGroup(panel);
tool_buttons_group->setExclusive(true);
button_show_stage_dock_widget->setEnabled(bool(getStageFactory()));
button_show_stage_dock_widget->setToolTip("Show available stages");
button_show_stage_dock_widget->setVisible(false); // hide for now
property_root = new rviz::Property("Global Settings");
}

Expand Down
Loading