scene.h

Go to the documentation of this file.

//
// Copyright (c) 2018, University of Edinburgh
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//  * Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//  * Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
//  * Neither the name of  nor the names of its contributors may be used to
//    endorse or promote products derived from this software without specific
//    prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#ifndef EXOTICA_CORE_SCENE_H_
#define EXOTICA_CORE_SCENE_H_

#include <fstream>
#include <functional>
#include <iostream>
#include <string>

#include <geometric_shapes/shapes.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/PlanningScene.h>

#include <exotica_core/collision_scene.h>
#include <exotica_core/kinematic_tree.h>
#include <exotica_core/object.h>
#include <exotica_core/property.h>
#include <exotica_core/tools/conversions.h>
#include <exotica_core/trajectory.h>

#include <exotica_core/scene_initializer.h>

namespace exotica
{
struct AttachedObject
{
    AttachedObject() = default;
    AttachedObject(std::string _parent) : parent(_parent) {}
    AttachedObject(std::string _parent, KDL::Frame _pose) : parent(_parent), pose(_pose) {}
    std::string parent = "";
    KDL::Frame pose;
};

#ifndef EXOTICA_CORE_DYNAMICS_SOLVER_H_
template <typename T, int NX, int NU>
class AbstractDynamicsSolver;
typedef AbstractDynamicsSolver<double, Eigen::Dynamic, Eigen::Dynamic> DynamicsSolver;
#endif

class Scene : public Object, Uncopyable, public Instantiable<SceneInitializer>, public std::enable_shared_from_this<Scene>
{
public:
    Scene(const std::string& name);
    Scene();
    virtual ~Scene();
    virtual void Instantiate(const SceneInitializer& init);
    void RequestKinematics(KinematicsRequest& request, std::function<void(std::shared_ptr<KinematicResponse>)> callback);
    const std::string& GetName() const;  // Deprecated - use GetObjectName
    void Update(Eigen::VectorXdRefConst x, double t = 0);

    const CollisionScenePtr& GetCollisionScene() const;

    std::shared_ptr<DynamicsSolver> GetDynamicsSolver() const;

    std::string GetRootFrameName();
    std::string GetRootJointName();

    exotica::KinematicTree& GetKinematicTree();
    std::vector<std::string> GetControlledJointNames();
    std::vector<std::string> GetModelJointNames();
    std::vector<std::string> GetControlledLinkNames();
    std::vector<std::string> GetModelLinkNames();
    Eigen::VectorXd GetControlledState();
    Eigen::VectorXd GetModelState();
    std::map<std::string, double> GetModelStateMap();
    std::map<std::string, std::weak_ptr<KinematicElement>> GetTreeMap();
    void SetModelState(Eigen::VectorXdRefConst x, double t = 0, bool update_traj = true);
    void SetModelState(const std::map<std::string, double>& x, double t = 0, bool update_traj = true);

    void AddTrajectoryFromFile(const std::string& link, const std::string& traj);
    void AddTrajectory(const std::string& link, const std::string& traj);
    void AddTrajectory(const std::string& link, std::shared_ptr<Trajectory> traj);
    std::shared_ptr<Trajectory> GetTrajectory(const std::string& link);
    void RemoveTrajectory(const std::string& link);

    void UpdateSceneFrames();
    void AttachObject(const std::string& name, const std::string& parent);
    void AttachObjectLocal(const std::string& name, const std::string& parent, const KDL::Frame& pose);
    void AttachObjectLocal(const std::string& name, const std::string& parent, const Eigen::VectorXd& pose);
    void DetachObject(const std::string& name);
    bool HasAttachedObject(const std::string& name);

    void AddObject(const std::string& name, const KDL::Frame& transform = KDL::Frame(), 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 bool update_collision_scene = true);

    void AddObject(const std::string& name, const KDL::Frame& transform = KDL::Frame(), const std::string& parent = "", const std::string& shape_resource_path = "", const 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 bool update_collision_scene = true);

    void AddObjectToEnvironment(const std::string& name, const KDL::Frame& transform = KDL::Frame(), shapes::ShapeConstPtr shape = nullptr, const Eigen::Vector4d& color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0), const bool update_collision_scene = true);

    void RemoveObject(const std::string& name);

    void UpdatePlanningSceneWorld(const moveit_msgs::PlanningSceneWorldConstPtr& world);

    void UpdatePlanningScene(const moveit_msgs::PlanningScene& scene);

    moveit_msgs::PlanningScene GetPlanningSceneMsg();

    void UpdateCollisionObjects();
    void UpdateTrajectoryGenerators(double t = 0);

    void PublishScene();
    void PublishProxies(const std::vector<CollisionProxy>& proxies);
    visualization_msgs::Marker ProxyToMarker(const std::vector<CollisionProxy>& proxies, const std::string& frame);
    void LoadScene(const std::string& scene, const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity(), bool update_collision_scene = true);
    void LoadScene(const std::string& scene, const KDL::Frame& offset = KDL::Frame(), bool update_collision_scene = true);
    void LoadSceneFile(const std::string& file_name, const Eigen::Isometry3d& offset = Eigen::Isometry3d::Identity(), bool update_collision_scene = true);
    void LoadSceneFile(const std::string& file_name, const KDL::Frame& offset = KDL::Frame(), bool update_collision_scene = true);
    std::string GetScene();
    void CleanScene();

    bool AlwaysUpdatesCollisionScene() const { return force_collision_; }
    const 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::shared_ptr<KinematicElement>>>& GetModelLinkToCollisionElementMap() const { return model_link_to_collision_element_map_; };
    const std::map<std::string, std::vector<std::string>>& GetControlledJointToCollisionLinkMap() const { return controlled_joint_to_collision_link_map_; };
    const std::set<std::string>& get_world_links_to_exclude_from_collision_scene() const { return world_links_to_exclude_from_collision_scene_; }
    int get_num_positions() const;
    int get_num_velocities() const;
    int get_num_controls() const;
    int get_num_state() const;
    int get_num_state_derivative() const;
    bool get_has_quaternion_floating_base() const;

private:
    bool has_quaternion_floating_base_ = false;  

    void UpdateInternalFrames(bool update_request = true);

    void UpdateMoveItPlanningScene();

    void LoadSceneFromStringStream(std::istream& in, const Eigen::Isometry3d& offset, bool update_collision_scene);

    exotica::KinematicTree kinematica_;

    CollisionScenePtr collision_scene_;

    std::shared_ptr<DynamicsSolver> dynamics_solver_ = std::shared_ptr<DynamicsSolver>(nullptr);

    int num_positions_ = 0;         
    int num_velocities_ = 0;        
    int num_controls_ = 0;          
    int num_state_ = 0;             
    int num_state_derivative_ = 0;  

    planning_scene::PlanningScenePtr ps_;

    ros::Publisher ps_pub_;
    ros::Publisher proxy_pub_;

    std::map<std::string, AttachedObject> attached_objects_;

    std::vector<std::shared_ptr<KinematicElement>> custom_links_;

    std::map<std::string, std::pair<std::weak_ptr<KinematicElement>, std::shared_ptr<Trajectory>>> trajectory_generators_;

    bool force_collision_;

    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_;

    std::map<std::string, std::vector<std::string>> controlled_joint_to_collision_link_map_;

    std::set<std::string> robot_links_to_exclude_from_collision_scene_;

    std::set<std::string> world_links_to_exclude_from_collision_scene_;

    KinematicsRequest kinematic_request_;
    std::shared_ptr<KinematicResponse> kinematic_solution_;
    std::function<void(std::shared_ptr<KinematicResponse>)> kinematic_request_callback_;
    bool request_needs_updating_;
};

typedef std::shared_ptr<Scene> ScenePtr;
}  // namespace exotica

#endif  // EXOTICA_CORE_SCENE_H_