diff --git a/MIGRATION.md b/MIGRATION.md
index fa60a34604..c324638bd0 100644
--- a/MIGRATION.md
+++ b/MIGRATION.md
@@ -29,6 +29,7 @@ API changes in MoveIt releases
 - The joint states of `passive` joints must be published in ROS and the CurrentStateMonitor will now wait for them as well. Their semantics dictate that they cannot be actively controlled, but they must be known to use the full robot state in collision checks. (https://github.com/ros-planning/moveit/pull/2663)
 - Removed deprecated header `moveit/macros/deprecation.h`. Use `[[deprecated]]` instead.
 - All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase).
+- In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `<ns>/move_group/display_planned_path` instead of `/move_group/display_planned_path`).
 
 ## ROS Melodic
 
diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml
index 7fe1596ca6..8bd6f9d2d1 100644
--- a/moveit_commander/package.xml
+++ b/moveit_commander/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>moveit_commander</name>
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <description>Python interfaces to MoveIt</description>
   <maintainer email="me@v4hn.de">Michael Görner</maintainer>
   <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h
index 901ff8d7e1..7446f2f160 100644
--- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h
+++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/bullet_utils.h
@@ -409,7 +409,7 @@ inline btScalar addDiscreteSingleResult(btManifoldPoint& cp, const btCollisionOb
   contact.nearest_points[1] = convertBtToEigen(cp.m_positionWorldOnB);
 
   contact.body_type_1 = cd0->getTypeID();
-  contact.body_type_2 = cd0->getTypeID();
+  contact.body_type_2 = cd1->getTypeID();
 
   if (!processResult(collisions, contact, pc, found))
   {
@@ -442,7 +442,7 @@ inline btScalar addCastSingleResult(btManifoldPoint& cp, const btCollisionObject
   contact.pos = convertBtToEigen(cp.m_positionWorldOnA);
 
   contact.body_type_1 = cd0->getTypeID();
-  contact.body_type_2 = cd0->getTypeID();
+  contact.body_type_2 = cd1->getTypeID();
 
   collision_detection::Contact* col = processResult(collisions, contact, pc, found);
 
diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp
index 5513849533..0eada24361 100644
--- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp
+++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp
@@ -313,6 +313,21 @@ TEST_F(BulletCollisionDetectionTester, ContinuousCollisionWorld)
   cenv_->checkRobotCollision(req, res, state1, state2, *acm_);
   ASSERT_TRUE(res.collision);
   ASSERT_EQ(res.contact_count, 4u);
+  // test contact types
+  for (auto& contact_pair : res.contacts)
+  {
+    for (collision_detection::Contact& contact : contact_pair.second)
+    {
+      collision_detection::BodyType contact_type1 = contact.body_name_1 == "box" ?
+                                                        collision_detection::BodyType::WORLD_OBJECT :
+                                                        collision_detection::BodyType::ROBOT_LINK;
+      collision_detection::BodyType contact_type2 = contact.body_name_2 == "box" ?
+                                                        collision_detection::BodyType::WORLD_OBJECT :
+                                                        collision_detection::BodyType::ROBOT_LINK;
+      ASSERT_EQ(contact.body_type_1, contact_type1);
+      ASSERT_EQ(contact.body_type_2, contact_type2);
+    }
+  }
   res.clear();
 }
 
diff --git a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
index d94cd395b2..8459067ee2 100644
--- a/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
+++ b/moveit_core/robot_state/include/moveit/robot_state/robot_state.h
@@ -1225,6 +1225,15 @@ class RobotState
   /** \brief Set the joints in \e group to the position \e name defined in the SRDF */
   bool setToDefaultValues(const JointModelGroup* group, const std::string& name);
 
+  bool setToDefaultValues(const std::string& group_name, const std::string& state_name)
+  {
+    const JointModelGroup* jmg = getJointModelGroup(group_name);
+    if (jmg)
+      return setToDefaultValues(jmg, state_name);
+    else
+      return false;
+  }
+
   /** \brief Set all joints to random values.  Values will be within default bounds. */
   void setToRandomPositions();
 
diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml
index 8f6ca8f7a0..36f2fe9d24 100644
--- a/moveit_planners/chomp/chomp_interface/package.xml
+++ b/moveit_planners/chomp/chomp_interface/package.xml
@@ -2,7 +2,7 @@
   <name>moveit_planners_chomp</name>
   <description>The interface for using CHOMP within MoveIt</description>
 
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <author email="gjones@willowgarage.com">Gil Jones</author>
 
   <maintainer email="chitt@live.in">Chittaranjan Srinivas Swaminathan</maintainer>
diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml
index b0d8515033..3ddc837c28 100644
--- a/moveit_planners/chomp/chomp_motion_planner/package.xml
+++ b/moveit_planners/chomp/chomp_motion_planner/package.xml
@@ -2,7 +2,7 @@
   <name>chomp_motion_planner</name>
   <description>chomp_motion_planner</description>
 
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <author email="gjones@willowgarage.com">Gil Jones</author>
   <author email="mail@mrinal.net">Mrinal Kalakrishnan</author>
 
diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
index 98ed35836a..964630594c 100644
--- a/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
+++ b/moveit_planners/chomp/chomp_optimizer_adapter/package.xml
@@ -3,7 +3,7 @@
   <name>moveit_chomp_optimizer_adapter</name>
   <description>MoveIt planning request adapter utilizing chomp for solution optimization</description>
 
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <author email="raghavendersahdev@gmail.com">Raghavender Sahdev</author>
   <maintainer email="raghavendersahdev@gmail.com">Raghavender Sahdev</maintainer>
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>
diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png
index 724cdf1911..93f641245d 100644
Binary files a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png and b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.png differ
diff --git a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf
index 1a8a3bc7c5..a0613d8eb0 100644
--- a/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf
+++ b/moveit_planners/pilz_industrial_motion_planner/doc/diag_class_planning_context.uxf
@@ -3,8 +3,8 @@
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>287</x>
-      <y>273</y>
+      <x>357</x>
+      <y>399</y>
       <w>567</w>
       <h>133</h>
     </coordinates>
@@ -23,8 +23,8 @@
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>0</x>
-      <y>525</y>
+      <x>70</x>
+      <y>651</y>
       <w>259</w>
       <h>70</h>
     </coordinates>
@@ -39,8 +39,8 @@ PlanningContextLoaderPTP
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>448</x>
-      <y>525</y>
+      <x>518</x>
+      <y>651</y>
       <w>259</w>
       <h>70</h>
     </coordinates>
@@ -55,8 +55,8 @@ PlanningContextLoaderLIN
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>119</x>
-      <y>350</y>
+      <x>189</x>
+      <y>476</y>
       <w>182</w>
       <h>189</h>
     </coordinates>
@@ -66,8 +66,8 @@ PlanningContextLoaderLIN
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>567</x>
-      <y>399</y>
+      <x>637</x>
+      <y>525</y>
       <w>21</w>
       <h>140</h>
     </coordinates>
@@ -77,8 +77,8 @@ PlanningContextLoaderLIN
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>35</x>
-      <y>756</y>
+      <x>105</x>
+      <y>882</y>
       <w>133</w>
       <h>35</h>
     </coordinates>
@@ -90,8 +90,8 @@ PlanningContextLoaderLIN
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>511</x>
-      <y>749</y>
+      <x>581</x>
+      <y>875</y>
       <w>126</w>
       <h>35</h>
     </coordinates>
@@ -103,8 +103,8 @@ PlanningContextLoaderLIN
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>1260</x>
-      <y>616</y>
+      <x>1330</x>
+      <y>742</y>
       <w>119</w>
       <h>126</h>
     </coordinates>
@@ -117,8 +117,8 @@ PlanningContextBase
   <element>
     <id>Text</id>
     <coordinates>
-      <x>21</x>
-      <y>609</y>
+      <x>91</x>
+      <y>735</y>
       <w>175</w>
       <h>14</h>
     </coordinates>
@@ -128,8 +128,8 @@ PlanningContextBase
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>133</x>
-      <y>644</y>
+      <x>203</x>
+      <y>770</y>
       <w>1141</w>
       <h>126</h>
     </coordinates>
@@ -139,8 +139,8 @@ PlanningContextBase
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>602</x>
-      <y>672</y>
+      <x>672</x>
+      <y>798</y>
       <w>672</w>
       <h>91</h>
     </coordinates>
@@ -150,8 +150,8 @@ PlanningContextBase
   <element>
     <id>Text</id>
     <coordinates>
-      <x>490</x>
-      <y>609</y>
+      <x>560</x>
+      <y>735</y>
       <w>175</w>
       <h>14</h>
     </coordinates>
@@ -161,8 +161,8 @@ PlanningContextBase
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>511</x>
-      <y>1001</y>
+      <x>581</x>
+      <y>1127</y>
       <w>126</w>
       <h>35</h>
     </coordinates>
@@ -174,8 +174,8 @@ PlanningContextBase
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>98</x>
-      <y>588</y>
+      <x>168</x>
+      <y>714</y>
       <w>21</w>
       <h>182</h>
     </coordinates>
@@ -185,8 +185,8 @@ PlanningContextBase
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>35</x>
-      <y>1001</y>
+      <x>105</x>
+      <y>1127</y>
       <w>126</w>
       <h>35</h>
     </coordinates>
@@ -198,8 +198,8 @@ PlanningContextBase
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>1176</x>
-      <y>861</y>
+      <x>1246</x>
+      <y>987</y>
       <w>308</w>
       <h>98</h>
     </coordinates>
@@ -207,15 +207,15 @@ PlanningContextBase
 --
 --
 TrajectoryGenerator(RobotModelConstPtr, LimitsContainer)
-generate(request, response, sampling_time)
+generate(scene, request, response, sampling_time)
 </panel_attributes>
     <additional_attributes/>
   </element>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>567</x>
-      <y>588</y>
+      <x>637</x>
+      <y>714</y>
       <w>21</w>
       <h>175</h>
     </coordinates>
@@ -225,8 +225,8 @@ generate(request, response, sampling_time)
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>567</x>
-      <y>777</y>
+      <x>637</x>
+      <y>903</y>
       <w>21</w>
       <h>238</h>
     </coordinates>
@@ -237,8 +237,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>98</x>
-      <y>784</y>
+      <x>168</x>
+      <y>910</y>
       <w>21</w>
       <h>231</h>
     </coordinates>
@@ -249,8 +249,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>602</x>
-      <y>903</y>
+      <x>672</x>
+      <y>1029</y>
       <w>588</w>
       <h>112</h>
     </coordinates>
@@ -260,8 +260,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>126</x>
-      <y>875</y>
+      <x>196</x>
+      <y>1001</y>
       <w>1064</w>
       <h>140</h>
     </coordinates>
@@ -271,8 +271,8 @@ m1=1</panel_attributes>
   <element>
     <id>UMLNote</id>
     <coordinates>
-      <x>0</x>
-      <y>441</y>
+      <x>70</x>
+      <y>567</y>
       <w>98</w>
       <h>49</h>
     </coordinates>
@@ -283,8 +283,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>42</x>
-      <y>483</y>
+      <x>112</x>
+      <y>609</y>
       <w>21</w>
       <h>56</h>
     </coordinates>
@@ -294,8 +294,8 @@ m1=1</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>1589</x>
-      <y>525</y>
+      <x>1659</x>
+      <y>651</y>
       <w>147</w>
       <h>112</h>
     </coordinates>
@@ -309,8 +309,8 @@ m1=1</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>1792</x>
-      <y>525</y>
+      <x>1862</x>
+      <y>651</y>
       <w>147</w>
       <h>112</h>
     </coordinates>
@@ -330,8 +330,8 @@ m1=1</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>1673</x>
-      <y>399</y>
+      <x>1743</x>
+      <y>525</y>
       <w>196</w>
       <h>91</h>
     </coordinates>
@@ -349,8 +349,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>1764</x>
-      <y>483</y>
+      <x>1834</x>
+      <y>609</y>
       <w>126</w>
       <h>56</h>
     </coordinates>
@@ -361,8 +361,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>1659</x>
-      <y>483</y>
+      <x>1729</x>
+      <y>609</y>
       <w>70</w>
       <h>56</h>
     </coordinates>
@@ -373,8 +373,8 @@ m1=1</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>168</x>
-      <y>1232</y>
+      <x>238</x>
+      <y>1358</y>
       <w>126</w>
       <h>42</h>
     </coordinates>
@@ -388,8 +388,8 @@ m1=1</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>28</x>
-      <y>1302</y>
+      <x>98</x>
+      <y>1428</y>
       <w>147</w>
       <h>63</h>
     </coordinates>
@@ -407,8 +407,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>126</x>
-      <y>1246</y>
+      <x>196</x>
+      <y>1372</y>
       <w>56</w>
       <h>70</h>
     </coordinates>
@@ -418,8 +418,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>91</x>
-      <y>1029</y>
+      <x>161</x>
+      <y>1155</y>
       <w>21</w>
       <h>287</h>
     </coordinates>
@@ -430,8 +430,8 @@ m1=n</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>525</x>
-      <y>1316</y>
+      <x>595</x>
+      <y>1442</y>
       <w>91</w>
       <h>42</h>
     </coordinates>
@@ -445,8 +445,8 @@ Path_Line
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>833</x>
-      <y>1316</y>
+      <x>903</x>
+      <y>1442</y>
       <w>84</w>
       <h>42</h>
     </coordinates>
@@ -460,8 +460,8 @@ Path_Circle
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>980</x>
-      <y>1316</y>
+      <x>1050</x>
+      <y>1442</y>
       <w>105</w>
       <h>42</h>
     </coordinates>
@@ -475,8 +475,8 @@ Path_Circle_Wrapper
   <element>
     <id>UMLNote</id>
     <coordinates>
-      <x>1393</x>
-      <y>1176</y>
+      <x>1463</x>
+      <y>1302</y>
       <w>98</w>
       <h>49</h>
     </coordinates>
@@ -487,8 +487,8 @@ bg=white</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>1330</x>
-      <y>1190</y>
+      <x>1400</x>
+      <y>1316</y>
       <w>77</w>
       <h>21</h>
     </coordinates>
@@ -498,8 +498,8 @@ bg=white</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>560</x>
-      <y>1029</y>
+      <x>630</x>
+      <y>1155</y>
       <w>21</w>
       <h>301</h>
     </coordinates>
@@ -509,8 +509,8 @@ bg=white</panel_attributes>
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>882</x>
-      <y>1001</y>
+      <x>952</x>
+      <y>1127</y>
       <w>168</w>
       <h>35</h>
     </coordinates>
@@ -522,8 +522,8 @@ bg=white</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>1022</x>
-      <y>1029</y>
+      <x>1092</x>
+      <y>1155</y>
       <w>21</w>
       <h>301</h>
     </coordinates>
@@ -533,8 +533,8 @@ bg=white</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>581</x>
-      <y>1197</y>
+      <x>651</x>
+      <y>1323</y>
       <w>693</w>
       <h>133</h>
     </coordinates>
@@ -544,8 +544,8 @@ bg=white</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>889</x>
-      <y>1232</y>
+      <x>959</x>
+      <y>1358</y>
       <w>385</w>
       <h>98</h>
     </coordinates>
@@ -555,8 +555,8 @@ bg=white</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>1050</x>
-      <y>1267</y>
+      <x>1120</x>
+      <y>1393</y>
       <w>224</w>
       <h>63</h>
     </coordinates>
@@ -566,8 +566,8 @@ bg=white</panel_attributes>
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>896</x>
-      <y>742</y>
+      <x>966</x>
+      <y>868</y>
       <w>126</w>
       <h>35</h>
     </coordinates>
@@ -579,8 +579,8 @@ bg=white</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>952</x>
-      <y>770</y>
+      <x>1022</x>
+      <y>896</y>
       <w>21</w>
       <h>245</h>
     </coordinates>
@@ -591,8 +591,8 @@ m1=1</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>987</x>
-      <y>700</y>
+      <x>1057</x>
+      <y>826</y>
       <w>287</w>
       <h>56</h>
     </coordinates>
@@ -602,8 +602,8 @@ m1=1</panel_attributes>
   <element>
     <id>UMLGeneric</id>
     <coordinates>
-      <x>826</x>
-      <y>525</y>
+      <x>896</x>
+      <y>651</y>
       <w>259</w>
       <h>70</h>
     </coordinates>
@@ -618,8 +618,8 @@ PlanningContextLoaderCIRC
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>952</x>
-      <y>588</y>
+      <x>1022</x>
+      <y>714</y>
       <w>21</w>
       <h>168</h>
     </coordinates>
@@ -629,8 +629,8 @@ PlanningContextLoaderCIRC
   <element>
     <id>Text</id>
     <coordinates>
-      <x>875</x>
-      <y>609</y>
+      <x>945</x>
+      <y>735</y>
       <w>175</w>
       <h>14</h>
     </coordinates>
@@ -640,8 +640,8 @@ PlanningContextLoaderCIRC
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>987</x>
-      <y>931</y>
+      <x>1057</x>
+      <y>1057</y>
       <w>203</w>
       <h>84</h>
     </coordinates>
@@ -651,8 +651,8 @@ PlanningContextLoaderCIRC
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>847</x>
-      <y>350</y>
+      <x>917</x>
+      <y>476</y>
       <w>119</w>
       <h>189</h>
     </coordinates>
@@ -662,8 +662,8 @@ PlanningContextLoaderCIRC
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>91</x>
-      <y>469</y>
+      <x>161</x>
+      <y>595</y>
       <w>434</w>
       <h>70</h>
     </coordinates>
@@ -673,8 +673,8 @@ PlanningContextLoaderCIRC
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>91</x>
-      <y>448</y>
+      <x>161</x>
+      <y>574</y>
       <w>812</w>
       <h>91</h>
     </coordinates>
@@ -684,8 +684,8 @@ PlanningContextLoaderCIRC
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>287</x>
-      <y>126</y>
+      <x>357</x>
+      <y>252</y>
       <w>567</w>
       <h>63</h>
     </coordinates>
@@ -702,8 +702,8 @@ moveit_msgs::MoveItErrorCodes&amp; error_code):planning_interface::PlanningConte
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>567</x>
-      <y>182</y>
+      <x>637</x>
+      <y>308</y>
       <w>21</w>
       <h>105</h>
     </coordinates>
@@ -714,8 +714,8 @@ m1=n</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>476</x>
-      <y>0</y>
+      <x>546</x>
+      <y>126</y>
       <w>203</w>
       <h>49</h>
     </coordinates>
@@ -729,8 +729,8 @@ m1=n</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>567</x>
-      <y>42</y>
+      <x>637</x>
+      <y>168</y>
       <w>21</w>
       <h>98</h>
     </coordinates>
@@ -740,8 +740,8 @@ m1=n</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>1211</x>
-      <y>518</y>
+      <x>1281</x>
+      <y>644</y>
       <w>203</w>
       <h>49</h>
     </coordinates>
@@ -755,8 +755,8 @@ m1=n</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>1309</x>
-      <y>560</y>
+      <x>1379</x>
+      <y>686</y>
       <w>21</w>
       <h>77</h>
     </coordinates>
@@ -766,8 +766,8 @@ m1=n</panel_attributes>
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>910</x>
-      <y>1330</y>
+      <x>980</x>
+      <y>1456</y>
       <w>84</w>
       <h>21</h>
     </coordinates>
@@ -777,8 +777,8 @@ m1=n</panel_attributes>
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>308</x>
-      <y>1302</y>
+      <x>378</x>
+      <y>1428</y>
       <w>147</w>
       <h>63</h>
     </coordinates>
@@ -796,8 +796,8 @@ VelocityProfile_Trap
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>420</x>
-      <y>1008</y>
+      <x>490</x>
+      <y>1134</y>
       <w>476</w>
       <h>308</h>
     </coordinates>
@@ -807,8 +807,8 @@ VelocityProfile_Trap
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>770</x>
-      <y>1141</y>
+      <x>840</x>
+      <y>1267</y>
       <w>126</w>
       <h>42</h>
     </coordinates>
@@ -822,8 +822,8 @@ Trajectory_Segment
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>889</x>
-      <y>1029</y>
+      <x>959</x>
+      <y>1155</y>
       <w>63</w>
       <h>147</h>
     </coordinates>
@@ -833,8 +833,8 @@ Trajectory_Segment
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>287</x>
-      <y>1246</y>
+      <x>357</x>
+      <y>1372</y>
       <w>63</w>
       <h>70</h>
     </coordinates>
@@ -844,8 +844,8 @@ Trajectory_Segment
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>784</x>
-      <y>1064</y>
+      <x>854</x>
+      <y>1190</y>
       <w>98</w>
       <h>35</h>
     </coordinates>
@@ -859,8 +859,8 @@ Trajectory_Segment
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>826</x>
-      <y>1092</y>
+      <x>896</x>
+      <y>1218</y>
       <w>21</w>
       <h>63</h>
     </coordinates>
@@ -870,8 +870,8 @@ Trajectory_Segment
   <element>
     <id>UMLClass</id>
     <coordinates>
-      <x>1260</x>
-      <y>1183</y>
+      <x>1330</x>
+      <y>1309</y>
       <w>77</w>
       <h>112</h>
     </coordinates>
@@ -884,8 +884,8 @@ Trajectory_Segment
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>588</x>
-      <y>1029</y>
+      <x>658</x>
+      <y>1155</y>
       <w>196</w>
       <h>147</h>
     </coordinates>
@@ -895,8 +895,8 @@ Trajectory_Segment
   <element>
     <id>Relation</id>
     <coordinates>
-      <x>357</x>
-      <y>1008</y>
+      <x>427</x>
+      <y>1134</y>
       <w>168</w>
       <h>308</h>
     </coordinates>
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h
index a89b3fbbd3..4c15153283 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/plan_components_builder.h
@@ -39,6 +39,7 @@
 
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
+#include <moveit/planning_interface/planning_interface.h>
 
 #include "pilz_industrial_motion_planner/trajectory_blend_request.h"
 #include "pilz_industrial_motion_planner/trajectory_blender.h"
@@ -87,13 +88,16 @@ class PlanComponentsBuilder
    * appended/attached to the newly created empty trajectory:
    *      - if the given and previous trajectory are from different groups.
    *
+   * @param planning_scene The scene planning is occurring in.
+   *
    * @param other Trajectories which has to be added to the trajectory container
    * under construction.
    *
    * @param blend_radius The blending radius between the previous and the
    * specified trajectory.
    */
-  void append(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius);
+  void append(const planning_scene::PlanningSceneConstPtr& planning_scene,
+              const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius);
 
   /**
    * @brief Clears the trajectory container under construction.
@@ -106,7 +110,8 @@ class PlanComponentsBuilder
   std::vector<robot_trajectory::RobotTrajectoryPtr> build() const;
 
 private:
-  void blend(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius);
+  void blend(const planning_scene::PlanningSceneConstPtr& planning_scene,
+             const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius);
 
 private:
   /**
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h
index 14143a0ed8..86fb36dbaf 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/planning_context_base.h
@@ -128,7 +128,7 @@ bool pilz_industrial_motion_planner::PlanningContextBase<GeneratorT>::solve(plan
       moveit::core::robotStateToRobotStateMsg(getPlanningScene()->getCurrentState(), current_state);
       request_.start_state = current_state;
     }
-    bool result = generator_.generate(request_, res);
+    bool result = generator_.generate(getPlanningScene(), request_, res);
     return result;
     // res.error_code_.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
     // return false; // TODO
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h
index ce647b5479..6c7667fe1d 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender.h
@@ -36,6 +36,7 @@
 
 #include "pilz_industrial_motion_planner/limits_container.h"
 #include <moveit/robot_model/robot_model.h>
+#include <moveit/planning_interface/planning_interface.h>
 
 #include "pilz_industrial_motion_planner/trajectory_blend_request.h"
 #include "pilz_industrial_motion_planner/trajectory_blend_response.h"
@@ -58,11 +59,13 @@ class TrajectoryBlender
 
   /**
    * @brief Blend two robot trajectories with the given blending radius
+   * @param planning_scene: planning scene
    * @param req: trajectory blend request
    * @param res: trajectroy blend response
    * @return true if blend succeed
    */
-  virtual bool blend(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
+  virtual bool blend(const planning_scene::PlanningSceneConstPtr& planning_scene,
+                     const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
                      pilz_industrial_motion_planner::TrajectoryBlendResponse& res) = 0;
 
 protected:
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h
index 233bdd3e31..fd7608025b 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_blender_transition_window.h
@@ -34,6 +34,7 @@
 
 #pragma once
 
+#include <moveit/planning_interface/planning_interface.h>
 #include "pilz_industrial_motion_planner/cartesian_trajectory.h"
 #include "pilz_industrial_motion_planner/cartesian_trajectory_point.h"
 #include "pilz_industrial_motion_planner/trajectory_blend_request.h"
@@ -64,6 +65,7 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender
    * @brief Blend two trajectories using transition window. The trajectories
    * have to be equally and uniformly
    * discretized.
+   * @param planning_scene: The scene planning is occurring in.
    * @param req: following fields need to be filled for a valid request:
    *    - group_name : name of the planning group
    *    - link_name : name of the target link
@@ -91,7 +93,8 @@ class TrajectoryBlenderTransitionWindow : public TrajectoryBlender
    * error_code: information of failed blend
    * @return true if succeed
    */
-  bool blend(const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
+  bool blend(const planning_scene::PlanningSceneConstPtr& planning_scene,
+             const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
              pilz_industrial_motion_planner::TrajectoryBlendResponse& res) override;
 
 private:
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h
index cb600259b4..fc78cf994b 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h
@@ -41,6 +41,7 @@
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <tf2/transform_datatypes.h>
 #include <trajectory_msgs/MultiDOFJointTrajectory.h>
+#include <moveit/planning_scene/planning_scene.h>
 
 #include "pilz_industrial_motion_planner/cartesian_trajectory.h"
 #include "pilz_industrial_motion_planner/limits_container.h"
@@ -50,6 +51,7 @@ namespace pilz_industrial_motion_planner
 /**
  * @brief compute the inverse kinematics of a given pose, also check robot self
  * collision
+ * @param scene: planning scene
  * @param robot_model: kinematic model of the robot
  * @param group_name: name of planning group
  * @param link_name: name of target link
@@ -62,12 +64,12 @@ namespace pilz_industrial_motion_planner
  * @param timeout: timeout for IK, if not set the default solver timeout is used
  * @return true if succeed
  */
-bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
+bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name,
                    const std::string& link_name, const Eigen::Isometry3d& pose, const std::string& frame_id,
                    const std::map<std::string, double>& seed, std::map<std::string, double>& solution,
                    bool check_self_collision = true, const double timeout = 0.0);
 
-bool computePoseIK(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name,
+bool computePoseIK(const planning_scene::PlanningSceneConstPtr& scene, const std::string& group_name,
                    const std::string& link_name, const geometry_msgs::Pose& pose, const std::string& frame_id,
                    const std::map<std::string, double>& seed, std::map<std::string, double>& solution,
                    bool check_self_collision = true, const double timeout = 0.0);
@@ -107,6 +109,7 @@ bool verifySampleJointLimits(const std::map<std::string, double>& position_last,
 
 /**
  * @brief Generate joint trajectory from a KDL Cartesian trajectory
+ * @param scene: planning scene
  * @param robot_model: robot kinematics model
  * @param joint_limits: joint limits
  * @param trajectory: KDL Cartesian trajectory
@@ -122,7 +125,7 @@ bool verifySampleJointLimits(const std::map<std::string, double>& position_last,
  * @param check_self_collision: check for self collision during creation
  * @return true if succeed
  */
-bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model,
+bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene,
                              const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory,
                              const std::string& group_name, const std::string& link_name,
                              const std::map<std::string, double>& initial_joint_position, const double& sampling_time,
@@ -131,6 +134,7 @@ bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model,
 
 /**
  * @brief Generate joint trajectory from a MultiDOFJointTrajectory
+ * @param scene: planning scene
  * @param trajectory: Cartesian trajectory
  * @param info: motion plan information
  * @param sampling_time
@@ -138,7 +142,7 @@ bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model,
  * @param error_code
  * @return true if succeed
  */
-bool generateJointTrajectory(const robot_model::RobotModelConstPtr& robot_model,
+bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene,
                              const JointLimitsContainer& joint_limits,
                              const pilz_industrial_motion_planner::CartesianTrajectory& trajectory,
                              const std::string& group_name, const std::string& link_name,
@@ -202,6 +206,7 @@ bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p
 
 /**
  * @brief Checks if current robot state is in self collision.
+ * @param scene: planning scene.
  * @param test_for_self_collision Flag to deactivate this check during IK.
  * @param robot_model: robot kinematics model.
  * @param state Robot state instance used for .
@@ -209,7 +214,7 @@ bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p
  * @param ik_solution
  * @return
  */
-bool isStateColliding(const bool test_for_self_collision, const moveit::core::RobotModelConstPtr& robot_model,
+bool isStateColliding(const bool test_for_self_collision, const planning_scene::PlanningSceneConstPtr& scene,
                       robot_state::RobotState* state, const robot_state::JointModelGroup* const group,
                       const double* const ik_solution);
 }  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h
index 09055fcb81..4cb7d261ea 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h
@@ -42,6 +42,7 @@
 #include <kdl/trajectory.hpp>
 #include <moveit/planning_interface/planning_interface.h>
 #include <moveit/robot_model/robot_model.h>
+#include <moveit/planning_scene/planning_scene.h>
 
 #include "pilz_industrial_motion_planner/joint_limits_extension.h"
 #include "pilz_industrial_motion_planner/limits_container.h"
@@ -103,10 +104,10 @@ class TrajectoryGenerator
    * @param res: motion plan response
    * @param sampling_time: sampling time of the generate trajectory
    * @return motion plan succeed/fail, detailed information in motion plan
-   * responce
+   * response
    */
-  bool generate(const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
-                double sampling_time = 0.1);
+  bool generate(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
+                planning_interface::MotionPlanResponse& res, double sampling_time = 0.1);
 
 protected:
   /**
@@ -144,13 +145,16 @@ class TrajectoryGenerator
    * @brief Extract needed information from a motion plan request in order to
    * simplify
    * further usages.
+   * @param scene: planning scene
    * @param req: motion plan request
    * @param info: information extracted from motion plan request which is
    * necessary for the planning
    */
-  virtual void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const = 0;
+  virtual void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                                     const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const = 0;
 
-  virtual void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
+  virtual void plan(const planning_scene::PlanningSceneConstPtr& scene,
+                    const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                     const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) = 0;
 
 private:
@@ -200,7 +204,7 @@ class TrajectoryGenerator
   /**
    * @brief set MotionPlanResponse from joint trajectory
    */
-  void setSuccessResponse(const std::string& group_name, const moveit_msgs::RobotState& start_state,
+  void setSuccessResponse(const moveit::core::RobotState& start_rs, const std::string& group_name,
                           const trajectory_msgs::JointTrajectory& joint_trajectory, const ros::Time& planning_start,
                           planning_interface::MotionPlanResponse& res) const;
 
@@ -230,10 +234,6 @@ class TrajectoryGenerator
 
   void checkCartesianGoalConstraint(const moveit_msgs::Constraints& constraint, const std::string& group_name) const;
 
-  void convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory,
-                                const moveit_msgs::RobotState& start_state,
-                                robot_trajectory::RobotTrajectory& robot_trajectory) const;
-
 private:
   /**
    * @return True if scaling factor is valid, otherwise false.
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h
index 080239a581..13dda37739 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h
@@ -37,6 +37,7 @@
 #include <eigen3/Eigen/Eigen>
 #include <kdl/path.hpp>
 #include <kdl/velocityprofile.hpp>
+#include <moveit/planning_scene/planning_scene.h>
 
 #include "pilz_industrial_motion_planner/trajectory_generator.h"
 
@@ -83,10 +84,12 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator
 private:
   void cmdSpecificRequestValidation(const planning_interface::MotionPlanRequest& req) const override;
 
-  void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
+  void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                             const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
 
-  void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
-            const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override;
+  void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
+            const MotionPlanInfo& plan_info, const double& sampling_time,
+            trajectory_msgs::JointTrajectory& joint_trajectory) override;
 
   /**
    * @brief Construct a KDL::Path object for a Cartesian path of an arc.
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h
index 2d17768cad..269c9c0151 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h
@@ -36,6 +36,7 @@
 
 #include <eigen3/Eigen/Eigen>
 #include <kdl/rotational_interpolation_sa.hpp>
+#include <moveit/planning_scene/planning_scene.h>
 
 #include "pilz_industrial_motion_planner/trajectory_generator.h"
 #include "pilz_industrial_motion_planner/velocity_profile_atrap.h"
@@ -70,10 +71,12 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator
                          const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
 
 private:
-  void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
+  void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                             const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final;
 
-  void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
-            const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override;
+  void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
+            const MotionPlanInfo& plan_info, const double& sampling_time,
+            trajectory_msgs::JointTrajectory& joint_trajectory) override;
 
   /**
    * @brief construct a KDL::Path object for a Cartesian straight line
diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h
index 09048557ba..2ec0097ffb 100644
--- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h
+++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h
@@ -34,6 +34,8 @@
 
 #pragma once
 
+#include <moveit/planning_scene/planning_scene.h>
+
 #include "eigen3/Eigen/Eigen"
 #include "pilz_industrial_motion_planner/trajectory_generation_exceptions.h"
 #include "pilz_industrial_motion_planner/trajectory_generator.h"
@@ -64,7 +66,8 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
                          const pilz_industrial_motion_planner::LimitsContainer& planner_limits);
 
 private:
-  void extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const override;
+  void extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                             const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const override;
 
   /**
    * @brief plan ptp joint trajectory with zero start velocity
@@ -81,8 +84,9 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator
                const double& velocity_scaling_factor, const double& acceleration_scaling_factor,
                const double& sampling_time);
 
-  void plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
-            const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory) override;
+  void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req,
+            const MotionPlanInfo& plan_info, const double& sampling_time,
+            trajectory_msgs::JointTrajectory& joint_trajectory) override;
 
 private:
   const double MIN_MOVEMENT = 0.001;
diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml
index ebaf5fd17e..c149e396d3 100644
--- a/moveit_planners/pilz_industrial_motion_planner/package.xml
+++ b/moveit_planners/pilz_industrial_motion_planner/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>pilz_industrial_motion_planner</name>
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <description>MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof.</description>
 
   <maintainer email="a.gutenkunst@pilz.de">Alexander Gutenkunst</maintainer>
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
index 52941969d2..5a544b13a3 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp
@@ -97,7 +97,7 @@ RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConst
   plan_comp_builder_.reset();
   for (MotionResponseCont::size_type i = 0; i < resp_cont.size(); ++i)
   {
-    plan_comp_builder_.append(resp_cont.at(i).trajectory_,
+    plan_comp_builder_.append(planning_scene, resp_cont.at(i).trajectory_,
                               // The blend radii has to be "attached" to
                               // the second part of a blend trajectory,
                               // therefore: "i-1".
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
index 175a7deca9..fe078b2ef4 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/plan_components_builder.cpp
@@ -68,7 +68,8 @@ void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::Robot
   }
 }
 
-void PlanComponentsBuilder::blend(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius)
+void PlanComponentsBuilder::blend(const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                  const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius)
 {
   if (!blender_)
   {
@@ -86,7 +87,7 @@ void PlanComponentsBuilder::blend(const robot_trajectory::RobotTrajectoryPtr& ot
   blend_request.link_name = getSolverTipFrame(model_->getJointModelGroup(blend_request.group_name));
 
   pilz_industrial_motion_planner::TrajectoryBlendResponse blend_response;
-  if (!blender_->blend(blend_request, blend_response))
+  if (!blender_->blend(planning_scene, blend_request, blend_response))
   {
     throw BlendingFailedException("Blending failed");
   }
@@ -98,7 +99,8 @@ void PlanComponentsBuilder::blend(const robot_trajectory::RobotTrajectoryPtr& ot
   traj_tail_ = blend_response.second_trajectory;  // first for next blending segment
 }
 
-void PlanComponentsBuilder::append(const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius)
+void PlanComponentsBuilder::append(const planning_scene::PlanningSceneConstPtr& planning_scene,
+                                   const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius)
 {
   if (!model_)
   {
@@ -131,7 +133,7 @@ void PlanComponentsBuilder::append(const robot_trajectory::RobotTrajectoryPtr& o
     return;
   }
 
-  blend(other, blend_radius);
+  blend(planning_scene, other, blend_radius);
 }
 
 }  // namespace pilz_industrial_motion_planner
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp
index 301bcc23e0..e6deb8c28f 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp
@@ -42,8 +42,10 @@
 #else
 #include <tf2_eigen/tf2_eigen.h>
 #endif
+#include <moveit/planning_interface/planning_interface.h>
 
 bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
+    const planning_scene::PlanningSceneConstPtr& planning_scene,
     const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
     pilz_industrial_motion_planner::TrajectoryBlendResponse& res)
 {
@@ -90,10 +92,10 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
   }
   trajectory_msgs::JointTrajectory blend_joint_trajectory;
   moveit_msgs::MoveItErrorCodes error_code;
-  if (!generateJointTrajectory(req.first_trajectory->getFirstWayPointPtr()->getRobotModel(),
-                               limits_.getJointLimitContainer(), blend_trajectory_cartesian, req.group_name,
-                               req.link_name, initial_joint_position, initial_joint_velocity, blend_joint_trajectory,
-                               error_code, true))
+
+  if (!generateJointTrajectory(planning_scene, limits_.getJointLimitContainer(), blend_trajectory_cartesian,
+                               req.group_name, req.link_name, initial_joint_position, initial_joint_velocity,
+                               blend_joint_trajectory, error_code, true))
   {
     // LCOV_EXCL_START
     ROS_INFO("Failed to generate joint trajectory for blending trajectory.");
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
index 466cc70fdc..6d3516066f 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp
@@ -52,13 +52,14 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #endif
 
-bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model,
+bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::PlanningSceneConstPtr& scene,
                                                    const std::string& group_name, const std::string& link_name,
                                                    const Eigen::Isometry3d& pose, const std::string& frame_id,
                                                    const std::map<std::string, double>& seed,
                                                    std::map<std::string, double>& solution, bool check_self_collision,
                                                    const double timeout)
 {
+  const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
   if (!robot_model->hasJointModelGroup(group_name))
   {
     ROS_ERROR_STREAM("Robot model has no planning group named as " << group_name);
@@ -86,7 +87,7 @@ bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotMode
 
   moveit::core::GroupStateValidityCallbackFn ik_constraint_function;
   ik_constraint_function =
-      boost::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, robot_model, _1, _2, _3);
+      boost::bind(&pilz_industrial_motion_planner::isStateColliding, check_self_collision, scene, _1, _2, _3);
 
   // call ik
   if (rstate.setFromIK(robot_model->getJointModelGroup(group_name), pose, link_name, timeout, ik_constraint_function))
@@ -105,7 +106,7 @@ bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotMode
   }
 }
 
-bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotModelConstPtr& robot_model,
+bool pilz_industrial_motion_planner::computePoseIK(const planning_scene::PlanningSceneConstPtr& scene,
                                                    const std::string& group_name, const std::string& link_name,
                                                    const geometry_msgs::Pose& pose, const std::string& frame_id,
                                                    const std::map<std::string, double>& seed,
@@ -114,7 +115,7 @@ bool pilz_industrial_motion_planner::computePoseIK(const moveit::core::RobotMode
 {
   Eigen::Isometry3d pose_eigen;
   tf2::fromMsg(pose, pose_eigen);
-  return computePoseIK(robot_model, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision,
+  return computePoseIK(scene, group_name, link_name, pose_eigen, frame_id, seed, solution, check_self_collision,
                        timeout);
 }
 
@@ -203,7 +204,7 @@ bool pilz_industrial_motion_planner::verifySampleJointLimits(
 }
 
 bool pilz_industrial_motion_planner::generateJointTrajectory(
-    const moveit::core::RobotModelConstPtr& robot_model,
+    const planning_scene::PlanningSceneConstPtr& scene,
     const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory,
     const std::string& group_name, const std::string& link_name,
     const std::map<std::string, double>& initial_joint_position, const double& sampling_time,
@@ -212,6 +213,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
 {
   ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory.");
 
+  const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
   ros::Time generation_begin = ros::Time::now();
 
   // generate the time samples
@@ -237,7 +239,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
   {
     tf2::fromMsg(tf2::toMsg(trajectory.Pos(*time_iter)), pose_sample);
 
-    if (!computePoseIK(robot_model, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last,
+    if (!computePoseIK(scene, group_name, link_name, pose_sample, robot_model->getModelFrame(), ik_solution_last,
                        ik_solution, check_self_collision))
     {
       ROS_ERROR("Failed to compute inverse kinematics solution for sampled "
@@ -318,7 +320,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
 }
 
 bool pilz_industrial_motion_planner::generateJointTrajectory(
-    const moveit::core::RobotModelConstPtr& robot_model,
+    const planning_scene::PlanningSceneConstPtr& scene,
     const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits,
     const pilz_industrial_motion_planner::CartesianTrajectory& trajectory, const std::string& group_name,
     const std::string& link_name, const std::map<std::string, double>& initial_joint_position,
@@ -327,6 +329,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
 {
   ROS_DEBUG("Generate joint trajectory from a Cartesian trajectory.");
 
+  const moveit::core::RobotModelConstPtr& robot_model = scene->getRobotModel();
   ros::Time generation_begin = ros::Time::now();
 
   std::map<std::string, double> ik_solution_last = initial_joint_position;
@@ -342,7 +345,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory(
   for (size_t i = 0; i < trajectory.points.size(); ++i)
   {
     // compute inverse kinematics
-    if (!computePoseIK(robot_model, group_name, link_name, trajectory.points.at(i).pose, robot_model->getModelFrame(),
+    if (!computePoseIK(scene, group_name, link_name, trajectory.points.at(i).pose, robot_model->getModelFrame(),
                        ik_solution_last, ik_solution, check_self_collision))
     {
       ROS_ERROR("Failed to compute inverse kinematics solution for sampled "
@@ -568,7 +571,7 @@ bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_
 }
 
 bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_collision,
-                                                      const moveit::core::RobotModelConstPtr& robot_model,
+                                                      const planning_scene::PlanningSceneConstPtr& scene,
                                                       robot_state::RobotState* rstate,
                                                       const robot_state::JointModelGroup* const group,
                                                       const double* const ik_solution)
@@ -582,9 +585,9 @@ bool pilz_industrial_motion_planner::isStateColliding(const bool test_for_self_c
   rstate->update();
   collision_detection::CollisionRequest collision_req;
   collision_req.group_name = group->getName();
+  collision_req.verbose = true;
   collision_detection::CollisionResult collision_res;
-  planning_scene::PlanningScene(robot_model).checkSelfCollision(collision_req, collision_res, *rstate);
-
+  scene->checkSelfCollision(collision_req, collision_res, *rstate);
   return !collision_res.collision;
 }
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
index 2084f11e4c..aebdfaf90d 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp
@@ -214,23 +214,13 @@ void TrajectoryGenerator::validateRequest(const planning_interface::MotionPlanRe
   checkGoalConstraints(req.goal_constraints, req.start_state.joint_state.name, req.group_name);
 }
 
-void TrajectoryGenerator::convertToRobotTrajectory(const trajectory_msgs::JointTrajectory& joint_trajectory,
-                                                   const moveit_msgs::RobotState& start_state,
-                                                   robot_trajectory::RobotTrajectory& robot_trajectory) const
-{
-  moveit::core::RobotState start_rs(robot_model_);
-  start_rs.setToDefaultValues();
-  moveit::core::robotStateMsgToRobotState(start_state, start_rs, false);
-  robot_trajectory.setRobotTrajectoryMsg(start_rs, joint_trajectory);
-}
-
-void TrajectoryGenerator::setSuccessResponse(const std::string& group_name, const moveit_msgs::RobotState& start_state,
+void TrajectoryGenerator::setSuccessResponse(const moveit::core::RobotState& start_state, const std::string& group_name,
                                              const trajectory_msgs::JointTrajectory& joint_trajectory,
                                              const ros::Time& planning_start,
                                              planning_interface::MotionPlanResponse& res) const
 {
   robot_trajectory::RobotTrajectoryPtr rt(new robot_trajectory::RobotTrajectory(robot_model_, group_name));
-  convertToRobotTrajectory(joint_trajectory, start_state, *rt);
+  rt->setRobotTrajectoryMsg(start_state, joint_trajectory);
 
   res.trajectory_ = rt;
   res.error_code_.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
@@ -267,7 +257,8 @@ TrajectoryGenerator::cartesianTrapVelocityProfile(const double& max_velocity_sca
   return vp_trans;
 }
 
-bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest& req,
+bool TrajectoryGenerator::generate(const planning_scene::PlanningSceneConstPtr& scene,
+                                   const planning_interface::MotionPlanRequest& req,
                                    planning_interface::MotionPlanResponse& res, double sampling_time)
 {
   ROS_INFO_STREAM("Generating " << req.planner_id << " trajectory...");
@@ -300,7 +291,7 @@ bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest&
   MotionPlanInfo plan_info;
   try
   {
-    extractMotionPlanInfo(req, plan_info);
+    extractMotionPlanInfo(scene, req, plan_info);
   }
   catch (const MoveItErrorCodeException& ex)
   {
@@ -313,7 +304,7 @@ bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest&
   trajectory_msgs::JointTrajectory joint_trajectory;
   try
   {
-    plan(req, plan_info, sampling_time, joint_trajectory);
+    plan(scene, req, plan_info, sampling_time, joint_trajectory);
   }
   catch (const MoveItErrorCodeException& ex)
   {
@@ -323,7 +314,9 @@ bool TrajectoryGenerator::generate(const planning_interface::MotionPlanRequest&
     return false;
   }
 
-  setSuccessResponse(req.group_name, req.start_state, joint_trajectory, planning_begin, res);
+  moveit::core::RobotState start_state(scene->getCurrentState());
+  moveit::core::robotStateMsgToRobotState(req.start_state, start_state, true);
+  setSuccessResponse(start_state, req.group_name, joint_trajectory, planning_begin, res);
   return true;
 }
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp
index feeb891e75..e35fc08cd1 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp
@@ -96,7 +96,8 @@ void TrajectoryGeneratorCIRC::cmdSpecificRequestValidation(const planning_interf
   }
 }
 
-void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req,
+void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                                                    const planning_interface::MotionPlanRequest& req,
                                                     TrajectoryGenerator::MotionPlanInfo& info) const
 {
   ROS_DEBUG("Extract necessary information from motion plan request.");
@@ -179,7 +180,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::Mo
 
   // check goal pose ik before Cartesian motion plan starts
   std::map<std::string, double> ik_solution;
-  if (!computePoseIK(robot_model_, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
+  if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
                      ik_solution))
   {
     // LCOV_EXCL_START
@@ -198,7 +199,8 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_interface::Mo
   info.circ_path_point.second = circ_path_point;
 }
 
-void TrajectoryGeneratorCIRC::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
+void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene,
+                                   const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                    const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory)
 {
   std::unique_ptr<KDL::Path> cart_path(setPathCIRC(plan_info));
@@ -214,9 +216,9 @@ void TrajectoryGeneratorCIRC::plan(const planning_interface::MotionPlanRequest&
   moveit_msgs::MoveItErrorCodes error_code;
   // sample the Cartesian trajectory and compute joint trajectory using inverse
   // kinematics
-  if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_trajectory,
-                               plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, sampling_time,
-                               joint_trajectory, error_code))
+  if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
+                               plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
+                               error_code))
   {
     throw CircTrajectoryConversionFailure("Failed to generate valid joint trajectory from the Cartesian path",
                                           error_code.val);
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
index d158c0d0e7..cbf949c9fd 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
@@ -74,7 +74,8 @@ TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelCon
   }
 }
 
-void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req,
+void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                                                   const planning_interface::MotionPlanRequest& req,
                                                    TrajectoryGenerator::MotionPlanInfo& info) const
 {
   ROS_DEBUG("Extract necessary information from motion plan request.");
@@ -156,7 +157,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::Mot
 
   // check goal pose ik before Cartesian motion plan starts
   std::map<std::string, double> ik_solution;
-  if (!computePoseIK(robot_model_, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
+  if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
                      ik_solution))
   {
     std::ostringstream os;
@@ -165,7 +166,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_interface::Mot
   }
 }
 
-void TrajectoryGeneratorLIN::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
+void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene,
+                                  const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                   const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory)
 {
   // create Cartesian path for lin
@@ -184,9 +186,9 @@ void TrajectoryGeneratorLIN::plan(const planning_interface::MotionPlanRequest& r
   moveit_msgs::MoveItErrorCodes error_code;
   // sample the Cartesian trajectory and compute joint trajectory using inverse
   // kinematics
-  if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_trajectory,
-                               plan_info.group_name, plan_info.link_name, plan_info.start_joint_position, sampling_time,
-                               joint_trajectory, error_code))
+  if (!generateJointTrajectory(scene, planner_limits_.getJointLimitContainer(), cart_trajectory, plan_info.group_name,
+                               plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
+                               error_code))
   {
     std::ostringstream os;
     os << "Failed to generate valid joint trajectory from the Cartesian path";
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp
index 53b5a9000f..905091b7a5 100644
--- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp
@@ -220,7 +220,8 @@ void TrajectoryGeneratorPTP::planPTP(const std::map<std::string, double>& start_
             0.0);
 }
 
-void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_interface::MotionPlanRequest& req,
+void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
+                                                   const planning_interface::MotionPlanRequest& req,
                                                    MotionPlanInfo& info) const
 {
   info.group_name = req.group_name;
@@ -256,7 +257,7 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_interface::Mot
     Eigen::Isometry3d pose_eigen;
     normalizeQuaternion(pose.orientation);
     tf2::fromMsg(pose, pose_eigen);
-    if (!computePoseIK(robot_model_, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
+    if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
                        pose_eigen, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
     {
       throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose");
@@ -264,7 +265,8 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_interface::Mot
   }
 }
 
-void TrajectoryGeneratorPTP::plan(const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
+void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& scene,
+                                  const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                   const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory)
 {
   // plan the ptp trajectory
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp
index 43f0415771..fb86d128cb 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/integrationtest_plan_components_builder.cpp
@@ -60,6 +60,7 @@ class IntegrationTestPlanComponentBuilder : public testing::Test
 protected:
   ros::NodeHandle ph_{ "~" };
   robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION_STR).getModel() };
+  planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) };
 
   std::string planning_group_;
 };
@@ -103,7 +104,7 @@ TEST_F(IntegrationTestPlanComponentBuilder, TestModelSet)
   robot_trajectory::RobotTrajectoryPtr traj{ new robot_trajectory::RobotTrajectory(robot_model_, planning_group_) };
   PlanComponentsBuilder builder;
 
-  EXPECT_THROW(builder.append(traj, 1.0), NoRobotModelSetException);
+  EXPECT_THROW(builder.append(planning_scene_, traj, 1.0), NoRobotModelSetException);
 }
 
 /**
@@ -116,9 +117,9 @@ TEST_F(IntegrationTestPlanComponentBuilder, TestNoBlenderSet)
   PlanComponentsBuilder builder;
   builder.setModel(robot_model_);
 
-  builder.append(traj, 0.0);
+  builder.append(planning_scene_, traj, 0.0);
 
-  EXPECT_THROW(builder.append(traj, 1.0), NoBlenderSetException);
+  EXPECT_THROW(builder.append(planning_scene_, traj, 1.0), NoBlenderSetException);
 }
 
 int main(int argc, char** argv)
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp
index cb54ed4aa8..747c13bdf0 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp
@@ -1042,12 +1042,14 @@ bool testutils::getBlendTestData(const ros::NodeHandle& nh, const size_t& datase
 }
 
 bool testutils::generateTrajFromBlendTestData(
-    const robot_model::RobotModelConstPtr& robot_model,
+    const planning_scene::PlanningSceneConstPtr& scene,
     const std::shared_ptr<pilz_industrial_motion_planner::TrajectoryGenerator>& tg, const std::string& group_name,
     const std::string& link_name, const testutils::BlendTestData& data, const double& sampling_time_1,
     const double& sampling_time_2, planning_interface::MotionPlanResponse& res_1,
     planning_interface::MotionPlanResponse& res_2, double& dis_1, double& dis_2)
 {
+  const robot_model::RobotModelConstPtr robot_model = scene->getRobotModel();
+
   // generate first trajectory
   planning_interface::MotionPlanRequest req_1;
   req_1.group_name = group_name;
@@ -1066,7 +1068,7 @@ bool testutils::generateTrajFromBlendTestData(
       goal_state_1, goal_state_1.getRobotModel()->getJointModelGroup(group_name)));
 
   // trajectory generation
-  if (!tg->generate(req_1, res_1, sampling_time_1))
+  if (!tg->generate(scene, req_1, res_1, sampling_time_1))
   {
     std::cout << "Failed to generate first trajectory." << std::endl;
     return false;
@@ -1086,7 +1088,7 @@ bool testutils::generateTrajFromBlendTestData(
   req_2.goal_constraints.push_back(kinematic_constraints::constructGoalConstraints(
       goal_state_2, goal_state_2.getRobotModel()->getJointModelGroup(group_name)));
   // trajectory generation
-  if (!tg->generate(req_2, res_2, sampling_time_2))
+  if (!tg->generate(scene, req_2, res_2, sampling_time_2))
   {
     std::cout << "Failed to generate second trajectory." << std::endl;
     return false;
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h
index fd1ebfbe5e..bd0fae7a47 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h
+++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h
@@ -434,7 +434,7 @@ bool checkBlendResult(const pilz_industrial_motion_planner::TrajectoryBlendReque
  * @param[out] dis_lin_2: translational distance of the second LIN
  * @return true if succeed
  */
-bool generateTrajFromBlendTestData(const moveit::core::RobotModelConstPtr& robot_model,
+bool generateTrajFromBlendTestData(const planning_scene::PlanningSceneConstPtr& scene,
                                    const std::shared_ptr<pilz_industrial_motion_planner::TrajectoryGenerator>& tg,
                                    const std::string& group_name, const std::string& link_name,
                                    const BlendTestData& data, const double& sampling_time_1,
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp
index 3804460f6d..85a5987840 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp
@@ -98,6 +98,7 @@ class TrajectoryBlenderTransitionWindowTest : public testing::TestWithParam<std:
   // ros stuff
   ros::NodeHandle ph_{ "~" };
   robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+  planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) };
 
   std::unique_ptr<TrajectoryGenerator> lin_generator_;
   std::unique_ptr<TrajectoryBlenderTransitionWindow> blender_;
@@ -166,7 +167,7 @@ TrajectoryBlenderTransitionWindowTest::generateLinTrajs(const Sequence& seq, siz
     }
     // generate trajectory
     planning_interface::MotionPlanResponse resp;
-    if (!lin_generator_->generate(req, resp, sampling_time_))
+    if (!lin_generator_->generate(planning_scene_, req, resp, sampling_time_))
     {
       std::runtime_error("Failed to generate trajectory.");
     }
@@ -205,7 +206,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName)
   blend_req.second_trajectory = res.at(1).trajectory_;
 
   blend_req.blend_radius = seq.getBlendRadius(0);
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -234,7 +235,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink)
   blend_req.second_trajectory = res.at(1).trajectory_;
 
   blend_req.blend_radius = seq.getBlendRadius(0);
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -264,7 +265,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius)
   blend_req.second_trajectory = res.at(1).trajectory_;
 
   blend_req.blend_radius = -0.1;
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -293,7 +294,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius)
   blend_req.second_trajectory = res.at(1).trajectory_;
 
   blend_req.blend_radius = 0.;
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -328,7 +329,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes)
     }
     // generate trajectory
     planning_interface::MotionPlanResponse resp;
-    if (!lin_generator_->generate(req, resp, sampling_time_))
+    if (!lin_generator_->generate(planning_scene_, req, resp, sampling_time_))
     {
       std::runtime_error("Failed to generate trajectory.");
     }
@@ -343,7 +344,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes)
   blend_req.first_trajectory = responses[0].trajectory_;
   blend_req.second_trajectory = responses[1].trajectory_;
   blend_req.blend_radius = seq.getBlendRadius(0);
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -378,7 +379,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime)
   blend_req.first_trajectory = res.at(0).trajectory_;
   blend_req.second_trajectory = res.at(1).trajectory_;
   blend_req.blend_radius = seq.getBlendRadius(0);
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -410,7 +411,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories)
   // intersect
   blend_req.second_trajectory = res.at(0).trajectory_;
   blend_req.blend_radius = seq.getBlendRadius(0);
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -446,7 +447,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint)
   blend_req.first_trajectory->getLastWayPointPtr()->setVariableVelocity(0, 1.0);
   blend_req.second_trajectory->getFirstWayPointPtr()->setVariableVelocity(0, 1.0);
 
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -483,7 +484,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius)
   blend_req.first_trajectory = res.at(0).trajectory_;
   blend_req.second_trajectory = res.at(1).trajectory_;
 
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -515,7 +516,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius)
   blend_req.first_trajectory = res.at(0).trajectory_;
   blend_req.second_trajectory = res.at(1).trajectory_;
 
-  EXPECT_FALSE(blender_->blend(blend_req, blend_res));
+  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
 }
 
 /**
@@ -552,7 +553,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending)
   blend_req.first_trajectory = res.at(0).trajectory_;
   blend_req.second_trajectory = res.at(1).trajectory_;
 
-  EXPECT_TRUE(blender_->blend(blend_req, blend_res));
+  EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res));
 
   EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_,
                                           joint_acceleration_tolerance_, cartesian_velocity_tolerance_,
@@ -595,7 +596,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories)
   blend_req.first_trajectory = res.at(0).trajectory_;
   blend_req.second_trajectory = res.at(1).trajectory_;
   blend_req.blend_radius = seq.getBlendRadius(0);
-  EXPECT_TRUE(blender_->blend(blend_req, blend_res));
+  EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res));
 
   EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_,
                                           joint_acceleration_tolerance_, cartesian_velocity_tolerance_,
@@ -690,7 +691,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
     }
 
     moveit_msgs::MoveItErrorCodes error_code;
-    if (!generateJointTrajectory(robot_model_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_,
+    if (!generateJointTrajectory(planning_scene_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_,
                                  target_link_, initial_joint_position, initial_joint_velocity, joint_traj, error_code,
                                  true))
     {
@@ -716,7 +717,7 @@ TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
   blend_req.first_trajectory = sine_trajs.at(0);
   blend_req.second_trajectory = sine_trajs.at(1);
 
-  EXPECT_TRUE(blender_->blend(blend_req, blend_res));
+  EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res));
 
   EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_,
                                           joint_acceleration_tolerance_, cartesian_velocity_tolerance_,
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp
index 9678580556..b82e9472ae 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_functions.cpp
@@ -113,6 +113,7 @@ class TrajectoryFunctionsTestBase : public testing::TestWithParam<std::string>
   // ros stuff
   ros::NodeHandle ph_{ "~" };
   robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+  planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) };
 
   // test parameters from parameter server
   std::string planning_group_, group_tip_link_, tcp_link_, ik_fast_link_;
@@ -359,7 +360,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIK)
 
     // compute the ik
     std::map<std::string, double> ik_actual;
-    EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
+    EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                               frame_id, ik_seed, ik_actual, false));
 
     // compare ik solution and expected value
@@ -384,8 +385,8 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidGroupNam
 
   // compute the ik
   std::map<std::string, double> ik_actual;
-  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, "InvalidGroupName", tcp_link_, pose_expect,
-                                                             frame_id, ik_seed, ik_actual, false));
+  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, "InvalidGroupName", tcp_link_,
+                                                             pose_expect, frame_id, ik_seed, ik_actual, false));
 }
 
 /**
@@ -400,7 +401,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidLinkName
 
   // compute the ik
   std::map<std::string, double> ik_actual;
-  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, "WrongLink", pose_expect,
+  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, "WrongLink", pose_expect,
                                                              frame_id, ik_seed, ik_actual, false));
 }
 
@@ -417,7 +418,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKInvalidFrameId)
 
   // compute the ik
   std::map<std::string, double> ik_actual;
-  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
+  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                              "InvalidFrameId", ik_seed, ik_actual, false));
 }
 
@@ -455,7 +456,7 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali
   // compute the ik without self collision check and expect the resulting pose
   // to be in self collission.
   std::map<std::string, double> ik_actual1;
-  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
+  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                             frame_id, ik_seed, ik_actual1, false));
 
   robot_state::RobotState rstate(robot_model_);
@@ -479,7 +480,7 @@ TEST_P(TrajectoryFunctionsTestOnlyGripper, testComputePoseIKSelfCollisionForVali
   // compute the ik with collision detection activated and expect the resulting
   // pose to be without self collision.
   std::map<std::string, double> ik_actual2;
-  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
+  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                             frame_id, ik_seed, ik_actual2, true));
 
   std::vector<double> ik_state2;
@@ -522,11 +523,11 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testComputePoseIKSelfCollisionFo
 
   // compute the ik with disabled collision check
   std::map<std::string, double> ik_actual;
-  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
+  EXPECT_TRUE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                             frame_id, ik_seed, ik_actual, false));
 
   // compute the ik with enabled collision check
-  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(robot_model_, planning_group_, tcp_link_, pose_expect,
+  EXPECT_FALSE(pilz_industrial_motion_planner::computePoseIK(planning_scene_, planning_group_, tcp_link_, pose_expect,
                                                              frame_id, ik_seed, ik_actual, true));
 }
 
@@ -707,7 +708,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI
   bool check_self_collision{ false };
 
   EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory(
-      robot_model_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time,
+      planning_scene_, joint_limits, kdl_trajectory, group_name, tcp_link_, initial_joint_position, sampling_time,
       joint_trajectory, error_code, check_self_collision));
 
   std::map<std::string, double> initial_joint_velocity;
@@ -719,7 +720,7 @@ TEST_P(TrajectoryFunctionsTestFlangeAndGripper, testGenerateJointTrajectoryWithI
   cart_traj.points.push_back(cart_traj_point);
 
   EXPECT_FALSE(pilz_industrial_motion_planner::generateJointTrajectory(
-      robot_model_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity,
+      planning_scene_, joint_limits, cart_traj, group_name, tcp_link_, initial_joint_position, initial_joint_velocity,
       joint_trajectory, error_code, check_self_collision));
 }
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp
index 2b7868728a..732feb116a 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_circ.cpp
@@ -99,6 +99,7 @@ class TrajectoryGeneratorCIRCTest : public testing::TestWithParam<std::string>
   // ros stuff
   ros::NodeHandle ph_{ "~" };
   robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+  planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) };
   std::unique_ptr<TrajectoryGeneratorCIRC> circ_;
   // test data provider
   std::unique_ptr<pilz_industrial_motion_planner_testutils::TestdataLoader> tdp_;
@@ -308,7 +309,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, incompleteStartState)
   req.start_state.joint_state.position.resize(1);  // prevent failing check for equal sizes
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(req, res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
 }
 
@@ -322,7 +323,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, nonZeroStartVelocity)
   // start state has non-zero velocity
   req.start_state.joint_state.velocity.push_back(1.0);
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(req, res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
   req.start_state.joint_state.velocity.clear();
 }
@@ -332,7 +333,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, ValidCommand)
   auto circ{ tdp_->getCircCartCenterCart("circ1_center_2") };
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_TRUE(circ_->generate(circ.toRequest(), res));
+  EXPECT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
 }
 
@@ -345,7 +346,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, velScaleToHigh)
 
   circ.setVelocityScale(1.0);
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(circ.toRequest(), res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::PLANNING_FAILED);
 }
 
@@ -358,7 +359,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, accScaleToHigh)
 
   circ.setAccelerationScale(1.0);
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(circ.toRequest(), res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::PLANNING_FAILED);
 }
 
@@ -380,7 +381,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithCenter)
   circ.getGoalConfiguration().getPose().position.z -= 1e-8;
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(circ.toRequest(), res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -403,7 +404,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, samePointsWithInterim)
   circ.getGoalConfiguration().getPose().position.z -= 1e-8;
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(circ.toRequest(), res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -419,7 +420,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, emptyAux)
   req.path_constraints.position_constraints.clear();
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(req, res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -435,7 +436,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxName)
   req.path_constraints.name = "";
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(req, res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -452,7 +453,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidAuxLinkName)
   req.path_constraints.position_constraints.front().link_name = "INVALID";
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(req, res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME);
 }
 
@@ -466,7 +467,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, invalidCenter)
   circ.getAuxiliaryConfiguration().getConfiguration().getPose().position.y += 1;
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(circ.toRequest(), res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -486,7 +487,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenter)
   circ.getGoalConfiguration().getPose().position.x += 0.1;
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(circ.toRequest(), res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -507,7 +508,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearInterim)
   circ.getGoalConfiguration().getPose().position.x += 0.1;
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(circ.toRequest(), res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -525,7 +526,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterDueToInterim)
   auto circ{ tdp_->getCircCartInterimCart("circ3_interim") };
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(circ.toRequest(), res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, circ.toRequest(), res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
 }
 
@@ -558,7 +559,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, colinearCenterAndInterim)
   moveit_msgs::MotionPlanRequest req = circ.toRequest();
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_TRUE(circ_->generate(req, res));
+  EXPECT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -592,7 +593,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, interimLarger180Degree)
   moveit_msgs::MotionPlanRequest req = circ.toRequest();
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_TRUE(circ_->generate(req, res));
+  EXPECT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -608,7 +609,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, centerPointJointGoal)
   moveit_msgs::MotionPlanRequest req = circ.toRequest();
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(req, res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -635,7 +636,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidAdditionalPrimitivePose)
   req.path_constraints.position_constraints.back().constraint_region.primitive_poses.push_back(center_position);
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_FALSE(circ_->generate(req, res));
+  ASSERT_FALSE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -657,7 +658,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InvalidExtraJointConstraint)
   req.goal_constraints.front().joint_constraints.push_back(joint_constraint);  //<-- Additional constraint
 
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(circ_->generate(req, res));
+  EXPECT_FALSE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
@@ -671,7 +672,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoal)
   moveit_msgs::MotionPlanRequest req = circ.toRequest();
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(req, res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -688,7 +689,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdPositionConstraint
   req.goal_constraints.front().position_constraints.front().header.frame_id = robot_model_->getModelFrame();
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(req, res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -704,7 +705,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdOrientationConstra
   req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame();
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(req, res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -723,7 +724,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, CenterPointPoseGoalFrameIdBothConstraints)
   req.goal_constraints.front().orientation_constraints.front().header.frame_id = robot_model_->getModelFrame();
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(req, res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -740,7 +741,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoal)
   moveit_msgs::MotionPlanRequest req = circ.toRequest();
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(req, res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -763,7 +764,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointJointGoalStartVelNearZero)
   req.start_state.joint_state.velocity = std::vector<double>(req.start_state.joint_state.position.size(), 1e-16);
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(req, res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
@@ -777,7 +778,7 @@ TEST_P(TrajectoryGeneratorCIRCTest, InterimPointPoseGoal)
   moveit_msgs::MotionPlanRequest req = circ.toRequest();
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(circ_->generate(req, res));
+  ASSERT_TRUE(circ_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
   checkCircResult(req, res);
 }
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp
index aaba86c0f1..7a50c654f6 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_common.cpp
@@ -147,6 +147,7 @@ class TrajectoryGeneratorCommonTest : public ::testing::Test
     robot_model_loader::RobotModelLoader(!T::VALUE ? PARAM_MODEL_NO_GRIPPER_NAME : PARAM_MODEL_WITH_GRIPPER_NAME)
         .getModel()
   };
+  planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) };
 
   // trajectory generator
   std::unique_ptr<pilz_industrial_motion_planner::TrajectoryGenerator> trajectory_generator_;
@@ -177,22 +178,22 @@ TYPED_TEST_SUITE(TrajectoryGeneratorCommonTestWithGripper, TrajectoryGeneratorCo
 TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor)
 {
   this->req_.max_velocity_scaling_factor = 2.0;
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 
   this->req_.max_velocity_scaling_factor = 1.0;
   this->req_.max_acceleration_scaling_factor = 0;
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 
   this->req_.max_velocity_scaling_factor = 0.00001;
   this->req_.max_acceleration_scaling_factor = 1;
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 
   this->req_.max_velocity_scaling_factor = 1;
   this->req_.max_acceleration_scaling_factor = -1;
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN);
 }
 
@@ -202,7 +203,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideScalingFactor)
 TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName)
 {
   this->req_.group_name = "foot";
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val);
 }
 
@@ -212,7 +213,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidGroupName)
 TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup)
 {
   this->req_.group_name = "gripper";
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME, this->res_.error_code_.val);
 }
 
@@ -222,7 +223,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestNoGripper, GripperGroup)
 TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup)
 {
   this->req_.group_name = "gripper";
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS, this->res_.error_code_.val);
 }
 
@@ -247,7 +248,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTestWithGripper, GripperGroup)
 TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState)
 {
   this->req_.start_state.joint_state.name.clear();
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
 }
 
@@ -257,7 +258,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyJointNamesInStartState)
 TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState)
 {
   this->req_.start_state.joint_state.name.push_back("joint_7");
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
 }
 
@@ -267,7 +268,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InconsistentStartState)
 TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit)
 {
   this->req_.start_state.joint_state.position[0] = 100;
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
 }
 
@@ -281,7 +282,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPostionOutOfLimit)
 TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero)
 {
   this->req_.start_state.joint_state.velocity[0] = 100;
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
 }
 
@@ -291,7 +292,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, StartPositionVelocityNoneZero)
 TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyGoalConstraints)
 {
   this->req_.goal_constraints.clear();
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
@@ -308,7 +309,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals)
   // two goal constraints
   this->req_.goal_constraints.push_back(goal_constraint);
   this->req_.goal_constraints.push_back(goal_constraint);
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 
   // one joint constraint and one orientation constraint
@@ -316,7 +317,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals)
   goal_constraint.orientation_constraints.push_back(orientation_constraint);
   this->req_.goal_constraints.clear();
   this->req_.goal_constraints.push_back(goal_constraint);
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 
   // one joint constraint and one Cartesian constraint
@@ -324,7 +325,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals)
   goal_constraint.orientation_constraints.push_back(orientation_constraint);
   this->req_.goal_constraints.clear();
   this->req_.goal_constraints.push_back(goal_constraint);
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 
   // two Cartesian constraints
@@ -335,7 +336,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MultipleGoals)
   goal_constraint.orientation_constraints.push_back(orientation_constraint);
   this->req_.goal_constraints.clear();
   this->req_.goal_constraints.push_back(goal_constraint);
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
@@ -347,7 +348,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointNameInGoal)
   moveit_msgs::JointConstraint joint_constraint;
   joint_constraint.joint_name = "test_joint_2";
   this->req_.goal_constraints.front().joint_constraints[0] = joint_constraint;
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
@@ -359,7 +360,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint)
   moveit_msgs::JointConstraint joint_constraint;
   joint_constraint.joint_name = "test_joint_2";
   this->req_.goal_constraints.front().joint_constraints.pop_back();  //<-- Missing joint constraint
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
@@ -369,7 +370,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, MissingJointConstraint)
 TYPED_TEST(TrajectoryGeneratorCommonTest, InvalideJointPositionInGoal)
 {
   this->req_.goal_constraints.front().joint_constraints[0].position = 100;
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
@@ -386,7 +387,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal)
   goal_constraint.orientation_constraints.push_back(orientation_constraint);
   this->req_.goal_constraints.clear();
   this->req_.goal_constraints.push_back(goal_constraint);
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 
   // different link names in position and orientation goals
@@ -394,14 +395,14 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, InvalidLinkNameInCartesianGoal)
   goal_constraint.orientation_constraints.front().link_name = "test_link_2";
   this->req_.goal_constraints.clear();
   this->req_.goal_constraints.push_back(goal_constraint);
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 
   // no solver for the link
   goal_constraint.orientation_constraints.front().link_name = "test_link_1";
   this->req_.goal_constraints.clear();
   this->req_.goal_constraints.push_back(goal_constraint);
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION);
 }
 
@@ -421,7 +422,7 @@ TYPED_TEST(TrajectoryGeneratorCommonTest, EmptyPrimitivePoses)
   goal_constraint.orientation_constraints.push_back(orientation_constraint);
   this->req_.goal_constraints.clear();
   this->req_.goal_constraints.push_back(goal_constraint);
-  EXPECT_FALSE(this->trajectory_generator_->generate(this->req_, this->res_));
+  EXPECT_FALSE(this->trajectory_generator_->generate(this->planning_scene_, this->req_, this->res_));
   EXPECT_EQ(this->res_.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp
index cfdaffee0c..4b8edf69a6 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_lin.cpp
@@ -91,6 +91,7 @@ class TrajectoryGeneratorLINTest : public testing::TestWithParam<std::string>
   // ros stuff
   ros::NodeHandle ph_{ "~" };
   robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+  planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) };
 
   // lin trajectory generator using model without gripper
   std::unique_ptr<TrajectoryGenerator> lin_;
@@ -213,7 +214,7 @@ TEST_P(TrajectoryGeneratorLINTest, nonZeroStartVelocity)
 
   // try to generate the result
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(lin_->generate(req, res));
+  EXPECT_FALSE(lin_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
 }
 
@@ -226,7 +227,7 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoal)
 
   // generate the LIN trajectory
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(lin_->generate(lin_joint_req, res));
+  ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res));
   EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   // check the resulted trajectory
@@ -247,7 +248,7 @@ TEST_P(TrajectoryGeneratorLINTest, jointSpaceGoalNearZeroStartVelocity)
 
   // generate the LIN trajectory
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(lin_->generate(lin_joint_req, res));
+  ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res));
   EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   // check the resulted trajectory
@@ -264,7 +265,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartesianSpaceGoal)
 
   // generate lin trajectory
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(lin_->generate(lin_cart_req, res));
+  ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res));
   EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   // check the resulted trajectory
@@ -288,7 +289,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartesianTrapezoidProfile)
   /// + plan LIN trajectory +
   /// +++++++++++++++++++++++
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(lin_->generate(lin_joint_req, res, 0.01));
+  ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res, 0.01));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   ASSERT_TRUE(testutils::checkCartesianTranslationalPath(res.trajectory_, target_link_hcd_));
@@ -320,7 +321,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinPlannerLimitViolation)
   lin.setAccelerationScale(1.01);
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_FALSE(lin_->generate(lin.toRequest(), res));
+  ASSERT_FALSE(lin_->generate(planning_scene_, lin.toRequest(), res));
 }
 
 /**
@@ -346,7 +347,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinPlannerDiscontinuousJointTraj)
   lin.setAccelerationScale(1.0);
 
   planning_interface::MotionPlanResponse res;
-  ASSERT_FALSE(lin_->generate(lin.toRequest(), res));
+  ASSERT_FALSE(lin_->generate(planning_scene_, lin.toRequest(), res));
 }
 
 /**
@@ -373,7 +374,7 @@ TEST_P(TrajectoryGeneratorLINTest, LinStartEqualsGoal)
 
   // generate the LIN trajectory
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(lin_->generate(lin_joint_req, res));
+  ASSERT_TRUE(lin_->generate(planning_scene_, lin_joint_req, res));
   EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   // check the resulted trajectory
@@ -417,7 +418,7 @@ TEST_P(TrajectoryGeneratorLINTest, IncorrectJointNumber)
 
   // generate the LIN trajectory
   planning_interface::MotionPlanResponse res;
-  ASSERT_FALSE(lin_->generate(lin_joint_req, res));
+  ASSERT_FALSE(lin_->generate(planning_scene_, lin_joint_req, res));
   EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
@@ -435,7 +436,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartGoalIncompleteStartState)
 
   // generate lin trajectory
   planning_interface::MotionPlanResponse res;
-  EXPECT_FALSE(lin_->generate(lin_cart_req, res));
+  EXPECT_FALSE(lin_->generate(planning_scene_, lin_cart_req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE);
 }
 
@@ -453,7 +454,7 @@ TEST_P(TrajectoryGeneratorLINTest, cartGoalFrameIdBothConstraints)
 
   // generate lin trajectory
   planning_interface::MotionPlanResponse res;
-  ASSERT_TRUE(lin_->generate(lin_cart_req, res));
+  ASSERT_TRUE(lin_->generate(planning_scene_, lin_cart_req, res));
   EXPECT_TRUE(res.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   // check the resulted trajectory
diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp
index e7a0a82b99..ed8d32ca4f 100644
--- a/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp
+++ b/moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_generator_ptp.cpp
@@ -82,6 +82,7 @@ class TrajectoryGeneratorPTPTest : public testing::TestWithParam<std::string>
   // ros stuff
   ros::NodeHandle ph_{ "~" };
   robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
+  planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) };
 
   // trajectory generator
   std::unique_ptr<TrajectoryGenerator> ptp_;
@@ -194,7 +195,7 @@ TEST_P(TrajectoryGeneratorPTPTest, emptyRequest)
 
   EXPECT_FALSE(res.trajectory_->empty());
 
-  EXPECT_FALSE(ptp_->generate(req, res));
+  EXPECT_FALSE(ptp_->generate(planning_scene_, req, res));
 
   EXPECT_TRUE(res.trajectory_->empty());
 }
@@ -359,7 +360,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoal)
   //****************************************
   //*** test robot model without gripper ***
   //****************************************
-  ASSERT_TRUE(ptp_->generate(req, res));
+  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   moveit_msgs::MotionPlanResponse res_msg;
@@ -407,12 +408,12 @@ TEST_P(TrajectoryGeneratorPTPTest, testCartesianGoalMissingLinkNameConstraints)
 
   planning_interface::MotionPlanRequest req_no_position_constaint_link_name = req;
   req_no_position_constaint_link_name.goal_constraints.front().position_constraints.front().link_name = "";
-  ASSERT_FALSE(ptp_->generate(req_no_position_constaint_link_name, res));
+  ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_position_constaint_link_name, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 
   planning_interface::MotionPlanRequest req_no_orientation_constaint_link_name = req;
   req_no_orientation_constaint_link_name.goal_constraints.front().orientation_constraints.front().link_name = "";
-  ASSERT_FALSE(ptp_->generate(req_no_orientation_constaint_link_name, res));
+  ASSERT_FALSE(ptp_->generate(planning_scene_, req_no_orientation_constaint_link_name, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS);
 }
 
@@ -439,7 +440,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testInvalidCartesianGoal)
       kinematic_constraints::constructGoalConstraints(target_link_, pose, tolerance_pose, tolerance_angle);
   req.goal_constraints.push_back(pose_goal);
 
-  ASSERT_FALSE(ptp_->generate(req, res));
+  ASSERT_FALSE(ptp_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION);
   EXPECT_EQ(res.trajectory_, nullptr);
 }
@@ -465,7 +466,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAlreadyReached)
   req.goal_constraints.push_back(gc);
 
   // TODO lin and circ has different settings
-  ASSERT_TRUE(ptp_->generate(req, res));
+  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   moveit_msgs::MotionPlanResponse res_msg;
@@ -537,7 +538,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testScalingFactor)
   req.max_velocity_scaling_factor = 0.5;
   req.max_acceleration_scaling_factor = 1.0 / 3.0;
 
-  ASSERT_TRUE(ptp_->generate(req, res));
+  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   moveit_msgs::MotionPlanResponse res_msg;
@@ -664,7 +665,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalAndAlmostZeroStartVelocity)
   gc.joint_constraints.push_back(jc);
   req.goal_constraints.push_back(gc);
 
-  ASSERT_TRUE(ptp_->generate(req, res));
+  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   moveit_msgs::MotionPlanResponse res_msg;
@@ -806,7 +807,7 @@ TEST_P(TrajectoryGeneratorPTPTest, testJointGoalNoStartVel)
   gc.joint_constraints.push_back(jc);
   req.goal_constraints.push_back(gc);
 
-  ASSERT_TRUE(ptp_->generate(req, res));
+  ASSERT_TRUE(ptp_->generate(planning_scene_, req, res));
   EXPECT_EQ(res.error_code_.val, moveit_msgs::MoveItErrorCodes::SUCCESS);
 
   moveit_msgs::MotionPlanResponse res_msg;
diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
index b70265c3c5..171a4f4cdc 100644
--- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
+++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>pilz_industrial_motion_planner_testutils</name>
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <description>Helper scripts and functionality to test industrial motion generation</description>
 
   <maintainer email="a.gutenkunst@pilz.de">Alexander Gutenkunst</maintainer>
diff --git a/moveit_ros/manipulation/package.xml b/moveit_ros/manipulation/package.xml
index aed01703b6..73f270b240 100644
--- a/moveit_ros/manipulation/package.xml
+++ b/moveit_ros/manipulation/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>moveit_ros_manipulation</name>
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <description>Components of MoveIt used for manipulation</description>
   <maintainer email="me@v4hn.de">Michael Görner</maintainer>
   <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
index ee3613d9d4..70fb04ee52 100644
--- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
+++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
@@ -588,14 +588,16 @@ void MotionPlanningFrame::enable()
   ui_->object_status->setText("");
 
   // activate the frame
-  parentWidget()->show();
+  if (parentWidget())
+    parentWidget()->show();
 }
 
 void MotionPlanningFrame::disable()
 {
   move_group_.reset();
   scene_marker_.reset();
-  parentWidget()->hide();
+  if (parentWidget())
+    parentWidget()->hide();
 }
 
 void MotionPlanningFrame::tabChanged(int index)
diff --git a/moveit_setup_assistant/package.xml b/moveit_setup_assistant/package.xml
index 820c750bd1..1ec01b6259 100644
--- a/moveit_setup_assistant/package.xml
+++ b/moveit_setup_assistant/package.xml
@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>moveit_setup_assistant</name>
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <description>Generates a configuration package that makes it easy to use MoveIt</description>
   <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
   <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
diff --git a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template
index dc5691d6a3..d52f3a9f5f 100644
--- a/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template
+++ b/moveit_setup_assistant/templates/moveit_config_pkg_template/package.xml.template
@@ -28,6 +28,10 @@
   <run_depend>rviz</run_depend>
   <run_depend>tf2_ros</run_depend>
   <run_depend>xacro</run_depend>
+  <!-- The next 2 packages are required for the gazebo simulation.
+       We don't include them by default to prevent installing gazebo and all its dependencies. -->
+  <!-- <run_depend>joint_trajectory_controller</run_depend> -->
+  <!-- <run_depend>gazebo_ros_control</run_depend> -->
   <!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
   <!-- <run_depend>warehouse_ros_mongo</run_depend> -->
   [OTHER_DEPENDENCIES]