diff --git a/dependencies.rosinstall b/dependencies.rosinstall
index 793e57ae3..d9eb3a401 100644
--- a/dependencies.rosinstall
+++ b/dependencies.rosinstall
@@ -5,11 +5,11 @@
 - git:
     local-name: tesseract
     uri: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.28.0
+    version: 0.28.4
 - git:
     local-name: trajopt
     uri: https://github.com/tesseract-robotics/trajopt.git
-    version: 0.28.0
+    version: 0.28.1
 - git:
     local-name: tesseract_planning
     uri: https://github.com/tesseract-robotics/tesseract_planning.git
@@ -17,7 +17,7 @@
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.28.0
+    version: 0.28.1
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git
diff --git a/dependencies_with_ext.rosinstall b/dependencies_with_ext.rosinstall
index b51647f1b..e3139751b 100644
--- a/dependencies_with_ext.rosinstall
+++ b/dependencies_with_ext.rosinstall
@@ -9,11 +9,11 @@
 - git:
     local-name: tesseract
     uri: https://github.com/tesseract-robotics/tesseract.git
-    version: 0.28.0
+    version: 0.28.4
 - git:
     local-name: trajopt
     uri: https://github.com/tesseract-robotics/trajopt_ros.git
-    version: 0.28.0
+    version: 0.28.1
 - git:
     local-name: tesseract_planning
     uri: https://github.com/tesseract-robotics/tesseract_planning.git
@@ -21,7 +21,7 @@
 - git:
     local-name: tesseract_qt
     uri: https://github.com/tesseract-robotics/tesseract_qt.git
-    version: 0.28.0
+    version: 0.28.1
 - git:
     local-name: descartes_light
     uri: https://github.com/swri-robotics/descartes_light.git
diff --git a/tesseract_monitoring/include/tesseract_monitoring/environment_monitor_interface.h b/tesseract_monitoring/include/tesseract_monitoring/environment_monitor_interface.h
index 87c9072e3..9992b0e2e 100644
--- a/tesseract_monitoring/include/tesseract_monitoring/environment_monitor_interface.h
+++ b/tesseract_monitoring/include/tesseract_monitoring/environment_monitor_interface.h
@@ -75,21 +75,33 @@ class ROSEnvironmentMonitorInterface : public tesseract_environment::Environment
   tesseract_scene_graph::SceneState getEnvironmentState(const std::string& monitor_namespace) const override final;
 
   bool setEnvironmentState(const std::string& monitor_namespace,
-                           const std::unordered_map<std::string, double>& joints) const override final;
+                           const std::unordered_map<std::string, double>& joints,
+                           const tesseract_common::TransformMap& floating_joints = {}) const override final;
   bool setEnvironmentState(const std::string& monitor_namespace,
                            const std::vector<std::string>& joint_names,
-                           const std::vector<double>& joint_values) const override final;
+                           const std::vector<double>& joint_values,
+                           const tesseract_common::TransformMap& floating_joints = {}) const override final;
   bool setEnvironmentState(const std::string& monitor_namespace,
                            const std::vector<std::string>& joint_names,
-                           const Eigen::Ref<const Eigen::VectorXd>& joint_values) const override final;
+                           const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                           const tesseract_common::TransformMap& floating_joints = {}) const override final;
+  bool setEnvironmentState(const std::string& monitor_namespace,
+                           const tesseract_common::TransformMap& floating_joints = {}) const override final;
 
   std::vector<std::string>
-  setEnvironmentState(const std::unordered_map<std::string, double>& joints) const override final;
-  std::vector<std::string> setEnvironmentState(const std::vector<std::string>& joint_names,
-                                               const std::vector<double>& joint_values) const override final;
+  setEnvironmentState(const std::unordered_map<std::string, double>& joints,
+                      const tesseract_common::TransformMap& floating_joints = {}) const override final;
+  std::vector<std::string>
+  setEnvironmentState(const std::vector<std::string>& joint_names,
+                      const std::vector<double>& joint_values,
+                      const tesseract_common::TransformMap& floating_joints = {}) const override final;
   std::vector<std::string>
   setEnvironmentState(const std::vector<std::string>& joint_names,
-                      const Eigen::Ref<const Eigen::VectorXd>& joint_values) const override final;
+                      const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                      const tesseract_common::TransformMap& floating_joints = {}) const override final;
+
+  std::vector<std::string>
+  setEnvironmentState(const tesseract_common::TransformMap& floating_joints = {}) const override final;
 
   std::unique_ptr<tesseract_environment::Environment>
   getEnvironment(const std::string& monitor_namespace) const override final;
diff --git a/tesseract_monitoring/src/environment_monitor.cpp b/tesseract_monitoring/src/environment_monitor.cpp
index d4c3e456b..9732a25f1 100644
--- a/tesseract_monitoring/src/environment_monitor.cpp
+++ b/tesseract_monitoring/src/environment_monitor.cpp
@@ -690,11 +690,12 @@ struct ROSEnvironmentMonitor::Implementation
       }
     }
 
-    if (!tesseract_rosutils::isMsgEmpty(env_msg->joint_state))
+    if (!tesseract_rosutils::isMsgEmpty(env_msg->joint_state) ||
+        !tesseract_rosutils::isMsgEmpty(env_msg->floating_joint_states))
     {
       if (last_robot_motion_time != env_msg->joint_state.header.stamp)
       {
-        tesseract_rosutils::processMsg(*parent->env_, env_msg->joint_state);
+        tesseract_rosutils::processMsg(*parent->env_, env_msg->joint_state, env_msg->floating_joint_states);
         last_robot_motion_time = env_msg->joint_state.header.stamp;
       }
     }
@@ -875,6 +876,12 @@ struct ROSEnvironmentMonitor::Implementation
         res.success = false;
         return false;
       }
+
+      if (!tesseract_rosutils::toMsg(res.floating_joint_states, state.floating_joints))
+      {
+        res.success = false;
+        return false;
+      }
     }
 
     res.success = true;
@@ -922,7 +929,6 @@ struct ROSEnvironmentMonitor::Implementation
         filtered_commands.push_back(cmd);
     }
 
-    std::string old_scene_name;
     if (!filtered_commands.empty())
       result = tesseract_rosutils::processMsg(env, filtered_commands);
 
@@ -930,7 +936,7 @@ struct ROSEnvironmentMonitor::Implementation
     {
       for (const auto& cmd : update_joint_state_commands)
       {
-        if (tesseract_rosutils::processMsg(env, cmd.joint_state))
+        if (tesseract_rosutils::processMsg(env, cmd.joint_state, cmd.floating_joint_states))
         {
           last_robot_motion_time = ros::Time::now();
         }
diff --git a/tesseract_monitoring/src/environment_monitor_interface.cpp b/tesseract_monitoring/src/environment_monitor_interface.cpp
index c734f5708..d111bcd0a 100644
--- a/tesseract_monitoring/src/environment_monitor_interface.cpp
+++ b/tesseract_monitoring/src/environment_monitor_interface.cpp
@@ -245,6 +245,7 @@ ROSEnvironmentMonitorInterface::getEnvironmentState(const std::string& monitor_n
 
   tesseract_scene_graph::SceneState env_state;
   tesseract_rosutils::fromMsg(env_state.joints, res.response.joint_states);
+  tesseract_rosutils::fromMsg(env_state.floating_joints, res.response.floating_joint_states);
   tesseract_rosutils::fromMsg(env_state.link_transforms, res.response.link_transforms);
   tesseract_rosutils::fromMsg(env_state.joint_transforms, res.response.joint_transforms);
 
@@ -252,17 +253,20 @@ ROSEnvironmentMonitorInterface::getEnvironmentState(const std::string& monitor_n
 }
 
 bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& monitor_namespace,
-                                                         const std::unordered_map<std::string, double>& joints) const
+                                                         const std::unordered_map<std::string, double>& joints,
+                                                         const tesseract_common::TransformMap& floating_joints) const
 {
   tesseract_msgs::EnvironmentCommand command;
   command.command = tesseract_msgs::EnvironmentCommand::UPDATE_JOINT_STATE;
   tesseract_rosutils::toMsg(command.joint_state, joints);
+  tesseract_rosutils::toMsg(command.floating_joint_states, floating_joints);
   return sendCommands(env_name_, monitor_namespace, { command });
 }
 
 bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& monitor_namespace,
                                                          const std::vector<std::string>& joint_names,
-                                                         const std::vector<double>& joint_values) const
+                                                         const std::vector<double>& joint_values,
+                                                         const tesseract_common::TransformMap& floating_joints) const
 {
   std::unordered_map<std::string, double> joints;
   for (std::size_t i = 0; i < joint_names.size(); ++i)
@@ -271,12 +275,14 @@ bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& moni
   tesseract_msgs::EnvironmentCommand command;
   command.command = tesseract_msgs::EnvironmentCommand::UPDATE_JOINT_STATE;
   tesseract_rosutils::toMsg(command.joint_state, joints);
+  tesseract_rosutils::toMsg(command.floating_joint_states, floating_joints);
   return sendCommands(env_name_, monitor_namespace, { command });
 }
 
 bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& monitor_namespace,
                                                          const std::vector<std::string>& joint_names,
-                                                         const Eigen::Ref<const Eigen::VectorXd>& joint_values) const
+                                                         const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                                         const tesseract_common::TransformMap& floating_joints) const
 {
   std::unordered_map<std::string, double> joints;
   for (std::size_t i = 0; i < joint_names.size(); ++i)
@@ -285,16 +291,27 @@ bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& moni
   tesseract_msgs::EnvironmentCommand command;
   command.command = tesseract_msgs::EnvironmentCommand::UPDATE_JOINT_STATE;
   tesseract_rosutils::toMsg(command.joint_state, joints);
+  tesseract_rosutils::toMsg(command.floating_joint_states, floating_joints);
+  return sendCommands(env_name_, monitor_namespace, { command });
+}
+
+bool ROSEnvironmentMonitorInterface::setEnvironmentState(const std::string& monitor_namespace,
+                                                         const tesseract_common::TransformMap& floating_joints) const
+{
+  tesseract_msgs::EnvironmentCommand command;
+  command.command = tesseract_msgs::EnvironmentCommand::UPDATE_JOINT_STATE;
+  tesseract_rosutils::toMsg(command.floating_joint_states, floating_joints);
   return sendCommands(env_name_, monitor_namespace, { command });
 }
 
 std::vector<std::string>
-ROSEnvironmentMonitorInterface::setEnvironmentState(const std::unordered_map<std::string, double>& joints) const
+ROSEnvironmentMonitorInterface::setEnvironmentState(const std::unordered_map<std::string, double>& joints,
+                                                    const tesseract_common::TransformMap& floating_joints) const
 {
   std::vector<std::string> failed_namespace;
   failed_namespace.reserve(ns_.size());
   for (const auto& ns : ns_)
-    if (!setEnvironmentState(ns, joints))
+    if (!setEnvironmentState(ns, joints, floating_joints))
       failed_namespace.push_back(ns);
 
   return failed_namespace;
@@ -302,12 +319,13 @@ ROSEnvironmentMonitorInterface::setEnvironmentState(const std::unordered_map<std
 
 std::vector<std::string>
 ROSEnvironmentMonitorInterface::setEnvironmentState(const std::vector<std::string>& joint_names,
-                                                    const std::vector<double>& joint_values) const
+                                                    const std::vector<double>& joint_values,
+                                                    const tesseract_common::TransformMap& floating_joints) const
 {
   std::vector<std::string> failed_namespace;
   failed_namespace.reserve(ns_.size());
   for (const auto& ns : ns_)
-    if (!setEnvironmentState(ns, joint_names, joint_values))
+    if (!setEnvironmentState(ns, joint_names, joint_values, floating_joints))
       failed_namespace.push_back(ns);
 
   return failed_namespace;
@@ -315,12 +333,25 @@ ROSEnvironmentMonitorInterface::setEnvironmentState(const std::vector<std::strin
 
 std::vector<std::string>
 ROSEnvironmentMonitorInterface::setEnvironmentState(const std::vector<std::string>& joint_names,
-                                                    const Eigen::Ref<const Eigen::VectorXd>& joint_values) const
+                                                    const Eigen::Ref<const Eigen::VectorXd>& joint_values,
+                                                    const tesseract_common::TransformMap& floating_joints) const
+{
+  std::vector<std::string> failed_namespace;
+  failed_namespace.reserve(ns_.size());
+  for (const auto& ns : ns_)
+    if (!setEnvironmentState(ns, joint_names, joint_values, floating_joints))
+      failed_namespace.push_back(ns);
+
+  return failed_namespace;
+}
+
+std::vector<std::string>
+ROSEnvironmentMonitorInterface::setEnvironmentState(const tesseract_common::TransformMap& floating_joints) const
 {
   std::vector<std::string> failed_namespace;
   failed_namespace.reserve(ns_.size());
   for (const auto& ns : ns_)
-    if (!setEnvironmentState(ns, joint_names, joint_values))
+    if (!setEnvironmentState(ns, floating_joints))
       failed_namespace.push_back(ns);
 
   return failed_namespace;
diff --git a/tesseract_msgs/msg/Environment.msg b/tesseract_msgs/msg/Environment.msg
index 93776c256..dca3308bf 100644
--- a/tesseract_msgs/msg/Environment.msg
+++ b/tesseract_msgs/msg/Environment.msg
@@ -1,2 +1,3 @@
 tesseract_msgs/EnvironmentCommand[] command_history
 sensor_msgs/JointState joint_states
+tesseract_msgs/TransformMap floating_joint_states
diff --git a/tesseract_msgs/msg/EnvironmentCommand.msg b/tesseract_msgs/msg/EnvironmentCommand.msg
index b713b1589..553b00221 100644
--- a/tesseract_msgs/msg/EnvironmentCommand.msg
+++ b/tesseract_msgs/msg/EnvironmentCommand.msg
@@ -72,6 +72,7 @@ string remove_allowed_collision_link
 
 # UPDATE_JOINT_STATE
 sensor_msgs/JointState joint_state
+tesseract_msgs/TransformMap floating_joint_states
 
 # ADD_SCENE_GRAPH Command
 tesseract_msgs/SceneGraph scene_graph
diff --git a/tesseract_msgs/msg/EnvironmentState.msg b/tesseract_msgs/msg/EnvironmentState.msg
index 9725aa74a..af243ac5a 100644
--- a/tesseract_msgs/msg/EnvironmentState.msg
+++ b/tesseract_msgs/msg/EnvironmentState.msg
@@ -3,3 +3,4 @@ uint64 revision # This is the revision number so others know when it changes.
 
 # This message contains information about the state, i.e. the positions of its joints and links
 sensor_msgs/JointState joint_state
+tesseract_msgs/TransformMap floating_joint_states
diff --git a/tesseract_msgs/srv/GetEnvironmentInformation.srv b/tesseract_msgs/srv/GetEnvironmentInformation.srv
index 3ba1cfc53..9939e7674 100644
--- a/tesseract_msgs/srv/GetEnvironmentInformation.srv
+++ b/tesseract_msgs/srv/GetEnvironmentInformation.srv
@@ -39,3 +39,4 @@ tesseract_msgs/TransformMap joint_transforms
 tesseract_msgs/AllowedCollisionEntry[] allowed_collision_matrix
 tesseract_msgs/KinematicsInformation kinematics_information
 sensor_msgs/JointState joint_states
+tesseract_msgs/TransformMap floating_joint_states
diff --git a/tesseract_qt_ros/src/widgets/environment_monitor_widget.cpp b/tesseract_qt_ros/src/widgets/environment_monitor_widget.cpp
index 73d6cdee6..15a7b3203 100644
--- a/tesseract_qt_ros/src/widgets/environment_monitor_widget.cpp
+++ b/tesseract_qt_ros/src/widgets/environment_monitor_widget.cpp
@@ -265,13 +265,15 @@ void EnvironmentMonitorWidget::snapshotCallback(const tesseract_msgs::Environmen
 
   tesseract_environment::Commands commands = tesseract_rosutils::fromMsg(msg->command_history);
   std::unordered_map<std::string, double> jv;
+  tesseract_common::TransformMap fjv;
   tesseract_rosutils::fromMsg(jv, msg->joint_states);
+  tesseract_rosutils::fromMsg(fjv, msg->floating_joint_states);
   auto env = std::make_shared<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
   env->setResourceLocator(locator);
   if (env->init(commands))
   {
-    env->setState(jv);
+    env->setState(jv, fjv);
 
     if (data_->monitor != nullptr)
       data_->monitor->shutdown();
diff --git a/tesseract_rosutils/include/tesseract_rosutils/utils.h b/tesseract_rosutils/include/tesseract_rosutils/utils.h
index 23e2253f8..dab50afed 100644
--- a/tesseract_rosutils/include/tesseract_rosutils/utils.h
+++ b/tesseract_rosutils/include/tesseract_rosutils/utils.h
@@ -46,6 +46,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_collision/core/fwd.h>
 #include <tesseract_motion_planners/core/fwd.h>
 #include <tesseract_task_composer/core/fwd.h>
+#include <tesseract_msgs/TransformMap.h>
 
 #include <ros/message_forward.h>
 namespace tesseract_msgs
@@ -86,7 +87,6 @@ ROS_DECLARE_MESSAGE(StringPair)
 ROS_DECLARE_MESSAGE(Environment)
 ROS_DECLARE_MESSAGE(EnvironmentState)
 ROS_DECLARE_MESSAGE(Trajectory)
-ROS_DECLARE_MESSAGE(TransformMap)
 ROS_DECLARE_MESSAGE(VisualGeometry)
 ROS_DECLARE_MESSAGE(JointTrajectory)
 ROS_DECLARE_MESSAGE(PluginInfo)
@@ -135,6 +135,7 @@ class ROSResourceLocator : public tesseract_common::ResourceLocator
 };
 
 bool isMsgEmpty(const sensor_msgs::JointState& msg);
+bool isMsgEmpty(const tesseract_msgs::TransformMap& msg);
 
 bool isIdentical(const tesseract_geometry::Geometry& shape1, const tesseract_geometry::Geometry& shape2);
 
@@ -263,7 +264,9 @@ void toMsg(tesseract_msgs::JointTrajectory& traj_msg, const tesseract_common::Jo
  */
 tesseract_common::JointTrajectory fromMsg(const tesseract_msgs::JointTrajectory& traj_msg);
 
-bool processMsg(tesseract_environment::Environment& env, const sensor_msgs::JointState& joint_state_msg);
+bool processMsg(tesseract_environment::Environment& env,
+                const sensor_msgs::JointState& joint_state_msg,
+                const tesseract_msgs::TransformMap& floating_joint_state_msg = {});
 
 /**
  * @brief Apply the provided commands to the environment
diff --git a/tesseract_rosutils/src/utils.cpp b/tesseract_rosutils/src/utils.cpp
index ec6ce6621..df6b42027 100644
--- a/tesseract_rosutils/src/utils.cpp
+++ b/tesseract_rosutils/src/utils.cpp
@@ -64,7 +64,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <tesseract_msgs/Environment.h>
 #include <tesseract_msgs/EnvironmentState.h>
 #include <tesseract_msgs/Trajectory.h>
-#include <tesseract_msgs/TransformMap.h>
 #include <tesseract_msgs/VisualGeometry.h>
 #include <tesseract_msgs/PluginInfo.h>
 #include <tesseract_msgs/TaskComposerKey.h>
@@ -145,6 +144,8 @@ bool isMsgEmpty(const sensor_msgs::JointState& msg)
   return msg.name.empty() && msg.position.empty() && msg.velocity.empty() && msg.effort.empty();
 }
 
+bool isMsgEmpty(const tesseract_msgs::TransformMap& msg) { return (msg.names.empty() || msg.transforms.empty()); }
+
 bool isIdentical(const tesseract_scene_graph::Visual& /*visual1*/, const tesseract_scene_graph::Visual& /*visual2*/)
 {
   assert(false);
@@ -1579,7 +1580,11 @@ void toMsg(tesseract_msgs::EnvironmentState& state_msg,
   state_msg.revision = static_cast<unsigned long>(env.getRevision());
 
   if (include_joint_states)
-    toMsg(state_msg.joint_state, env.getState().joints);
+  {
+    tesseract_scene_graph::SceneState env_state = env.getState();
+    toMsg(state_msg.joint_state, env_state.joints);
+    toMsg(state_msg.floating_joint_states, env_state.floating_joints);
+  }
 }
 
 void toMsg(const tesseract_msgs::EnvironmentStatePtr& state_msg, const tesseract_environment::Environment& env)
@@ -1645,19 +1650,31 @@ tesseract_common::JointTrajectory fromMsg(const tesseract_msgs::JointTrajectory&
   return trajectory;
 }
 
-bool processMsg(tesseract_environment::Environment& env, const sensor_msgs::JointState& joint_state_msg)
+bool processMsg(tesseract_environment::Environment& env,
+                const sensor_msgs::JointState& joint_state_msg,
+                const tesseract_msgs::TransformMap& floating_joint_state_msg)
 {
-  if (!isMsgEmpty(joint_state_msg))
+  const bool joint_state_msg_empty = isMsgEmpty(joint_state_msg);
+  const bool floating_joint_state_msg_empty = isMsgEmpty(floating_joint_state_msg);
+
+  if (joint_state_msg_empty && floating_joint_state_msg_empty)
+    return false;
+
+  tesseract_common::TransformMap floating_joints;
+  if (!floating_joint_state_msg_empty)
+    fromMsg(floating_joints, floating_joint_state_msg);
+
+  if (!joint_state_msg_empty)
   {
     std::unordered_map<std::string, double> joints;
     for (auto i = 0u; i < joint_state_msg.name.size(); ++i)
-    {
       joints[joint_state_msg.name[i]] = joint_state_msg.position[i];
-    }
-    env.setState(joints);
+    env.setState(joints, floating_joints);
     return true;
   }
-  return false;
+
+  env.setState(floating_joints);
+  return true;
 }
 
 bool processMsg(tesseract_environment::Environment& env,
@@ -2125,14 +2142,13 @@ bool toMsg(tesseract_msgs::Environment& environment_msg,
            bool include_joint_states)
 {
   if (include_joint_states)
-    toMsg(environment_msg.joint_states, env.getState().joints);
-
-  if (!tesseract_rosutils::toMsg(environment_msg.command_history, env.getCommandHistory(), 0))
   {
-    return false;
+    tesseract_scene_graph::SceneState env_state = env.getState();
+    toMsg(environment_msg.joint_states, env_state.joints);
+    toMsg(environment_msg.floating_joint_states, env_state.floating_joints);
   }
 
-  if (!tesseract_rosutils::toMsg(environment_msg.joint_states, env.getState().joints))
+  if (!tesseract_rosutils::toMsg(environment_msg.command_history, env.getCommandHistory(), 0))
   {
     return false;
   }
@@ -2176,7 +2192,12 @@ std::unique_ptr<tesseract_environment::Environment> fromMsg(const tesseract_msgs
     ROS_ERROR_STREAM("fromMsg(Environment): Failed to get joint states");
     return nullptr;
   }
-  env->setState(env_state->joints);
+  if (!tesseract_rosutils::fromMsg(env_state->floating_joints, environment_msg.floating_joint_states))
+  {
+    ROS_ERROR_STREAM("fromMsg(Environment): Failed to get floating joint states");
+    return nullptr;
+  }
+  env->setState(env_state->joints, env_state->floating_joints);
 
   return env;
 }
diff --git a/tesseract_rviz/src/environment_monitor_properties.cpp b/tesseract_rviz/src/environment_monitor_properties.cpp
index c01af64f1..15a1a51fa 100644
--- a/tesseract_rviz/src/environment_monitor_properties.cpp
+++ b/tesseract_rviz/src/environment_monitor_properties.cpp
@@ -289,13 +289,15 @@ void EnvironmentMonitorProperties::snapshotCallback(const tesseract_msgs::Enviro
 
   tesseract_environment::Commands commands = tesseract_rosutils::fromMsg(msg->command_history);
   std::unordered_map<std::string, double> jv;
+  tesseract_common::TransformMap fjv;
   tesseract_rosutils::fromMsg(jv, msg->joint_states);
+  tesseract_rosutils::fromMsg(fjv, msg->floating_joint_states);
   auto env = std::make_shared<tesseract_environment::Environment>();
   auto locator = std::make_shared<tesseract_rosutils::ROSResourceLocator>();
   env->setResourceLocator(locator);
   if (env->init(commands))
   {
-    env->setState(jv);
+    env->setState(jv, fjv);
 
     if (data_->monitor != nullptr)
       data_->monitor->shutdown();