From ed76d6a1b2e63cb630d05ab6eb96b743fb889a8b Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@gmail.com>
Date: Thu, 14 Dec 2017 17:30:30 -0700
Subject: [PATCH 1/8] Removing test dependency and adding collision checking,
 fixing minor bug with class forwarding, and removing unecessary failure if
 tip_link != group tip

---
 .../include/descartes_core/robot_model.h      | 10 +-
 .../ikfast_moveit_state_adapter.h             |  3 +-
 .../descartes_moveit/moveit_state_adapter.h   | 27 +++++-
 descartes_moveit/package.xml                  |  2 +-
 .../src/ikfast_moveit_state_adapter.cpp       | 11 +--
 descartes_moveit/src/moveit_state_adapter.cpp | 92 +++++++++++++------
 .../descartes_trajectory/cart_trajectory_pt.h | 12 +--
 .../src/cart_trajectory_pt.cpp                | 17 ++--
 8 files changed, 117 insertions(+), 57 deletions(-)

diff --git a/descartes_core/include/descartes_core/robot_model.h b/descartes_core/include/descartes_core/robot_model.h
index 497132e1..050d0466 100644
--- a/descartes_core/include/descartes_core/robot_model.h
+++ b/descartes_core/include/descartes_core/robot_model.h
@@ -23,6 +23,13 @@
 #include <moveit/kinematic_constraints/kinematic_constraint.h>
 #include "descartes_core/utils.h"
 
+
+namespace planning_scene_monitor
+{
+  MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
+  class PlanningSceneMonitor;
+}
+
 namespace descartes_core
 {
 DESCARTES_CLASS_FORWARD(RobotModel);
@@ -107,7 +114,8 @@ class RobotModel
    * a fixed location relative to the last link in 'group_name'.
    */
   virtual bool initialize(const std::string &robot_description, const std::string &group_name,
-                          const std::string &world_frame, const std::string &tcp_frame) = 0;
+                          const std::string &world_frame, const std::string &tcp_frame,
+                          planning_scene_monitor::PlanningSceneMonitorPtr psm = nullptr) = 0;
 
   /**
    * @brief Enables collision checks
diff --git a/descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h
index 37976d42..5fe79786 100644
--- a/descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h
+++ b/descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h
@@ -31,7 +31,8 @@ class IkFastMoveitStateAdapter : public descartes_moveit::MoveitStateAdapter
   }
 
   virtual bool initialize(const std::string& robot_description, const std::string& group_name,
-                          const std::string& world_frame, const std::string& tcp_frame);
+                          const std::string& world_frame, const std::string& tcp_frame,
+                          planning_scene_monitor::PlanningSceneMonitorPtr psm = nullptr);
 
   virtual bool getAllIK(const Eigen::Isometry3d& pose, std::vector<std::vector<double> >& joint_poses) const;
 
diff --git a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
index eaa6965d..23fa712e 100644
--- a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
+++ b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
@@ -21,11 +21,18 @@
 
 #include "descartes_core/robot_model.h"
 #include "descartes_trajectory/cart_trajectory_pt.h"
-#include <moveit/planning_scene/planning_scene.h>
+
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
 #include <string>
 
+
+namespace planning_scene_monitor
+{
+  MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
+  class PlanningSceneMonitor;
+}
+
 namespace descartes_moveit
 {
 /**
@@ -41,10 +48,12 @@ class MoveitStateAdapter : public descartes_core::RobotModel
   }
 
   virtual bool initialize(const std::string &robot_description, const std::string &group_name,
-                          const std::string &world_frame, const std::string &tcp_frame);
+                          const std::string &world_frame, const std::string &tcp_frame,
+                          planning_scene_monitor::PlanningSceneMonitorPtr psm = nullptr);
 
   virtual bool initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
-                          const std::string &world_frame, const std::string &tcp_frame);
+                          const std::string &world_frame, const std::string &tcp_frame,
+                          planning_scene_monitor::PlanningSceneMonitorPtr psm = nullptr);
 
   virtual bool getIK(const Eigen::Isometry3d &pose, const std::vector<double> &seed_state,
                      std::vector<double> &joint_pose) const;
@@ -128,8 +137,6 @@ class MoveitStateAdapter : public descartes_core::RobotModel
 
   mutable moveit::core::RobotStatePtr robot_state_;
 
-  planning_scene::PlanningScenePtr planning_scene_;
-
   robot_model::RobotModelConstPtr robot_model_ptr_;
 
   robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
@@ -146,6 +153,11 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    */
   std::string group_name_;
 
+  /**
+   * @brief name of parameter for robot URDF
+   */
+  std::string robot_description_ = "robot_description";
+
   /**
    * @brief Tool frame name
    */
@@ -160,6 +172,11 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    * @brief convenient transformation frame
    */
   descartes_core::Frame world_to_root_;
+
+  /**
+   * @brief Planning scene monitor (used to update internal planning scene)
+   */
+  mutable planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
 };
 
 }  // descartes_moveit
diff --git a/descartes_moveit/package.xml b/descartes_moveit/package.xml
index 610893b5..eaea0499 100644
--- a/descartes_moveit/package.xml
+++ b/descartes_moveit/package.xml
@@ -7,7 +7,7 @@
   <maintainer email="jnicho@swri.org">Jorge Nicho</maintainer>
   <maintainer email="jonathan.meyer@swri.org">Jonathan Meyer</maintainer>
   <maintainer email="sedwards@swri.org">Shaun Edwards</maintainer>
-  
+
   <license>Apache2</license>
 
   <author >Shaun Edwards</author>
diff --git a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
index 2b623ab5..8e69385f 100644
--- a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
+++ b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
@@ -17,6 +17,7 @@
  */
 
 #include "descartes_moveit/ikfast_moveit_state_adapter.h"
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 
 #include <eigen_conversions/eigen_msg.h>
 #include <ros/node_handle.h>
@@ -54,9 +55,10 @@ static size_t closestJointPose(const std::vector<double>& target, const std::vec
 bool descartes_moveit::IkFastMoveitStateAdapter::initialize(const std::string& robot_description,
                                                             const std::string& group_name,
                                                             const std::string& world_frame,
-                                                            const std::string& tcp_frame)
+                                                            const std::string& tcp_frame,
+                                                            planning_scene_monitor::PlanningSceneMonitorPtr psm)
 {
-  if (!MoveitStateAdapter::initialize(robot_description, group_name, world_frame, tcp_frame))
+  if (!MoveitStateAdapter::initialize(robot_description, group_name, world_frame, tcp_frame, psm))
   {
     return false;
   }
@@ -70,12 +72,9 @@ bool descartes_moveit::IkFastMoveitStateAdapter::getAllIK(const Eigen::Isometry3
   joint_poses.clear();
   const auto& solver = joint_group_->getSolverInstance();
 
-  // Transform input pose
-  Eigen::Isometry3d tool_pose = world_to_base_.frame_inv * pose * tool0_to_tip_.frame;
-
   // convert to geometry_msgs ...
   geometry_msgs::Pose geometry_pose;
-  tf::poseEigenToMsg(tool_pose, geometry_pose);
+  tf::poseEigenToMsg(pose, geometry_pose);
   std::vector<geometry_msgs::Pose> poses = { geometry_pose };
 
   std::vector<double> dummy_seed(getDOF(), 0.0);
diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp
index 6f6ad8ed..19db31dc 100644
--- a/descartes_moveit/src/moveit_state_adapter.cpp
+++ b/descartes_moveit/src/moveit_state_adapter.cpp
@@ -23,7 +23,7 @@
 #include "descartes_core/pretty_print.hpp"
 #include "descartes_moveit/seed_search.h"
 
-
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <eigen_conversions/eigen_msg.h>
 #include <random_numbers/random_numbers.h>
 #include <ros/assert.h>
@@ -70,8 +70,11 @@ MoveitStateAdapter::MoveitStateAdapter() : world_to_root_(Eigen::Isometry3d::Ide
 }
 
 bool MoveitStateAdapter::initialize(const std::string& robot_description, const std::string& group_name,
-                                    const std::string& world_frame, const std::string& tcp_frame)
+                                    const std::string& world_frame, const std::string& tcp_frame,
+                                    planning_scene_monitor::PlanningSceneMonitorPtr psm)
+
 {
+  robot_description_ = robot_description;
   // Initialize MoveIt state objects
   robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
   auto model = robot_model_loader_->getModel();
@@ -81,16 +84,16 @@ bool MoveitStateAdapter::initialize(const std::string& robot_description, const
     return false;
   }
 
-  return initialize(model, group_name, world_frame, tcp_frame);
+  return initialize(model, group_name, world_frame, tcp_frame, psm);
 }
 
 bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
-                                    const std::string &world_frame, const std::string &tcp_frame)
+                                    const std::string &world_frame, const std::string &tcp_frame,
+                                    planning_scene_monitor::PlanningSceneMonitorPtr psm)
 {
   robot_model_ptr_ = robot_model;
   robot_state_.reset(new moveit::core::RobotState(robot_model_ptr_));
   robot_state_->setToDefaultValues();
-  planning_scene_.reset(new planning_scene::PlanningScene(robot_model));
   joint_group_ = robot_model_ptr_->getJointModelGroup(group_name);
 
   // Assign robot frames
@@ -111,10 +114,9 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
   const auto& link_names = joint_group_->getLinkModelNames();
   if (tool_frame_ != link_names.back())
   {
-    CONSOLE_BRIDGE_logError("%s: Tool frame '%s' does not match group tool frame '%s', functionality"
-             "will be implemented in the future",
-             __FUNCTION__, tool_frame_.c_str(), link_names.back().c_str());
-    return false;
+    CONSOLE_BRIDGE_logWarn("%s: Tool frame '%s' does not match group tool frame '%s', functionality"
+                           "will be implemented in the future",
+                           __FUNCTION__, tool_frame_.c_str(), link_names.back().c_str());
   }
 
   if (!::getJointVelocityLimits(*robot_state_, group_name, velocity_limits_))
@@ -139,6 +141,15 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
     world_to_root_ = descartes_core::Frame(root_to_world.inverse());
   }
 
+  if(psm)
+  {
+    planning_scene_monitor_ = psm;
+  }
+  else
+  {
+    planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(robot_description_));
+  }
+  planning_scene_monitor_->startSceneMonitor();
   return true;
 }
 
@@ -161,7 +172,7 @@ bool MoveitStateAdapter::getIK(const Eigen::Isometry3d& pose, std::vector<double
     robot_state_->copyJointGroupPositions(group_name_, joint_pose);
     if (!isValid(joint_pose))
     {
-      ROS_DEBUG_STREAM("Robot joint pose is invalid");
+      CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getIK: Robot joint pose is invalid");
     }
     else
     {
@@ -183,7 +194,7 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
   // in the middle of a discretization step could be double counted.  In reality, we'd like solutions
   // to be further apart than this.
   double epsilon = 4 * joint_group_->getSolverInstance()->getSearchDiscretization();
-  CONSOLE_BRIDGE_logDebug("Utilizing an min. difference of %f between IK solutions", epsilon);
+  CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Utilizing an min. difference of %f between IK solutions", epsilon);
   joint_poses.clear();
   for (size_t sample_iter = 0; sample_iter < seed_states_.size(); ++sample_iter)
   {
@@ -194,14 +205,16 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
       if (joint_poses.empty())
       {
         std::stringstream msg;
-        msg << "Found *first* solution on " << sample_iter << " iteration, joint: " << joint_pose;
+        msg << "MoveitStateAdapter.getAllIK: " << "Found *first* solution on " << sample_iter
+            << " iteration, joint: " << joint_pose;
         CONSOLE_BRIDGE_logDebug(msg.str().c_str());
         joint_poses.push_back(joint_pose);
       }
       else
       {
         std::stringstream msg;
-        msg << "Found *potential* solution on " << sample_iter << " iteration, joint: " << joint_pose;
+        msg << "MoveitStateAdapter.getAllIK: " << "Found *potential* solution on " << sample_iter
+            << " iteration, joint: " << joint_pose;
         CONSOLE_BRIDGE_logDebug(msg.str().c_str());
 
         std::vector<std::vector<double> >::iterator joint_pose_it;
@@ -210,7 +223,7 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
         {
           if (descartes_core::utils::equal(joint_pose, (*joint_pose_it), epsilon))
           {
-            CONSOLE_BRIDGE_logDebug("Found matching, potential solution is not new");
+            CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Found matching, potential solution is not new");
             match_found = true;
             break;
           }
@@ -218,7 +231,8 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
         if (!match_found)
         {
           std::stringstream msg;
-          msg << "Found *new* solution on " << sample_iter << " iteration, joint: " << joint_pose;
+          msg << "MoveitStateAdapter.getAllIK: " << "Found *new* solution on " << sample_iter
+              << " iteration, joint: " << joint_pose;
           CONSOLE_BRIDGE_logDebug(msg.str().c_str());
           joint_poses.push_back(joint_pose);
         }
@@ -226,18 +240,18 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
     }
   }
 
-  CONSOLE_BRIDGE_logDebug("Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
-           static_cast<unsigned long>(seed_states_.size()));
+  CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
+                          static_cast<unsigned long>(seed_states_.size()));
 
   if (joint_poses.empty())
   {
-    CONSOLE_BRIDGE_logError("Found 0 joint solutions out of %lu iterations", static_cast<unsigned long>(seed_states_.size()));
+    CONSOLE_BRIDGE_logError("MoveitStateAdapter.getAllIK: Found 0 joint solutions out of %lu iterations", static_cast<unsigned long>(seed_states_.size()));
     return false;
   }
   else
   {
-    CONSOLE_BRIDGE_logInform("Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
-              static_cast<unsigned long>(seed_states_.size()));
+    CONSOLE_BRIDGE_logInform("MoveitStateAdapter.getAllIK: Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
+                             static_cast<unsigned long>(seed_states_.size()));
     return true;
   }
 }
@@ -247,11 +261,17 @@ bool MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) co
   bool in_collision = false;
   if (check_collisions_)
   {
-    moveit::core::RobotState state (robot_model_ptr_);
-    state.setToDefaultValues();
-    state.setJointGroupPositions(joint_group_, joint_pose);
-    in_collision = planning_scene_->isStateColliding(state, group_name_);
+    std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_));
+
+    // (*ls)->printKnownObjects(std::cout);
+
+    robot_state_->setJointGroupPositions(group_name_, joint_pose);
+    robot_state_->update();
+
+    // If either there is no locked planning scene or if the state is colliding return false
+    in_collision = !(*ls) || (*ls)->isStateColliding(*robot_state_, group_name_);
   }
+
   return in_collision;
 }
 
@@ -274,13 +294,13 @@ bool MoveitStateAdapter::getFK(const std::vector<double>& joint_pose, Eigen::Iso
     }
     else
     {
-      CONSOLE_BRIDGE_logError("Robot state does not recognize tool frame: %s", tool_frame_.c_str());
+      CONSOLE_BRIDGE_logError("MoveitStateAdapter.getFK: Robot state does not recognize tool frame: %s", tool_frame_.c_str());
       rtn = false;
     }
   }
   else
   {
-    CONSOLE_BRIDGE_logError("Invalid joint pose passed to get forward kinematics");
+    CONSOLE_BRIDGE_logError("MoveitStateAdapter.getFK: Invalid joint pose passed to get forward kinematics");
     rtn = false;
   }
 
@@ -298,7 +318,19 @@ bool MoveitStateAdapter::isValid(const std::vector<double>& joint_pose) const
     return false;
   }
 
-  return isInLimits(joint_pose) && !isInCollision(joint_pose);
+  // Satisfies joint positional bounds?
+  if (!isInLimits(joint_pose))
+  {
+    CONSOLE_BRIDGE_logError("Joint pose does not satisfy positional bounds");
+    return false;
+  }
+  // Is in collision (if collision is active)
+  if (isInCollision(joint_pose))
+  {
+    CONSOLE_BRIDGE_logDebug("Joint pose is in collision");
+    return false;
+  }
+  return true;
 }
 
 bool MoveitStateAdapter::isValid(const Eigen::Isometry3d& pose) const
@@ -335,10 +367,12 @@ std::vector<double> MoveitStateAdapter::getJointVelocityLimits() const
 
 void MoveitStateAdapter::setState(const moveit::core::RobotState& state)
 {
-  ROS_ASSERT_MSG(static_cast<bool>(robot_state_), "'robot_state_' member pointer is null. Have you called "
+  if(static_cast<bool>(robot_state_)){
+    logError("'robot_state_' member pointer is null. Have you called "
                                                   "initialize()?");
+  }
   *robot_state_ = state;
-  planning_scene_->setCurrentState(state);
+  planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)->setCurrentState(state);
 }
 
 }  // descartes_moveit
diff --git a/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h b/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h
index 1ab6fae9..956c4c2e 100644
--- a/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h
+++ b/descartes_trajectory/include/descartes_trajectory/cart_trajectory_pt.h
@@ -102,10 +102,10 @@ struct ToleranceBase
     , y_lower(y_lower_lim)
     , z_lower(z_lower_lim)
   {
-    ROS_DEBUG_STREAM("Creating fully defined Tolerance(base type)");
-    ROS_DEBUG_STREAM("Initializing x tolerance (lower/upper)" << x_lower << "/" << x_upper);
-    ROS_DEBUG_STREAM("Initializing y tolerance (lower/upper)" << y_lower << "/" << y_upper);
-    ROS_DEBUG_STREAM("Initializing z tolerance (lower/upper)" << z_lower << "/" << z_upper);
+    ROS_DEBUG_STREAM_NAMED("ToleranceBase", "Creating fully defined Tolerance(base type)");
+    ROS_DEBUG_STREAM_NAMED("ToleranceBase", "Initializing x tolerance (lower/upper)" << x_lower << "/" << x_upper);
+    ROS_DEBUG_STREAM_NAMED("ToleranceBase", "Initializing y tolerance (lower/upper)" << y_lower << "/" << y_upper);
+    ROS_DEBUG_STREAM_NAMED("ToleranceBase", "Initializing z tolerance (lower/upper)" << z_lower << "/" << z_upper);
   }
 
   void clear()
@@ -129,7 +129,7 @@ struct PositionTolerance : public ToleranceBase
                     double z_upper_lim)
     : ToleranceBase(x_lower_lim, x_upper_lim, y_lower_lim, y_upper_lim, z_lower_lim, z_upper_lim)
   {
-    ROS_DEBUG_STREAM("Created fully defined Position Tolerance");
+    ROS_DEBUG_STREAM_NAMED("PositionTolerance", "Created fully defined Position Tolerance");
   }
 };
 
@@ -146,7 +146,7 @@ struct OrientationTolerance : public ToleranceBase
                        double z_lower_lim, double z_upper_lim)
     : ToleranceBase(x_lower_lim, x_upper_lim, y_lower_lim, y_upper_lim, z_lower_lim, z_upper_lim)
   {
-    ROS_DEBUG_STREAM("Created fully defined Orientation Tolerance");
+    ROS_DEBUG_STREAM_NAMED("OrientationTolerance","Created fully defined Orientation Tolerance");
   }
 };
 
diff --git a/descartes_trajectory/src/cart_trajectory_pt.cpp b/descartes_trajectory/src/cart_trajectory_pt.cpp
index 07df6820..c9d37363 100644
--- a/descartes_trajectory/src/cart_trajectory_pt.cpp
+++ b/descartes_trajectory/src/cart_trajectory_pt.cpp
@@ -83,7 +83,7 @@ EigenSTL::vector_Isometry3d uniform(const TolerancedFrame &frame, const double o
   // Estimating the number of samples base on tolerance zones and sampling increment.
   size_t est_num_samples = ntx * nty * ntz * nrx * nry * nrz;
 
-  ROS_DEBUG_STREAM("Estimated number of samples: " << est_num_samples << ", reserving space");
+  ROS_DEBUG_STREAM_NAMED("Descartes", "Estimated number of samples: " << est_num_samples << ", reserving space");
   rtn.reserve(est_num_samples);
 
   // TODO: The following for loops do not ensure that the rull range is sample (lower to upper)
@@ -124,9 +124,10 @@ EigenSTL::vector_Isometry3d uniform(const TolerancedFrame &frame, const double o
       }
     }
   }
-  ROS_DEBUG_STREAM("Uniform sampling of frame, utilizing orientation increment: "
-                   << orient_increment << ", and position increment: " << pos_increment << " resulted in " << rtn.size()
-                   << " samples");
+  ROS_DEBUG_STREAM_NAMED("Descartes",
+                         "Uniform sampling of frame, utilizing orientation increment: " << orient_increment <<
+                         ", and position increment: " << pos_increment <<
+                         " resulted in " << rtn.size() << " samples");
   return rtn;
 }
 
@@ -260,8 +261,8 @@ void CartTrajectoryPt::getCartesianPoses(const RobotModel &model, EigenSTL::vect
   }
   else
   {
-    ROS_DEBUG_STREAM("Get cartesian poses, sampled: " << all_poses.size() << ", with " << poses.size()
-                                                      << " valid(returned) poses");
+    ROS_DEBUG_STREAM_NAMED("Descartes", "Get cartesian poses, sampled: " << all_poses.size() << ", with " << poses.size()
+                                                                         << " valid(returned) poses");
   }
 }
 
@@ -397,8 +398,8 @@ void CartTrajectoryPt::getJointPoses(const RobotModel &model, std::vector<std::v
   }
   else
   {
-    ROS_DEBUG_STREAM("Get joint poses, sampled: " << poses.size() << ", with " << joint_poses.size()
-                                                  << " valid(returned) poses");
+    ROS_DEBUG_STREAM_NAMED("Descartes", "Get joint poses, sampled: " << poses.size() << ", with " << joint_poses.size()
+                                                                     << " valid(returned) poses");
   }
 }
 

From fdda755bfea96276890d6933901c6663a7aa6b51 Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Thu, 21 Mar 2019 17:48:25 -0600
Subject: [PATCH 2/8] removing moveit_core from descartes_core dependencies

---
 descartes_core/CMakeLists.txt                               | 2 --
 descartes_core/package.xml                                  | 2 --
 .../include/descartes_moveit/moveit_state_adapter.h         | 6 ------
 3 files changed, 10 deletions(-)

diff --git a/descartes_core/CMakeLists.txt b/descartes_core/CMakeLists.txt
index 0172621d..6baedd7f 100644
--- a/descartes_core/CMakeLists.txt
+++ b/descartes_core/CMakeLists.txt
@@ -5,7 +5,6 @@ add_compile_options(-std=c++11 -Wall -Wextra)
 
 find_package(catkin REQUIRED COMPONENTS
   cmake_modules
-  moveit_core
   roscpp
 )
 
@@ -23,7 +22,6 @@ catkin_package(
   LIBRARIES
     ${PROJECT_NAME}
   CATKIN_DEPENDS
-    moveit_core
     roscpp
   DEPENDS
     Boost
diff --git a/descartes_core/package.xml b/descartes_core/package.xml
index 387c7acd..96a8997a 100644
--- a/descartes_core/package.xml
+++ b/descartes_core/package.xml
@@ -16,13 +16,11 @@
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>boost</build_depend>
-  <build_depend>moveit_core</build_depend>
   <build_depend>roscpp</build_depend>
   <build_depend>cmake_modules</build_depend>
   <build_depend>eigen</build_depend>
 
   <run_depend>boost</run_depend>
-  <run_depend>moveit_core</run_depend>
   <run_depend>roscpp</run_depend>
 
   <test_depend>rosunit</test_depend>
diff --git a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
index 23fa712e..a1ed14d6 100644
--- a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
+++ b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
@@ -27,12 +27,6 @@
 #include <string>
 
 
-namespace planning_scene_monitor
-{
-  MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
-  class PlanningSceneMonitor;
-}
-
 namespace descartes_moveit
 {
 /**

From 24f9f4cb6a6ddcecf9a4d90159cfc87d234645b8 Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Thu, 21 Mar 2019 17:48:41 -0600
Subject: [PATCH 3/8] fixing logging call

---
 descartes_moveit/src/moveit_state_adapter.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp
index 19db31dc..7d276b4d 100644
--- a/descartes_moveit/src/moveit_state_adapter.cpp
+++ b/descartes_moveit/src/moveit_state_adapter.cpp
@@ -368,7 +368,7 @@ std::vector<double> MoveitStateAdapter::getJointVelocityLimits() const
 void MoveitStateAdapter::setState(const moveit::core::RobotState& state)
 {
   if(static_cast<bool>(robot_state_)){
-    logError("'robot_state_' member pointer is null. Have you called "
+    CONSOLE_BRIDGE_logDebug("'robot_state_' member pointer is null. Have you called "
                                                   "initialize()?");
   }
   *robot_state_ = state;

From 08fe4ca6d68a0e4b9c27352b1f450f00e5a5ace7 Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Thu, 21 Mar 2019 19:27:02 -0600
Subject: [PATCH 4/8] refactoring to be more efficient

---
 .../include/descartes_core/robot_model.h      |  4 +-
 .../ikfast_moveit_state_adapter.h             |  3 +-
 .../descartes_moveit/moveit_state_adapter.h   | 13 +++--
 .../src/ikfast_moveit_state_adapter.cpp       |  5 +-
 descartes_moveit/src/moveit_state_adapter.cpp | 48 +++++++++----------
 5 files changed, 36 insertions(+), 37 deletions(-)

diff --git a/descartes_core/include/descartes_core/robot_model.h b/descartes_core/include/descartes_core/robot_model.h
index 050d0466..44d6257f 100644
--- a/descartes_core/include/descartes_core/robot_model.h
+++ b/descartes_core/include/descartes_core/robot_model.h
@@ -114,8 +114,7 @@ class RobotModel
    * a fixed location relative to the last link in 'group_name'.
    */
   virtual bool initialize(const std::string &robot_description, const std::string &group_name,
-                          const std::string &world_frame, const std::string &tcp_frame,
-                          planning_scene_monitor::PlanningSceneMonitorPtr psm = nullptr) = 0;
+                          const std::string &world_frame, const std::string &tcp_frame) = 0;
 
   /**
    * @brief Enables collision checks
@@ -158,6 +157,7 @@ class RobotModel
   }
 
   bool check_collisions_;
+
 };
 
 }  // descartes_core
diff --git a/descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h
index 5fe79786..37976d42 100644
--- a/descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h
+++ b/descartes_moveit/include/descartes_moveit/ikfast_moveit_state_adapter.h
@@ -31,8 +31,7 @@ class IkFastMoveitStateAdapter : public descartes_moveit::MoveitStateAdapter
   }
 
   virtual bool initialize(const std::string& robot_description, const std::string& group_name,
-                          const std::string& world_frame, const std::string& tcp_frame,
-                          planning_scene_monitor::PlanningSceneMonitorPtr psm = nullptr);
+                          const std::string& world_frame, const std::string& tcp_frame);
 
   virtual bool getAllIK(const Eigen::Isometry3d& pose, std::vector<std::vector<double> >& joint_poses) const;
 
diff --git a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
index a1ed14d6..d2f90cd4 100644
--- a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
+++ b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
@@ -42,12 +42,10 @@ class MoveitStateAdapter : public descartes_core::RobotModel
   }
 
   virtual bool initialize(const std::string &robot_description, const std::string &group_name,
-                          const std::string &world_frame, const std::string &tcp_frame,
-                          planning_scene_monitor::PlanningSceneMonitorPtr psm = nullptr);
+                          const std::string &world_frame, const std::string &tcp_frame);
 
   virtual bool initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
-                          const std::string &world_frame, const std::string &tcp_frame,
-                          planning_scene_monitor::PlanningSceneMonitorPtr psm = nullptr);
+                          const std::string &world_frame, const std::string &tcp_frame);
 
   virtual bool getIK(const Eigen::Isometry3d &pose, const std::vector<double> &seed_state,
                      std::vector<double> &joint_pose) const;
@@ -99,6 +97,8 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    */
   void setState(const moveit::core::RobotState &state);
 
+  void setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm);
+
 protected:
   /**
    * Gets IK solution (assumes robot state is pre-seeded)
@@ -162,6 +162,11 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    */
   std::string world_frame_;
 
+  /**
+   * @brief Name of robot base frame
+   */
+  std::string robot_base_frame_;
+
   /**
    * @brief convenient transformation frame
    */
diff --git a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
index 8e69385f..972005a0 100644
--- a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
+++ b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
@@ -55,10 +55,9 @@ static size_t closestJointPose(const std::vector<double>& target, const std::vec
 bool descartes_moveit::IkFastMoveitStateAdapter::initialize(const std::string& robot_description,
                                                             const std::string& group_name,
                                                             const std::string& world_frame,
-                                                            const std::string& tcp_frame,
-                                                            planning_scene_monitor::PlanningSceneMonitorPtr psm)
+                                                            const std::string& tcp_frame)
 {
-  if (!MoveitStateAdapter::initialize(robot_description, group_name, world_frame, tcp_frame, psm))
+  if (!MoveitStateAdapter::initialize(robot_description, group_name, world_frame, tcp_frame))
   {
     return false;
   }
diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp
index 7d276b4d..932c6fbb 100644
--- a/descartes_moveit/src/moveit_state_adapter.cpp
+++ b/descartes_moveit/src/moveit_state_adapter.cpp
@@ -70,8 +70,7 @@ MoveitStateAdapter::MoveitStateAdapter() : world_to_root_(Eigen::Isometry3d::Ide
 }
 
 bool MoveitStateAdapter::initialize(const std::string& robot_description, const std::string& group_name,
-                                    const std::string& world_frame, const std::string& tcp_frame,
-                                    planning_scene_monitor::PlanningSceneMonitorPtr psm)
+                                    const std::string& world_frame, const std::string& tcp_frame)
 
 {
   robot_description_ = robot_description;
@@ -84,19 +83,18 @@ bool MoveitStateAdapter::initialize(const std::string& robot_description, const
     return false;
   }
 
-  return initialize(model, group_name, world_frame, tcp_frame, psm);
+  return initialize(model, group_name, world_frame, tcp_frame);
 }
 
 bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
-                                    const std::string &world_frame, const std::string &tcp_frame,
-                                    planning_scene_monitor::PlanningSceneMonitorPtr psm)
+                                    const std::string &world_frame, const std::string &tcp_frame)
 {
   robot_model_ptr_ = robot_model;
   robot_state_.reset(new moveit::core::RobotState(robot_model_ptr_));
   robot_state_->setToDefaultValues();
   joint_group_ = robot_model_ptr_->getJointModelGroup(group_name);
 
-  // Assign robot frames
+  // Assign robot parameters
   group_name_ = group_name;
   tool_frame_ = tcp_frame;
   world_frame_ = world_frame;
@@ -111,12 +109,11 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
     return false;
   }
 
-  const auto& link_names = joint_group_->getLinkModelNames();
-  if (tool_frame_ != link_names.back())
+  if (tool_frame_ != joint_group_->getLinkModelNames().back())
   {
     CONSOLE_BRIDGE_logWarn("%s: Tool frame '%s' does not match group tool frame '%s', functionality"
                            "will be implemented in the future",
-                           __FUNCTION__, tool_frame_.c_str(), link_names.back().c_str());
+                           __FUNCTION__, tool_frame_.c_str(), joint_group_->getLinkModelNames().back().c_str());
   }
 
   if (!::getJointVelocityLimits(*robot_state_, group_name, velocity_limits_))
@@ -130,29 +127,28 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
     CONSOLE_BRIDGE_logDebug("Generated %lu random seeds", static_cast<unsigned long>(seed_states_.size()));
   }
 
-  auto model_frame = robot_state_->getRobotModel()->getModelFrame();
-  if (world_frame_ != model_frame)
+  robot_base_frame_ = robot_state_->getRobotModel()->getModelFrame();
+  if (world_frame_ != robot_base_frame_)
   {
     CONSOLE_BRIDGE_logInform("%s: World frame '%s' does not match model root frame '%s', all poses will be"
               " transformed to world frame '%s'",
-              __FUNCTION__, world_frame_.c_str(), model_frame.c_str(), world_frame_.c_str());
+              __FUNCTION__, world_frame_.c_str(), robot_base_frame_.c_str(), world_frame_.c_str());
 
     Eigen::Isometry3d root_to_world = toIsometry(robot_state_->getFrameTransform(world_frame_));
     world_to_root_ = descartes_core::Frame(root_to_world.inverse());
   }
 
-  if(psm)
-  {
-    planning_scene_monitor_ = psm;
-  }
-  else
-  {
-    planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(robot_description_));
-  }
+  planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(robot_description_));
   planning_scene_monitor_->startSceneMonitor();
   return true;
 }
 
+void MoveitStateAdapter::setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm)
+{
+  planning_scene_monitor_ = psm;
+  planning_scene_monitor_->startSceneMonitor();
+}
+
 bool MoveitStateAdapter::getIK(const Eigen::Isometry3d& pose, const std::vector<double>& seed_state,
                                std::vector<double>& joint_pose) const
 {
@@ -263,13 +259,12 @@ bool MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) co
   {
     std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_));
 
-    // (*ls)->printKnownObjects(std::cout);
-
-    robot_state_->setJointGroupPositions(group_name_, joint_pose);
-    robot_state_->update();
+    robot_state::RobotState robot_state_copy = *robot_state_;
+    robot_state_copy.setJointGroupPositions(group_name_, joint_pose);
+    robot_state_copy.update();
 
     // If either there is no locked planning scene or if the state is colliding return false
-    in_collision = !(*ls) || (*ls)->isStateColliding(*robot_state_, group_name_);
+    in_collision = !(*ls) || (*ls)->isStateColliding(robot_state_copy, group_name_);
   }
 
   return in_collision;
@@ -321,9 +316,10 @@ bool MoveitStateAdapter::isValid(const std::vector<double>& joint_pose) const
   // Satisfies joint positional bounds?
   if (!isInLimits(joint_pose))
   {
-    CONSOLE_BRIDGE_logError("Joint pose does not satisfy positional bounds");
+    CONSOLE_BRIDGE_logInform("Joint pose does not satisfy positional bounds");
     return false;
   }
+
   // Is in collision (if collision is active)
   if (isInCollision(joint_pose))
   {

From e8524d003038f22abf1ccd9971c9147ea6956710 Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Mon, 25 Mar 2019 16:42:25 -0600
Subject: [PATCH 5/8] restoring unrelated changes

---
 .../include/descartes_core/robot_model.h      |  6 ----
 .../descartes_moveit/moveit_state_adapter.h   | 11 +------
 .../src/ikfast_moveit_state_adapter.cpp       | 14 ++++----
 descartes_moveit/src/moveit_state_adapter.cpp | 32 +++++++++++--------
 4 files changed, 27 insertions(+), 36 deletions(-)

diff --git a/descartes_core/include/descartes_core/robot_model.h b/descartes_core/include/descartes_core/robot_model.h
index 44d6257f..6f754dd3 100644
--- a/descartes_core/include/descartes_core/robot_model.h
+++ b/descartes_core/include/descartes_core/robot_model.h
@@ -24,12 +24,6 @@
 #include "descartes_core/utils.h"
 
 
-namespace planning_scene_monitor
-{
-  MOVEIT_CLASS_FORWARD(PlanningSceneMonitor);
-  class PlanningSceneMonitor;
-}
-
 namespace descartes_core
 {
 DESCARTES_CLASS_FORWARD(RobotModel);
diff --git a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
index d2f90cd4..564c5619 100644
--- a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
+++ b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
@@ -24,6 +24,7 @@
 
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <string>
 
 
@@ -147,11 +148,6 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    */
   std::string group_name_;
 
-  /**
-   * @brief name of parameter for robot URDF
-   */
-  std::string robot_description_ = "robot_description";
-
   /**
    * @brief Tool frame name
    */
@@ -162,11 +158,6 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    */
   std::string world_frame_;
 
-  /**
-   * @brief Name of robot base frame
-   */
-  std::string robot_base_frame_;
-
   /**
    * @brief convenient transformation frame
    */
diff --git a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
index 972005a0..f5cdf526 100644
--- a/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
+++ b/descartes_moveit/src/ikfast_moveit_state_adapter.cpp
@@ -17,13 +17,12 @@
  */
 
 #include "descartes_moveit/ikfast_moveit_state_adapter.h"
-#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 
 #include <eigen_conversions/eigen_msg.h>
 #include <ros/node_handle.h>
 
-const static std::string default_base_frame = "base_link";
-const static std::string default_tool_frame = "tool0";
+const static std::string DEFAULT_BASE_FRAME = "base_link";
+const static std::string DEFAULT_TOOL_FRAME = "tool0";
 
 // Compute the 'joint distance' between two poses
 static double distance(const std::vector<double>& a, const std::vector<double>& b)
@@ -71,9 +70,12 @@ bool descartes_moveit::IkFastMoveitStateAdapter::getAllIK(const Eigen::Isometry3
   joint_poses.clear();
   const auto& solver = joint_group_->getSolverInstance();
 
+  // Transform input pose
+  Eigen::Isometry3d tool_pose = world_to_base_.frame_inv * pose * tool0_to_tip_.frame;
+
   // convert to geometry_msgs ...
   geometry_msgs::Pose geometry_pose;
-  tf::poseEigenToMsg(pose, geometry_pose);
+  tf::poseEigenToMsg(tool_pose, geometry_pose);
   std::vector<geometry_msgs::Pose> poses = { geometry_pose };
 
   std::vector<double> dummy_seed(getDOF(), 0.0);
@@ -138,8 +140,8 @@ bool descartes_moveit::IkFastMoveitStateAdapter::computeIKFastTransforms()
   // look up the IKFast base and tool frame
   ros::NodeHandle nh;
   std::string ikfast_base_frame, ikfast_tool_frame;
-  nh.param<std::string>("ikfast_base_frame", ikfast_base_frame, default_base_frame);
-  nh.param<std::string>("ikfast_tool_frame", ikfast_tool_frame, default_tool_frame);
+  nh.param<std::string>("ikfast_base_frame", ikfast_base_frame, DEFAULT_BASE_FRAME);
+  nh.param<std::string>("ikfast_tool_frame", ikfast_tool_frame, DEFAULT_TOOL_FRAME);
 
   if (!robot_state_->knowsFrameTransform(ikfast_base_frame))
   {
diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp
index 932c6fbb..c8ff3ee3 100644
--- a/descartes_moveit/src/moveit_state_adapter.cpp
+++ b/descartes_moveit/src/moveit_state_adapter.cpp
@@ -23,7 +23,6 @@
 #include "descartes_core/pretty_print.hpp"
 #include "descartes_moveit/seed_search.h"
 
-#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
 #include <eigen_conversions/eigen_msg.h>
 #include <random_numbers/random_numbers.h>
 #include <ros/assert.h>
@@ -73,7 +72,6 @@ bool MoveitStateAdapter::initialize(const std::string& robot_description, const
                                     const std::string& world_frame, const std::string& tcp_frame)
 
 {
-  robot_description_ = robot_description;
   // Initialize MoveIt state objects
   robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
   auto model = robot_model_loader_->getModel();
@@ -109,11 +107,13 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
     return false;
   }
 
-  if (tool_frame_ != joint_group_->getLinkModelNames().back())
+  const auto& link_names = joint_group_->getLinkModelNames();
+  if (tool_frame_ != link_names.back())
   {
-    CONSOLE_BRIDGE_logWarn("%s: Tool frame '%s' does not match group tool frame '%s', functionality"
-                           "will be implemented in the future",
-                           __FUNCTION__, tool_frame_.c_str(), joint_group_->getLinkModelNames().back().c_str());
+    CONSOLE_BRIDGE_logError("%s: Tool frame '%s' does not match group tool frame '%s', functionality"
+             "will be implemented in the future",
+             __FUNCTION__, tool_frame_.c_str(), link_names.back().c_str());
+    return false;
   }
 
   if (!::getJointVelocityLimits(*robot_state_, group_name, velocity_limits_))
@@ -127,26 +127,29 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
     CONSOLE_BRIDGE_logDebug("Generated %lu random seeds", static_cast<unsigned long>(seed_states_.size()));
   }
 
-  robot_base_frame_ = robot_state_->getRobotModel()->getModelFrame();
-  if (world_frame_ != robot_base_frame_)
+  auto model_frame = robot_state_->getRobotModel()->getModelFrame();
+  if (world_frame_ != model_frame)
   {
     CONSOLE_BRIDGE_logInform("%s: World frame '%s' does not match model root frame '%s', all poses will be"
               " transformed to world frame '%s'",
-              __FUNCTION__, world_frame_.c_str(), robot_base_frame_.c_str(), world_frame_.c_str());
+              __FUNCTION__, world_frame_.c_str(), model_frame.c_str(), world_frame_.c_str());
 
     Eigen::Isometry3d root_to_world = toIsometry(robot_state_->getFrameTransform(world_frame_));
     world_to_root_ = descartes_core::Frame(root_to_world.inverse());
   }
 
-  planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(robot_description_));
-  planning_scene_monitor_->startSceneMonitor();
+  planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader_->getRobotDescription()));
+  // Start the psm
+  setPlanningSceneMonitor(planning_scene_monitor_);
   return true;
 }
 
 void MoveitStateAdapter::setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm)
 {
   planning_scene_monitor_ = psm;
-  planning_scene_monitor_->startSceneMonitor();
+  planning_scene_monitor->startSceneMonitor();
+  planning_scene_monitor->startWorldGeometryMonitor();
+  planning_scene_monitor->startStateMonitor();
 }
 
 bool MoveitStateAdapter::getIK(const Eigen::Isometry3d& pose, const std::vector<double>& seed_state,
@@ -316,16 +319,17 @@ bool MoveitStateAdapter::isValid(const std::vector<double>& joint_pose) const
   // Satisfies joint positional bounds?
   if (!isInLimits(joint_pose))
   {
-    CONSOLE_BRIDGE_logInform("Joint pose does not satisfy positional bounds");
+    CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.isValid: Joint pose does not satisfy positional bounds");
     return false;
   }
 
   // Is in collision (if collision is active)
   if (isInCollision(joint_pose))
   {
-    CONSOLE_BRIDGE_logDebug("Joint pose is in collision");
+    CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.isValid: Joint pose is in collision");
     return false;
   }
+
   return true;
 }
 

From 3da10dbde71c382d3319f6e9089f36ccb86a447e Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Mon, 25 Mar 2019 17:49:54 -0600
Subject: [PATCH 6/8] fixing psm segfault bug

---
 descartes_moveit/src/moveit_state_adapter.cpp | 19 ++++++++++++-------
 1 file changed, 12 insertions(+), 7 deletions(-)

diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp
index c8ff3ee3..a2aac05f 100644
--- a/descartes_moveit/src/moveit_state_adapter.cpp
+++ b/descartes_moveit/src/moveit_state_adapter.cpp
@@ -147,9 +147,7 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
 void MoveitStateAdapter::setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm)
 {
   planning_scene_monitor_ = psm;
-  planning_scene_monitor->startSceneMonitor();
-  planning_scene_monitor->startWorldGeometryMonitor();
-  planning_scene_monitor->startStateMonitor();
+  planning_scene_monitor_->startSceneMonitor();
 }
 
 bool MoveitStateAdapter::getIK(const Eigen::Isometry3d& pose, const std::vector<double>& seed_state,
@@ -262,12 +260,19 @@ bool MoveitStateAdapter::isInCollision(const std::vector<double>& joint_pose) co
   {
     std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_));
 
-    robot_state::RobotState robot_state_copy = *robot_state_;
+    // Check if we successfully got a locked planning scene
+    if (!(*ls))
+    {
+      CONSOLE_BRIDGE_logError("MoveitStateAdapter.isInCollision: Failed to secure a locked planning scene. Has the planing scene monitor been correctly initialized?");
+      return false;
+    }
+
+    // Seed with current state in case their are joints outside of the planning group
+    robot_state::RobotState robot_state_copy = (*ls)->getCurrentState();
     robot_state_copy.setJointGroupPositions(group_name_, joint_pose);
-    robot_state_copy.update();
 
-    // If either there is no locked planning scene or if the state is colliding return false
-    in_collision = !(*ls) || (*ls)->isStateColliding(robot_state_copy, group_name_);
+    // If the state is colliding return false
+    in_collision = (*ls)->isStateColliding(robot_state_copy);
   }
 
   return in_collision;

From 2de21d80d9f6f90c4b86cf4ad32f23def15921d3 Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Tue, 26 Mar 2019 20:30:40 -0600
Subject: [PATCH 7/8] use current joint state

---
 .../descartes_moveit/moveit_state_adapter.h   | 13 +++--
 descartes_moveit/src/moveit_state_adapter.cpp | 51 +++++++++++--------
 2 files changed, 35 insertions(+), 29 deletions(-)

diff --git a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
index 564c5619..6afc198d 100644
--- a/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
+++ b/descartes_moveit/include/descartes_moveit/moveit_state_adapter.h
@@ -45,7 +45,7 @@ class MoveitStateAdapter : public descartes_core::RobotModel
   virtual bool initialize(const std::string &robot_description, const std::string &group_name,
                           const std::string &world_frame, const std::string &tcp_frame);
 
-  virtual bool initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
+  virtual bool initialize(planning_scene_monitor::PlanningSceneMonitorPtr& psm, const std::string &group_name,
                           const std::string &world_frame, const std::string &tcp_frame);
 
   virtual bool getIK(const Eigen::Isometry3d &pose, const std::vector<double> &seed_state,
@@ -98,8 +98,6 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    */
   void setState(const moveit::core::RobotState &state);
 
-  void setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm);
-
 protected:
   /**
    * Gets IK solution (assumes robot state is pre-seeded)
@@ -124,6 +122,11 @@ class MoveitStateAdapter : public descartes_core::RobotModel
    */
   bool isInLimits(const std::vector<double>& joint_pose) const;
 
+  /**
+   * @brief Updates the robot_state_ variable with the most recent joint states from the psm.
+   */
+  bool getCurrentRobotState() const;
+
   /**
    * Maximum joint velocities (rad/s) for each joint in the chain. Used for checking in
    * `isValidMove()`
@@ -132,10 +135,6 @@ class MoveitStateAdapter : public descartes_core::RobotModel
 
   mutable moveit::core::RobotStatePtr robot_state_;
 
-  robot_model::RobotModelConstPtr robot_model_ptr_;
-
-  robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
-
   const moveit::core::JointModelGroup *joint_group_;
 
   /**
diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp
index a2aac05f..c8429c67 100644
--- a/descartes_moveit/src/moveit_state_adapter.cpp
+++ b/descartes_moveit/src/moveit_state_adapter.cpp
@@ -73,24 +73,20 @@ bool MoveitStateAdapter::initialize(const std::string& robot_description, const
 
 {
   // Initialize MoveIt state objects
-  robot_model_loader_.reset(new robot_model_loader::RobotModelLoader(robot_description));
-  auto model = robot_model_loader_->getModel();
-  if (!model)
-  {
-    CONSOLE_BRIDGE_logError("Failed to load robot model from robot description parameter: %s", robot_description.c_str());
-    return false;
-  }
-
-  return initialize(model, group_name, world_frame, tcp_frame);
+  planning_scene_monitor::PlanningSceneMonitorPtr psm(new planning_scene_monitor::PlanningSceneMonitor(robot_description));
+  return initialize(psm, group_name, world_frame, tcp_frame);
 }
 
-bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model, const std::string &group_name,
+bool MoveitStateAdapter::initialize(planning_scene_monitor::PlanningSceneMonitorPtr& psm, const std::string &group_name,
                                     const std::string &world_frame, const std::string &tcp_frame)
 {
-  robot_model_ptr_ = robot_model;
-  robot_state_.reset(new moveit::core::RobotState(robot_model_ptr_));
+  planning_scene_monitor_ = psm;
+  planning_scene_monitor_->startSceneMonitor();
+
+
+  robot_state_.reset(new moveit::core::RobotState(planning_scene_monitor_->getRobotModel()));
   robot_state_->setToDefaultValues();
-  joint_group_ = robot_model_ptr_->getJointModelGroup(group_name);
+  joint_group_ = planning_scene_monitor_->getRobotModel()->getJointModelGroup(group_name);
 
   // Assign robot parameters
   group_name_ = group_name;
@@ -138,18 +134,10 @@ bool MoveitStateAdapter::initialize(robot_model::RobotModelConstPtr robot_model,
     world_to_root_ = descartes_core::Frame(root_to_world.inverse());
   }
 
-  planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader_->getRobotDescription()));
   // Start the psm
-  setPlanningSceneMonitor(planning_scene_monitor_);
   return true;
 }
 
-void MoveitStateAdapter::setPlanningSceneMonitor(planning_scene_monitor::PlanningSceneMonitorPtr psm)
-{
-  planning_scene_monitor_ = psm;
-  planning_scene_monitor_->startSceneMonitor();
-}
-
 bool MoveitStateAdapter::getIK(const Eigen::Isometry3d& pose, const std::vector<double>& seed_state,
                                std::vector<double>& joint_pose) const
 {
@@ -193,9 +181,9 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
   double epsilon = 4 * joint_group_->getSolverInstance()->getSearchDiscretization();
   CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Utilizing an min. difference of %f between IK solutions", epsilon);
   joint_poses.clear();
+  getCurrentRobotState();
   for (size_t sample_iter = 0; sample_iter < seed_states_.size(); ++sample_iter)
   {
-    robot_state_->setJointGroupPositions(group_name_, seed_states_[sample_iter]);
     std::vector<double> joint_pose;
     if (getIK(pose, joint_pose))
     {
@@ -235,6 +223,7 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
         }
       }
     }
+    robot_state_->setJointGroupPositions(group_name_, seed_states_[sample_iter]);
   }
 
   CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Found %lu joint solutions out of %lu iterations", static_cast<unsigned long>(joint_poses.size()),
@@ -380,4 +369,22 @@ void MoveitStateAdapter::setState(const moveit::core::RobotState& state)
   planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)->setCurrentState(state);
 }
 
+bool MoveitStateAdapter::getCurrentRobotState() const
+{
+  planning_scene_monitor_->updateFrameTransforms();
+  std::unique_ptr<planning_scene_monitor::LockedPlanningSceneRO> ls(new planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_));
+
+  if (!(*ls))
+  {
+    CONSOLE_BRIDGE_logWarn("MoveitStateAdapter.getCurrentRobotState: Failed to get locked planning scene.");
+    return false;
+  }
+  else
+  {
+    *robot_state_ = (*ls)->getCurrentState();
+    robot_state_->update();
+    return true;
+  }
+}
+
 }  // descartes_moveit

From afac48805a5ba0b2131bb067fd7439d34de0904f Mon Sep 17 00:00:00 2001
From: Mike Lautman <mikeblautman@github.com>
Date: Thu, 28 Mar 2019 18:03:50 -0600
Subject: [PATCH 8/8] fixing bug with attached collision objects

---
 descartes_moveit/src/moveit_state_adapter.cpp | 11 ++++++-----
 descartes_moveit/src/seed_search.cpp          |  5 ++---
 2 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/descartes_moveit/src/moveit_state_adapter.cpp b/descartes_moveit/src/moveit_state_adapter.cpp
index c8429c67..3526e5b3 100644
--- a/descartes_moveit/src/moveit_state_adapter.cpp
+++ b/descartes_moveit/src/moveit_state_adapter.cpp
@@ -83,9 +83,9 @@ bool MoveitStateAdapter::initialize(planning_scene_monitor::PlanningSceneMonitor
   planning_scene_monitor_ = psm;
   planning_scene_monitor_->startSceneMonitor();
 
-
   robot_state_.reset(new moveit::core::RobotState(planning_scene_monitor_->getRobotModel()));
   robot_state_->setToDefaultValues();
+  getCurrentRobotState();
   joint_group_ = planning_scene_monitor_->getRobotModel()->getJointModelGroup(group_name);
 
   // Assign robot parameters
@@ -130,7 +130,7 @@ bool MoveitStateAdapter::initialize(planning_scene_monitor::PlanningSceneMonitor
               " transformed to world frame '%s'",
               __FUNCTION__, world_frame_.c_str(), model_frame.c_str(), world_frame_.c_str());
 
-    Eigen::Isometry3d root_to_world = toIsometry(robot_state_->getFrameTransform(world_frame_));
+    Eigen::Isometry3d root_to_world = Eigen::Isometry3d(robot_state_->getFrameTransform(world_frame_));
     world_to_root_ = descartes_core::Frame(root_to_world.inverse());
   }
 
@@ -150,7 +150,7 @@ bool MoveitStateAdapter::getIK(const Eigen::Isometry3d& pose, std::vector<double
   bool rtn = false;
 
   // transform to group base
-  Eigen::Isometry3d tool_pose = world_to_root_.frame * pose;
+  Eigen::Isometry3d tool_pose = Eigen::Isometry3d(world_to_root_.frame * pose);
 
   if (robot_state_->setFromIK(joint_group_, tool_pose, tool_frame_))
   {
@@ -181,7 +181,6 @@ bool MoveitStateAdapter::getAllIK(const Eigen::Isometry3d& pose, std::vector<std
   double epsilon = 4 * joint_group_->getSolverInstance()->getSearchDiscretization();
   CONSOLE_BRIDGE_logDebug("MoveitStateAdapter.getAllIK: Utilizing an min. difference of %f between IK solutions", epsilon);
   joint_poses.clear();
-  getCurrentRobotState();
   for (size_t sample_iter = 0; sample_iter < seed_states_.size(); ++sample_iter)
   {
     std::vector<double> joint_pose;
@@ -275,12 +274,13 @@ bool MoveitStateAdapter::isInLimits(const std::vector<double> &joint_pose) const
 bool MoveitStateAdapter::getFK(const std::vector<double>& joint_pose, Eigen::Isometry3d& pose) const
 {
   bool rtn = false;
+  getCurrentRobotState();
   robot_state_->setJointGroupPositions(group_name_, joint_pose);
   if (isValid(joint_pose))
   {
     if (robot_state_->knowsFrameTransform(tool_frame_))
     {
-      pose = toIsometry(world_to_root_.frame * robot_state_->getFrameTransform(tool_frame_));
+      pose = Eigen::Isometry3d(world_to_root_.frame * robot_state_->getFrameTransform(tool_frame_));
       //pose.
       rtn = true;
     }
@@ -366,6 +366,7 @@ void MoveitStateAdapter::setState(const moveit::core::RobotState& state)
                                                   "initialize()?");
   }
   *robot_state_ = state;
+  robot_state_->update();
   planning_scene_monitor::LockedPlanningSceneRW(planning_scene_monitor_)->setCurrentState(state);
 }
 
diff --git a/descartes_moveit/src/seed_search.cpp b/descartes_moveit/src/seed_search.cpp
index 4e29df19..0c856624 100644
--- a/descartes_moveit/src/seed_search.cpp
+++ b/descartes_moveit/src/seed_search.cpp
@@ -30,7 +30,7 @@ bool doFK(moveit::core::RobotState& state, const moveit::core::JointModelGroup*
     return false;
   }
 
-  result = toIsometry(state.getFrameTransform(tool));
+  result = Eigen::Isometry3d(state.getFrameTransform(tool));
   return true;
 }
 
@@ -41,11 +41,10 @@ bool doFK(moveit::core::RobotState& state, const moveit::core::JointModelGroup*
 bool doIK(moveit::core::RobotState& state, const moveit::core::JointModelGroup* group, const std::string& group_name,
           const std::string& tool, const Eigen::Isometry3d& pose, const JointConfig& seed, JointConfig& result)
 {
-  const static int N_ATTEMPTS = 1;
   const static double IK_TIMEOUT = 0.01;
 
   state.setJointGroupPositions(group_name, seed);
-  if (!state.setFromIK(group, pose, tool, N_ATTEMPTS, IK_TIMEOUT))
+  if (!state.setFromIK(group, pose, tool, IK_TIMEOUT))
   {
     return false;
   }