Fix floating-base Jacobian column switch bug, fix floating-base collision links

exotations/exotica_core_task_maps/include/exotica_core_task_maps/center_of_mass.h

@@ -41,9 +41,6 @@ namespace exotica
 class CenterOfMass : public TaskMap, public Instantiable<CenterOfMassInitializer>
 {
 public:
-    CenterOfMass();
-    virtual ~CenterOfMass();
-
     void AssignScene(ScenePtr scene) override;
 
     void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi) override;
@@ -64,6 +61,6 @@ class CenterOfMass : public TaskMap, public Instantiable<CenterOfMassInitializer
     bool enable_z_;
     int dim_;
 };
-}
+}  // namespace exotica
 
 #endif  // EXOTICA_CORE_TASK_MAPS_COM_H_

exotations/exotica_core_task_maps/include/exotica_core_task_maps/collision_distance.h

@@ -52,7 +52,8 @@ class CollisionDistance : public TaskMap, public Instantiable<CollisionDistanceI
 private:
     void Initialize();
 
-    std::vector<std::string> robot_links_;
+    std::vector<std::string> robot_joints_;
+    std::map<std::string, std::vector<std::string>> controlled_joint_to_collision_link_map_;
     bool check_self_collision_ = true;
     double robot_margin_;
     double world_margin_;

exotations/exotica_core_task_maps/include/exotica_core_task_maps/smooth_collision_distance.h

@@ -52,7 +52,8 @@ class SmoothCollisionDistance : public TaskMap, public Instantiable<SmoothCollis
 private:
     void Initialize();
 
-    std::vector<std::string> robot_links_;
+    std::vector<std::string> robot_joints_;
+    std::map<std::string, std::vector<std::string>> controlled_joint_to_collision_link_map_;
     double robot_margin_ = 0.0;
     double world_margin_ = 0.0;
     bool linear_ = false;
@@ -63,6 +64,6 @@ class SmoothCollisionDistance : public TaskMap, public Instantiable<SmoothCollis
 
     void Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef J, bool updateJacobian = true);
 };
-}
+}  // namespace exotica
 
 #endif  // EXOTICA_CORE_TASK_MAPS_SMOOTH_COLLISION_DISTANCE_H_

exotations/exotica_core_task_maps/src/center_of_mass.cpp

@@ -34,9 +34,6 @@ REGISTER_TASKMAP_TYPE("CenterOfMass", exotica::CenterOfMass);
 
 namespace exotica
 {
-CenterOfMass::CenterOfMass() = default;
-CenterOfMass::~CenterOfMass() = default;
-
 void CenterOfMass::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
 {
     if (phi.rows() != dim_) ThrowNamed("Wrong size of phi!");
@@ -58,7 +55,7 @@ void CenterOfMass::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi)
     com = com / M;
     for (int i = 0; i < dim_; ++i) phi(i) = com[i];
 
-    if (debug_)
+    if (debug_ && Server::IsRos())
     {
         com_marker_.pose.position.x = phi(0);
         com_marker_.pose.position.y = phi(1);
@@ -129,7 +126,7 @@ void CenterOfMass::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eig
     }
     for (int i = 0; i < dim_; ++i) phi(i) = com[i];
 
-    if (debug_)
+    if (debug_ && Server::IsRos())
     {
         com_marker_.pose.position.x = phi(0);
         com_marker_.pose.position.y = phi(1);
@@ -174,11 +171,8 @@ void CenterOfMass::Initialize()
         }
     }
 
-    if (debug_)
-    {
-        InitializeDebug();
-        HIGHLIGHT_NAMED("CenterOfMass", "Total model mass: " << mass_.sum() << " kg");
-    }
+    if (Server::IsRos()) InitializeDebug();
+    if (debug_) HIGHLIGHT_NAMED("CenterOfMass", "Total model mass: " << mass_.sum() << " kg");
 }
 
 void CenterOfMass::AssignScene(ScenePtr scene)
@@ -213,4 +207,4 @@ void CenterOfMass::InitializeDebug()
     com_links_pub_ = Server::Advertise<visualization_msgs::Marker>(object_name_ + "/com_links_marker", 1, true);
     com_pub_ = Server::Advertise<visualization_msgs::Marker>(object_name_ + "/com_marker", 1, true);
 }
-}
+}  // namespace exotica

exotations/exotica_core_task_maps/src/collision_distance.cpp

@@ -51,17 +51,17 @@ void CollisionDistance::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi
 
 void CollisionDistance::Update(Eigen::VectorXdRefConst x, Eigen::VectorXdRef phi, Eigen::MatrixXdRef J, bool updateJacobian)
 {
-    cscene_->UpdateCollisionObjectTransforms();
+    if (!scene_->AlwaysUpdatesCollisionScene())
+        cscene_->UpdateCollisionObjectTransforms();
 
     // For all robot links: Get all collision distances, sort by distance, and process the closest.
     for (int i = 0; i < dim_; ++i)
     {
-        // std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(scene_->getControlledLinkToCollisionLinkMap()[robot_links_[i]], check_self_collision_);  //, false);
-        std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(robot_links_[i], check_self_collision_);
+        std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(controlled_joint_to_collision_link_map_[robot_joints_[i]], check_self_collision_);
         if (proxies.size() == 0)
         {
-            phi(i) = 0;
-            J.row(i).setZero();
+            // phi(i) = 0;
+            // J.row(i).setZero();
             continue;
         }
 
@@ -111,8 +111,9 @@ void CollisionDistance::Initialize()
     robot_margin_ = parameters_.RobotMargin;
 
     // Get names of all controlled joints and their corresponding child links
-    robot_links_ = scene_->GetControlledLinkNames();
-    dim_ = static_cast<unsigned int>(robot_links_.size());
+    robot_joints_ = scene_->GetControlledJointNames();
+    controlled_joint_to_collision_link_map_ = scene_->GetControlledJointToCollisionLinkMap();
+    dim_ = static_cast<unsigned int>(robot_joints_.size());
     closest_proxies_.assign(dim_, CollisionProxy());
     if (debug_)
     {

exotations/exotica_core_task_maps/src/smooth_collision_distance.cpp

@@ -38,7 +38,7 @@ void SmoothCollisionDistance::Update(Eigen::VectorXdRefConst x,
 {
     if (phi.rows() != dim_) ThrowNamed("Wrong size of phi!");
     phi.setZero();
-    Eigen::MatrixXd J(dim_, robot_links_.size());
+    Eigen::MatrixXd J(dim_, robot_joints_.size());
     Update(x, phi, J, false);
 }
 
@@ -62,11 +62,10 @@ void SmoothCollisionDistance::Update(Eigen::VectorXdRefConst x,
 
     double& d = phi(0);
 
-    for (const auto& link : robot_links_)
+    for (const auto& joint : robot_joints_)
     {
         // Get all world collision links, then iterate through them
-        // std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(scene_->getControlledLinkToCollisionLinkMap()[link], check_self_collision_);
-        std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(link, check_self_collision_);
+        std::vector<CollisionProxy> proxies = cscene_->GetCollisionDistance(controlled_joint_to_collision_link_map_[joint], check_self_collision_);
 
         for (const auto& proxy : proxies)
         {
@@ -126,12 +125,13 @@ void SmoothCollisionDistance::Initialize()
 
     if (robot_margin_ == 0.0 || world_margin_ == 0.0) ThrowPretty("Setting the margin to zero is a bad idea. It will NaN.");
 
-    HIGHLIGHT_NAMED("Smooth Collision Distance",
-                    "World Margin: " << world_margin_ << " Robot Margin: " << robot_margin_ << "\t Linear: " << linear_);
+    if (debug_) HIGHLIGHT_NAMED("Smooth Collision Distance",
+                                "World Margin: " << world_margin_ << " Robot Margin: " << robot_margin_ << "\t Linear: " << linear_);
 
     // Get names of all controlled joints and their corresponding child links
-    robot_links_ = scene_->GetControlledLinkNames();
+    robot_joints_ = scene_->GetControlledJointNames();
+    controlled_joint_to_collision_link_map_ = scene_->GetControlledJointToCollisionLinkMap();
 }
 
 int SmoothCollisionDistance::TaskSpaceDim() { return dim_; }
-}
+}  // namespace exotica

exotica_core/include/exotica_core/kinematic_tree.h

@@ -184,14 +184,13 @@ class KinematicTree : public Uncopyable
     Eigen::MatrixXd Jdot(const KDL::Jacobian& jacobian);
 
     void ResetModel();
-    std::shared_ptr<KinematicElement> AddElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent = "", shapes::ShapeConstPtr shape = shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
-    std::shared_ptr<KinematicElement> AddEnvironmentElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent = "", shapes::ShapeConstPtr shape = shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
-    std::shared_ptr<KinematicElement> AddElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent, const std::string& shape_resource_path, Eigen::Vector3d scale = Eigen::Vector3d::Ones(), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
+    std::shared_ptr<KinematicElement> AddElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent = "", shapes::ShapeConstPtr shape = shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
+    std::shared_ptr<KinematicElement> AddEnvironmentElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent = "", shapes::ShapeConstPtr shape = shapes::ShapeConstPtr(nullptr), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
+    std::shared_ptr<KinematicElement> AddElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent, const std::string& shape_resource_path, Eigen::Vector3d scale = Eigen::Vector3d::Ones(), const KDL::RigidBodyInertia& inertia = KDL::RigidBodyInertia::Zero(), const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const std::vector<VisualElement>& visual = {}, bool is_controlled = false);
     void UpdateModel();
     void ChangeParent(const std::string& name, const std::string& parent, const KDL::Frame& pose, bool relative);
     int IsControlled(std::shared_ptr<KinematicElement> joint);
-    int IsControlled(std::string joint_name);
-    int IsControlledLink(std::string link_name);
+    int IsControlledLink(const std::string& link_name);
 
     Eigen::VectorXd GetModelState() const;
     std::map<std::string, double> GetModelStateMap() const;
@@ -235,7 +234,7 @@ class KinematicTree : public Uncopyable
 
 private:
     void BuildTree(const KDL::Tree& RobotKinematics);
-    void AddElement(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent);
+    void AddElementFromSegmentMapIterator(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent);
     void UpdateTree();
     void UpdateFK();
     void UpdateJ();

exotica_core/include/exotica_core/scene.h

@@ -165,15 +165,15 @@ class Scene : public Object, Uncopyable, public Instantiable<SceneInitializer>,
     bool AlwaysUpdatesCollisionScene() const { return force_collision_; }
     /// @brief Returns a map between a model link name and the names of associated collision links.
     /// @return Map between model links and all associated collision links.
-    std::map<std::string, std::vector<std::string>> GetModelLinkToCollisionLinkMap() const { return model_link_to_collision_link_map_; };
+    const std::map<std::string, std::vector<std::string>>& GetModelLinkToCollisionLinkMap() const { return model_link_to_collision_link_map_; };
     /// @brief Returns a map between a model link name and the KinematicElement of associated collision links.
     /// @return Map between model links and all the KinematicElements of the associated collision links.
-    std::map<std::string, std::vector<std::shared_ptr<KinematicElement>>> GetModelLinkToCollisionElementMap() const { return model_link_to_collision_element_map_; };
-    /// @brief Returns a map between controlled robot link names and associated collision link names. Here we consider all fixed links between controlled links as belonging to the previous controlled link (as if the collision links had been fused).
-    /// @return Map between controlled links and associated collision links.
-    std::map<std::string, std::vector<std::string>> GetControlledLinkToCollisionLinkMap() const { return controlled_link_to_collision_link_map_; };
+    const std::map<std::string, std::vector<std::shared_ptr<KinematicElement>>>& GetModelLinkToCollisionElementMap() const { return model_link_to_collision_element_map_; };
+    /// @brief Returns a map between controlled robot joint names and associated collision link names. Here we consider all fixed links between controlled links as belonging to the previous controlled joint (as if the collision links had been fused).
+    /// @return Map between controlled joints and associated collision links.
+    const std::map<std::string, std::vector<std::string>>& GetControlledJointToCollisionLinkMap() const { return controlled_joint_to_collision_link_map_; };
     /// @brief Returns world links that are to be excluded from collision checking.
-    std::set<std::string> get_world_links_to_exclude_from_collision_scene() const { return world_links_to_exclude_from_collision_scene_; }
+    const std::set<std::string>& get_world_links_to_exclude_from_collision_scene() const { return world_links_to_exclude_from_collision_scene_; }
 private:
     void UpdateInternalFrames(bool update_request = true);
 
@@ -213,8 +213,8 @@ class Scene : public Object, Uncopyable, public Instantiable<SceneInitializer>,
     std::map<std::string, std::vector<std::string>> model_link_to_collision_link_map_;
     std::map<std::string, std::vector<std::shared_ptr<KinematicElement>>> model_link_to_collision_element_map_;
 
-    /// \brief Mapping between controlled link name and collision links
-    std::map<std::string, std::vector<std::string>> controlled_link_to_collision_link_map_;
+    /// \brief Mapping between controlled joint name and collision links
+    std::map<std::string, std::vector<std::string>> controlled_joint_to_collision_link_map_;
 
     /// \brief List of robot links to be excluded from the collision scene
     std::set<std::string> robot_links_to_exclude_from_collision_scene_;

exotica_core/include/exotica_core/visualization_moveit.h

@@ -51,6 +51,6 @@ class VisualizationMoveIt : public Uncopyable
     ScenePtr scene_ = std::make_shared<Scene>(nullptr);
     ros::Publisher trajectory_pub_;
 };
-}
+}  // namespace exotica
 
 #endif  // EXOTICA_CORE_VISUALIZATION_MOVEIT_H_

exotica_core/src/kinematic_tree.cpp

@@ -190,6 +190,9 @@ void KinematicTree::BuildTree(const KDL::Tree& robot_kinematics)
         if (RotW != controlled_joints_names_.end()) controlled_joints_names_.erase(RotW);
         RotW = std::find(model_joints_names_.begin(), model_joints_names_.end(), root_joint->getVariableNames()[6]);
         if (RotW != model_joints_names_.end()) model_joints_names_.erase(RotW);
+
+        // NB: We do not want to swap the XYZ labels for the rotation joints
+        // to not change the order in which joint and model state are stored.
     }
     else if (root_joint->getType() == robot_model::JointModel::PLANAR)
     {
@@ -212,7 +215,7 @@ void KinematicTree::BuildTree(const KDL::Tree& robot_kinematics)
         ThrowPretty("Unsupported root joint type: " << root_joint->getTypeName());
     }
 
-    AddElement(robot_kinematics.getRootSegment(), *(model_tree_.end() - 1));
+    AddElementFromSegmentMapIterator(robot_kinematics.getRootSegment(), *(model_tree_.end() - 1));
 
     // Set root inertial
     if (root_joint->getType() == robot_model::JointModel::FIXED)
@@ -425,14 +428,14 @@ void KinematicTree::ChangeParent(const std::string& name, const std::string& par
     debug_scene_changed_ = true;
 }
 
-std::shared_ptr<KinematicElement> KinematicTree::AddEnvironmentElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent, shapes::ShapeConstPtr shape, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
+std::shared_ptr<KinematicElement> KinematicTree::AddEnvironmentElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent, shapes::ShapeConstPtr shape, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
 {
     std::shared_ptr<KinematicElement> element = AddElement(name, transform, parent, shape, inertia, color, visual, is_controlled);
     environment_tree_.push_back(element);
     return element;
 }
 
-std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent, const std::string& shape_resource_path, Eigen::Vector3d scale, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
+std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent, const std::string& shape_resource_path, Eigen::Vector3d scale, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
 {
     std::string shape_path(shape_resource_path);
     if (shape_path.empty())
@@ -460,7 +463,7 @@ std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& n
     return element;
 }
 
-std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& name, Eigen::Isometry3d& transform, const std::string& parent, shapes::ShapeConstPtr shape, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
+std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& name, const Eigen::Isometry3d& transform, const std::string& parent, shapes::ShapeConstPtr shape, const KDL::RigidBodyInertia& inertia, const Eigen::Vector4d& color, const std::vector<VisualElement>& visual, bool is_controlled)
 {
     std::shared_ptr<KinematicElement> parent_element;
     if (parent == "")
@@ -503,14 +506,14 @@ std::shared_ptr<KinematicElement> KinematicTree::AddElement(const std::string& n
     return new_element;
 }
 
-void KinematicTree::AddElement(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent)
+void KinematicTree::AddElementFromSegmentMapIterator(KDL::SegmentMap::const_iterator segment, std::shared_ptr<KinematicElement> parent)
 {
     std::shared_ptr<KinematicElement> new_element = std::make_shared<KinematicElement>(model_tree_.size(), parent, segment->second.segment);
     model_tree_.push_back(new_element);
     if (parent) parent->children.push_back(new_element);
     for (KDL::SegmentMap::const_iterator child : segment->second.children)
     {
-        AddElement(child, new_element);
+        AddElementFromSegmentMapIterator(child, new_element);
     }
 }
 
@@ -523,21 +526,30 @@ int KinematicTree::IsControlled(std::shared_ptr<KinematicElement> joint)
     return -1;
 }
 
-int KinematicTree::IsControlled(std::string joint_name)
+int KinematicTree::IsControlledLink(const std::string& link_name)
 {
-    for (int i = 0; i < controlled_joints_names_.size(); ++i)
+    try
     {
-        if (controlled_joints_names_[i] == joint_name) return i;
-    }
-    return -1;
-}
+        auto element = tree_map_[link_name].lock();
 
-int KinematicTree::IsControlledLink(std::string link_name)
-{
-    for (int i = 0; i < controlled_joints_.size(); ++i)
+        if (element && element->is_controlled)
+        {
+            return element->control_id;
+        }
+
+        while (element)
+        {
+            element = element->parent.lock();
+
+            if (element && element->is_controlled)
+            {
+                return element->control_id;
+            }
+        }
+    }
+    catch (const std::out_of_range& e)
     {
-        auto joint = controlled_joints_[i].lock();
-        if (joint->segment.getName() == link_name) return i;
+        return -1;
     }
     return -1;
 }
@@ -1188,4 +1200,4 @@ bool KinematicTree::DoesLinkWithNameExist(std::string name) const
     // Check whether it exists in TreeMap, which should encompass both EnvironmentTree and model_tree_
     return tree_map_.find(name) != tree_map_.end();
 }
-}
+}  // namespace exotica