From 4c1bfa47703787113e8caad5bf13370be1e26afd Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Sun, 20 Dec 2020 16:25:02 +0800 Subject: [PATCH 1/8] Add initial TPE support for door plugin --- .../building_ignition_plugins/src/door.cpp | 183 +++++++++++++++--- .../building_sim_common/door_common.hpp | 30 ++- 2 files changed, 183 insertions(+), 30 deletions(-) diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index 4f1c67f4..ef881f2a 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -6,6 +6,10 @@ #include #include #include +#include +#include +#include +#include #include @@ -29,14 +33,31 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin public ISystemPreUpdate { private: + struct DoorElement { + Entity link_entity; + Entity joint_entity; + ignition::math::Pose3d orig_position; + double orig_rotation; + }; + rclcpp::Node::SharedPtr _ros_node; - std::unordered_map _joints; + std::unordered_map _link_entities; + std::unordered_map _joint_entities; + Entity _en; + + std::unordered_map orig_positions; + std::string _physics_plugin_name; + ignition::math::Pose3d orig_pos; + double orig_rot = 0.0; + bool pos_set = false; + int axis = 0; + double vel_cmd = 0.0; std::shared_ptr _door_common = nullptr; bool _initialized = false; - void create_entity_components(Entity entity, EntityComponentManager& ecm) + void create_joint_components(Entity entity, EntityComponentManager& ecm) { if (!ecm.EntityHasComponentType(entity, components::JointPosition().TypeId())) @@ -49,6 +70,25 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin ecm.CreateComponent(entity, components::JointVelocityCmd({0})); } + void create_link_components(Entity entity, EntityComponentManager& ecm) + { + if (!ecm.EntityHasComponentType(entity, + components::LinearVelocityCmd().TypeId())) + ecm.CreateComponent(entity, components::LinearVelocityCmd()); + if (!ecm.EntityHasComponentType(entity, + components::AngularVelocityCmd().TypeId())) + ecm.CreateComponent(entity, components::AngularVelocityCmd()); + if (!ecm.EntityHasComponentType(entity, + components::Pose().TypeId())) + ecm.CreateComponent(entity, components::Pose()); + } + + bool is_tpe_plugin(const std::string& plugin_name) + { + static const std::string tpe_plugin = "ignition-physics-tpe-plugin"; + return plugin_name == tpe_plugin; + } + public: DoorPlugin() { @@ -63,6 +103,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin //_ros_node = gazebo_ros::Node::Get(sdf); // TODO get properties from sdf instead of hardcoded (will fail for multiple instantiations) // TODO proper rclcpp init (only once and pass args) + _en = entity; auto model = Model(entity); char const** argv = NULL; std::string name; @@ -86,18 +127,29 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin if (!_door_common) return; - for (const auto& joint_name : _door_common->joint_names()) + for (const auto& door_elem : _door_common->_doors) { - const auto joint = model.JointByName(ecm, joint_name); + const auto link = model.LinkByName(ecm, door_elem.second.link_name); + if (link == kNullEntity) + { + RCLCPP_ERROR(_ros_node->get_logger(), + " -- Door model is missing the link [%s]", + door_elem.second.link_name.c_str()); + return; + } + create_link_components(link, ecm); + _link_entities.insert({door_elem.second.joint_name, link}); + + const auto joint = model.JointByName(ecm, door_elem.second.joint_name); if (!joint) { RCLCPP_ERROR(_ros_node->get_logger(), - " -- Model is missing the joint [%s]", - joint_name.c_str()); + " -- Door model is missing the joint [%s]", + door_elem.second.joint_name.c_str()); return; } - create_entity_components(joint, ecm); - _joints.insert({joint_name, joint}); + create_joint_components(joint, ecm); + _joint_entities.insert({door_elem.second.joint_name, joint}); } _initialized = true; @@ -114,36 +166,123 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin if (!_initialized) return; + if(!pos_set){ + + orig_pos = ecm.Component(_en)->Data(); + orig_rot = ecm.Component(_en)->Data().Yaw(); + + Entity parent = _en; + while(ecm.ParentEntity(parent)){ + parent = ecm.ParentEntity(parent); + } + if (ecm.EntityHasComponentType(parent, + components::PhysicsEnginePlugin().TypeId())){ + _physics_plugin_name = ecm.Component(parent)->Data(); + } + pos_set = true; + } + double t = (std::chrono::duration_cast(info.simTime). count()) * 1e-9; // Create DoorUpdateRequest std::vector requests; - for (const auto& joint : _joints) + for (const auto& door : _door_common->_doors) { + Entity link = _link_entities[door.first]; DoorCommon::DoorUpdateRequest request; - request.joint_name = joint.first; - request.position = ecm.Component( - joint.second)->Data()[0]; - request.velocity = ecm.Component( - joint.second)->Data()[0]; + request.joint_name = door.first; + + if(is_tpe_plugin(_physics_plugin_name)) // No joint features support, use velocity commands instead. May need axis too + { + const DoorCommon::DoorElement& door_elem = door.second; + constexpr double eps = 0.01; + if(door_elem.door_type == "SwingDoor" || door_elem.door_type == "DoubleSwingDoor") + { + // In the event that Yaw angle of the door moves past -Pi rads, it experiences a discontinuous jump from -Pi to Pi (likewise when the Yaw angle moves past Pi) + // We need to take this jump into account when calculating the relative orientation of the door w.r.t to its original orientation + if(door_elem.closed_position > door_elem.open_position) + { + // Yaw may go past -Pi rads when opening, and experience discontinuous jump to +Pi. For e.g. when original angle is -(7/8)Pi, opening the door by Pi/2 rads would + // push it past -Pi rads. + // If closed position (0 rads) is larger than open position, then the relative orientation should theoretically never exceed 0, + //which is how we identify when it has experienced a discontinuous jump + request.position = ecm.Component(link)->Data().Yaw() - orig_rot > eps ? + -3.14 - fmod(ecm.Component(link)->Data().Yaw() - orig_rot,3.14) : ecm.Component(link)->Data().Yaw() - orig_rot; + } + else // Yaw may go past +180, and experience discontinuous jump to -180 + { + request.position = ecm.Component(link)->Data().Yaw() - orig_rot < -eps ? + 3.14 + abs(-3.14 - (ecm.Component(link)->Data().Yaw() - orig_rot)) : ecm.Component(link)->Data().Yaw() - orig_rot; + } + } + else if(door_elem.door_type == "SlidingDoor" || door_elem.door_type == "DoubleSlidingDoor") + { + ignition::math::Pose3d curr_pos = ecm.Component(link)->Data(); + ignition::math::Vector3d displacement = curr_pos.CoordPositionSub(orig_pos); + //std::cout << "diff: " << displacement.Length() << std::endl; + request.position = displacement.Length(); + } + else + { + continue; + } + //std::cout << "pos: " << request.position << std::endl; + //std::cout << "result vel: " << vel_cmd << std::endl; + request.velocity = vel_cmd; + } + else // Default Physics Engine with Joint Features support + { + Entity joint = _joint_entities[door.first]; + request.position = ecm.Component( + joint)->Data()[0]; + request.velocity = ecm.Component( + joint)->Data()[0]; + } + requests.push_back(request); } + // Get and apply motions to the joints auto results = _door_common->update(t, requests); - - // Apply motions to the joints for (const auto& result : results) { - const auto it = _joints.find(result.joint_name); - assert(it != _joints.end()); - auto vel_cmd = ecm.Component( - it->second); - vel_cmd->Data()[0] = result.velocity; + Entity link = _link_entities[result.joint_name]; + const auto it = _door_common->_doors.find(result.joint_name); + assert(it != _door_common->_doors.end()); + + //to get door element section + if(is_tpe_plugin(_physics_plugin_name)) + { + vel_cmd = result.velocity; + const DoorCommon::DoorElement& door_elem = it->second; + if(door_elem.door_type == "SwingDoor" || door_elem.door_type == "DoubleSwingDoor") + { + ecm.Component(link)->Data() = ignition::math::Vector3d(0,-0.5*vel_cmd,0); + ecm.Component(link)->Data() = ignition::math::Vector3d(0,0,vel_cmd); + } + else if(door_elem.door_type == "SlidingDoor") + { + ecm.Component(link)->Data() = ignition::math::Vector3d(vel_cmd,0,0); + } + else + { + continue; + } + //std::cout << "velocity: " << result.velocity << std::endl; + //std::cout << "Door Pose: " << ecm.Component(_en)->Data() << std::endl; + //std::cout << "Link Pose: " << ecm.Component(link)->Data() << std::endl; + } + else // Default Physics Engine with Joint Features support + { + Entity joint = _joint_entities[result.joint_name]; + auto joint_vel_cmd = ecm.Component( + joint); + joint_vel_cmd->Data()[0] = result.velocity; + } } } - }; IGNITION_ADD_PLUGIN( diff --git a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp index c7ae844d..b4138ffe 100644 --- a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp +++ b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp @@ -72,10 +72,11 @@ class DoorCommon std::vector update(const double time, const std::vector& request); -private: - struct DoorElement { + std::string door_type; + std::string link_name; + std::string joint_name; double closed_position; double open_position; double current_position; @@ -84,10 +85,16 @@ class DoorCommon DoorElement() {} DoorElement( + const std::string& door_type, + const std::string& link_name, + const std::string& joint_name, const double lower_limit, const double upper_limit, const bool flip_direction = false) - : current_position(0.0), + : door_type(door_type), + link_name(link_name), + joint_name(joint_name), + current_position(0.0), current_velocity(0.0) { if (flip_direction) @@ -105,6 +112,10 @@ class DoorCommon // Map joint name to its DoorElement using Doors = std::unordered_map; + // Map of joint_name and corresponding DoorElement + Doors _doors; + +private: DoorMode requested_mode() const; @@ -139,9 +150,6 @@ class DoorCommon double _last_pub_time = ((double) std::rand()) / ((double) (RAND_MAX)); bool _initialized = false; - - // Map of joint_name and corresponding DoorElement - Doors _doors; }; template @@ -209,18 +217,24 @@ std::shared_ptr DoorCommon::make( if (it != joint_names.end()) { auto element = joint_sdf_clone; + + std::string link_name; + get_sdf_param_required( + element, "child", link_name); + get_element_required(joint_sdf_clone, "axis", element); get_element_required(element, "limit", element); double lower_limit = -1.57; double upper_limit = 0.0; get_sdf_param_if_available(element, "lower", lower_limit); get_sdf_param_if_available(element, "upper", upper_limit); + DoorCommon::DoorElement door_element; if (joint_name == right_door_joint_name) door_element = - DoorCommon::DoorElement{lower_limit, upper_limit, true}; + DoorCommon::DoorElement{door_type, link_name, joint_name, lower_limit, upper_limit, true}; else if (joint_name == left_door_joint_name) - door_element = DoorCommon::DoorElement{lower_limit, upper_limit}; + door_element = DoorCommon::DoorElement{door_type, link_name, joint_name, lower_limit, upper_limit}; doors.insert({joint_name, door_element}); } }; From 1bd5ca7c179858c87f9c55370277437d562b9efc Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Wed, 23 Dec 2020 14:17:18 +0800 Subject: [PATCH 2/8] Fix linear cmds for door rotations with TPE Plugin Direction of Linear Vel Cmd is relative to coordinate frame defined by original pose of the link + its current pose. --- .../building_ignition_plugins/src/door.cpp | 74 +++++++++++------- .../building_sim_common/door_common.hpp | 75 ++++++++++++++----- 2 files changed, 101 insertions(+), 48 deletions(-) diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index ef881f2a..b191b3a5 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -33,25 +33,16 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin public ISystemPreUpdate { private: - struct DoorElement { - Entity link_entity; - Entity joint_entity; - ignition::math::Pose3d orig_position; - double orig_rotation; - }; - rclcpp::Node::SharedPtr _ros_node; std::unordered_map _link_entities; std::unordered_map _joint_entities; Entity _en; - std::unordered_map orig_positions; + std::unordered_map orig_positions; + std::unordered_map orig_rotations; + std::unordered_map vel_cmds; std::string _physics_plugin_name; - ignition::math::Pose3d orig_pos; - double orig_rot = 0.0; bool pos_set = false; - int axis = 0; - double vel_cmd = 0.0; std::shared_ptr _door_common = nullptr; @@ -151,7 +142,12 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin create_joint_components(joint, ecm); _joint_entities.insert({door_elem.second.joint_name, joint}); } - + /*if (!ecm.EntityHasComponentType(_en, + components::LinearVelocityCmd().TypeId())) + ecm.CreateComponent(_en, components::LinearVelocityCmd()); + if (!ecm.EntityHasComponentType(_en, + components::AngularVelocityCmd().TypeId())) + ecm.CreateComponent(_en, components::AngularVelocityCmd());*/ _initialized = true; RCLCPP_INFO(_ros_node->get_logger(), @@ -167,9 +163,11 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin return; if(!pos_set){ - - orig_pos = ecm.Component(_en)->Data(); - orig_rot = ecm.Component(_en)->Data().Yaw(); + for (const auto& link_entity : _link_entities) + { + orig_positions[link_entity.first] = ecm.Component(link_entity.second)->Data() + ecm.Component(_en)->Data(); + orig_rotations[link_entity.first] = ecm.Component(link_entity.second)->Data().Yaw() + ecm.Component(_en)->Data().Yaw(); + } Entity parent = _en; while(ecm.ParentEntity(parent)){ @@ -198,6 +196,14 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin { const DoorCommon::DoorElement& door_elem = door.second; constexpr double eps = 0.01; + double orig_rot = orig_rotations[door.first]; + double curr_rot = ecm.Component(link)->Data().Yaw() + ecm.Component(_en)->Data().Yaw(); + ignition::math::Pose3d orig_pos = orig_positions[door.first]; + ignition::math::Pose3d curr_pos = ecm.Component(link)->Data() + ecm.Component(_en)->Data(); + //std::cout << "velocity: " << result.velocity << std::endl; + //std::cout << "Door Pose: " << ecm.Component(_en)->Data() << std::endl; + //std::cout << "Link Pose: " << curr_pos << std::endl; + //std::cout << request.joint_name << " original pos: " << orig_pos << " current pos: " << curr_pos << std::endl; if(door_elem.door_type == "SwingDoor" || door_elem.door_type == "DoubleSwingDoor") { // In the event that Yaw angle of the door moves past -Pi rads, it experiences a discontinuous jump from -Pi to Pi (likewise when the Yaw angle moves past Pi) @@ -208,18 +214,17 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin // push it past -Pi rads. // If closed position (0 rads) is larger than open position, then the relative orientation should theoretically never exceed 0, //which is how we identify when it has experienced a discontinuous jump - request.position = ecm.Component(link)->Data().Yaw() - orig_rot > eps ? - -3.14 - fmod(ecm.Component(link)->Data().Yaw() - orig_rot,3.14) : ecm.Component(link)->Data().Yaw() - orig_rot; + request.position = curr_rot - orig_rot > eps ? + -3.14 - fmod(curr_rot - orig_rot,3.14) : curr_rot - orig_rot; } else // Yaw may go past +180, and experience discontinuous jump to -180 { - request.position = ecm.Component(link)->Data().Yaw() - orig_rot < -eps ? - 3.14 + abs(-3.14 - (ecm.Component(link)->Data().Yaw() - orig_rot)) : ecm.Component(link)->Data().Yaw() - orig_rot; + request.position = curr_rot - orig_rot < -eps ? + 3.14 + abs(-3.14 - (curr_rot - orig_rot)) : curr_rot - orig_rot; } } else if(door_elem.door_type == "SlidingDoor" || door_elem.door_type == "DoubleSlidingDoor") { - ignition::math::Pose3d curr_pos = ecm.Component(link)->Data(); ignition::math::Vector3d displacement = curr_pos.CoordPositionSub(orig_pos); //std::cout << "diff: " << displacement.Length() << std::endl; request.position = displacement.Length(); @@ -228,9 +233,11 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin { continue; } - //std::cout << "pos: " << request.position << std::endl; + if (door_elem.door_type == "DoubleSwingDoor"){ + std::cout << door.first << " pos: " << request.position << std::endl; + } //std::cout << "result vel: " << vel_cmd << std::endl; - request.velocity = vel_cmd; + request.velocity = vel_cmds[door.first]; } else // Default Physics Engine with Joint Features support { @@ -255,11 +262,25 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin //to get door element section if(is_tpe_plugin(_physics_plugin_name)) { - vel_cmd = result.velocity; + vel_cmds[result.joint_name] = result.velocity; + double vel_cmd = result.velocity; const DoorCommon::DoorElement& door_elem = it->second; if(door_elem.door_type == "SwingDoor" || door_elem.door_type == "DoubleSwingDoor") { - ecm.Component(link)->Data() = ignition::math::Vector3d(0,-0.5*vel_cmd,0); + double curr_rot = /*ecm.Component(link)->Data().Yaw()*/ + ecm.Component(_en)->Data().Yaw() - orig_rotations[result.joint_name]; + double x_cmd = cos(curr_rot) * vel_cmd * (door_elem.length / 2.0); + double y_cmd = sin(curr_rot) * vel_cmd * (door_elem.length / 2.0); + + double x_rot_cmd = -y_cmd; + double y_rot_cmd = x_cmd; + /*if (vel_cmd < 0){ + x_rot_cmd *= -1; + y_rot_cmd *= -1; + } + if (door_elem.door_type == "DoubleSwingDoor" && std::abs(x_cmd) > 0.00001 || std::abs(y_cmd) > 0.0001){ + std::cout << result.joint_name << " yaw: " << curr_rot << " x cmd: " << x_rot_cmd << " y cmd: " << y_rot_cmd << std::endl; + }*/ + ecm.Component(link)->Data() = ignition::math::Vector3d(x_rot_cmd,y_rot_cmd,0); ecm.Component(link)->Data() = ignition::math::Vector3d(0,0,vel_cmd); } else if(door_elem.door_type == "SlidingDoor") @@ -270,9 +291,6 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin { continue; } - //std::cout << "velocity: " << result.velocity << std::endl; - //std::cout << "Door Pose: " << ecm.Component(_en)->Data() << std::endl; - //std::cout << "Link Pose: " << ecm.Component(link)->Data() << std::endl; } else // Default Physics Engine with Joint Features support { diff --git a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp index b4138ffe..89f97382 100644 --- a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp +++ b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp @@ -77,6 +77,7 @@ class DoorCommon std::string door_type; std::string link_name; std::string joint_name; + double length; double closed_position; double open_position; double current_position; @@ -88,12 +89,14 @@ class DoorCommon const std::string& door_type, const std::string& link_name, const std::string& joint_name, + const double length, const double lower_limit, const double upper_limit, const bool flip_direction = false) : door_type(door_type), link_name(link_name), joint_name(joint_name), + length(length), current_position(0.0), current_velocity(0.0) { @@ -207,6 +210,23 @@ std::shared_ptr DoorCommon::make( Doors doors; + // Get the joint limits from parent sdf + auto parent = sdf->GetParent(); + if (!parent) + { + RCLCPP_ERROR(node->get_logger(), + "Unable to access parent sdf to retrieve joint limits"); + return nullptr; + } + + auto joint_element = parent->GetElement("joint"); + if (!joint_element) + { + RCLCPP_ERROR(node->get_logger(), + "Parent sdf missing required joint element"); + return nullptr; + } + auto extract_door = [&](SdfPtrT& joint_sdf) { auto joint_sdf_clone = joint_sdf->Clone(); @@ -218,10 +238,42 @@ std::shared_ptr DoorCommon::make( { auto element = joint_sdf_clone; + // Get name of the link associated with the joint std::string link_name; get_sdf_param_required( element, "child", link_name); + // Get the link's dimensions + double length = 0.0; + auto link = parent->GetElement("link"); + std::string link_search_name; + while(link && get_sdf_attribute_required( + link, "name", link_search_name)) + { + if(link_search_name == link_name) + { + auto visual = link->GetElement("visual"); + if (visual) + { + auto geometry = visual->GetElement("geometry"); + if (geometry) + { + auto box = geometry->GetElement("box"); + if (box) + { + get_sdf_param_required(box, "size", length); + } + } + } + break; + } + else + { + link = link->GetNextElement("link"); + } + }; + + // Get joint parameters get_element_required(joint_sdf_clone, "axis", element); get_element_required(element, "limit", element); double lower_limit = -1.57; @@ -229,34 +281,17 @@ std::shared_ptr DoorCommon::make( get_sdf_param_if_available(element, "lower", lower_limit); get_sdf_param_if_available(element, "upper", upper_limit); + // Create DoorElement corresponding to link+joint pair DoorCommon::DoorElement door_element; if (joint_name == right_door_joint_name) door_element = - DoorCommon::DoorElement{door_type, link_name, joint_name, lower_limit, upper_limit, true}; + DoorCommon::DoorElement{door_type, link_name, joint_name, length, lower_limit, upper_limit, true}; else if (joint_name == left_door_joint_name) - door_element = DoorCommon::DoorElement{door_type, link_name, joint_name, lower_limit, upper_limit}; + door_element = DoorCommon::DoorElement{door_type, link_name, joint_name, length, lower_limit, upper_limit}; doors.insert({joint_name, door_element}); } }; - // Get the joint limits from parent sdf - auto parent = sdf->GetParent(); - if (!parent) - { - RCLCPP_ERROR(node->get_logger(), - "Unable to access parent sdf to retrieve joint limits"); - return nullptr; - } - - auto joint_element = parent->GetElement("joint"); - if (!joint_element) - { - RCLCPP_ERROR(node->get_logger(), - "Parent sdf missing required joint element"); - return nullptr; - } - - extract_door(joint_element); // Find next joint element if present while (joint_element) { From 57555c8934ef42a64513fc13655e182ff90e62c5 Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Wed, 23 Dec 2020 16:24:45 +0800 Subject: [PATCH 3/8] Remove extraneous code and add comments --- .../building_ignition_plugins/src/door.cpp | 91 +++++++++++-------- 1 file changed, 51 insertions(+), 40 deletions(-) diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index b191b3a5..3a835646 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -120,6 +120,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin for (const auto& door_elem : _door_common->_doors) { + // For Trivial Physics Engine (TPE) const auto link = model.LinkByName(ecm, door_elem.second.link_name); if (link == kNullEntity) { @@ -131,6 +132,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin create_link_components(link, ecm); _link_entities.insert({door_elem.second.joint_name, link}); + // For default physics engine const auto joint = model.JointByName(ecm, door_elem.second.joint_name); if (!joint) { @@ -142,12 +144,6 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin create_joint_components(joint, ecm); _joint_entities.insert({door_elem.second.joint_name, joint}); } - /*if (!ecm.EntityHasComponentType(_en, - components::LinearVelocityCmd().TypeId())) - ecm.CreateComponent(_en, components::LinearVelocityCmd()); - if (!ecm.EntityHasComponentType(_en, - components::AngularVelocityCmd().TypeId())) - ecm.CreateComponent(_en, components::AngularVelocityCmd());*/ _initialized = true; RCLCPP_INFO(_ros_node->get_logger(), @@ -165,8 +161,10 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin if(!pos_set){ for (const auto& link_entity : _link_entities) { - orig_positions[link_entity.first] = ecm.Component(link_entity.second)->Data() + ecm.Component(_en)->Data(); - orig_rotations[link_entity.first] = ecm.Component(link_entity.second)->Data().Yaw() + ecm.Component(_en)->Data().Yaw(); + orig_positions[link_entity.first] = + ecm.Component(link_entity.second)->Data() + + ecm.Component(_en)->Data(); + orig_rotations[link_entity.first] = orig_positions[link_entity.first].Yaw(); } Entity parent = _en; @@ -184,7 +182,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin (std::chrono::duration_cast(info.simTime). count()) * 1e-9; - // Create DoorUpdateRequest + // Determine current angular position and velocity of the door elements std::vector requests; for (const auto& door : _door_common->_doors) { @@ -192,30 +190,33 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin DoorCommon::DoorUpdateRequest request; request.joint_name = door.first; - if(is_tpe_plugin(_physics_plugin_name)) // No joint features support, use velocity commands instead. May need axis too + // No joint features support, look at link pose instead + if(is_tpe_plugin(_physics_plugin_name)) { const DoorCommon::DoorElement& door_elem = door.second; - constexpr double eps = 0.01; double orig_rot = orig_rotations[door.first]; - double curr_rot = ecm.Component(link)->Data().Yaw() + ecm.Component(_en)->Data().Yaw(); + double curr_rot = ecm.Component(link)->Data().Yaw() + + ecm.Component(_en)->Data().Yaw(); ignition::math::Pose3d orig_pos = orig_positions[door.first]; - ignition::math::Pose3d curr_pos = ecm.Component(link)->Data() + ecm.Component(_en)->Data(); - //std::cout << "velocity: " << result.velocity << std::endl; - //std::cout << "Door Pose: " << ecm.Component(_en)->Data() << std::endl; - //std::cout << "Link Pose: " << curr_pos << std::endl; - //std::cout << request.joint_name << " original pos: " << orig_pos << " current pos: " << curr_pos << std::endl; + ignition::math::Pose3d curr_pos = ecm.Component(link)->Data() + + ecm.Component(_en)->Data(); + constexpr double eps = 0.01; + if(door_elem.door_type == "SwingDoor" || door_elem.door_type == "DoubleSwingDoor") { - // In the event that Yaw angle of the door moves past -Pi rads, it experiences a discontinuous jump from -Pi to Pi (likewise when the Yaw angle moves past Pi) - // We need to take this jump into account when calculating the relative orientation of the door w.r.t to its original orientation + // In the event that Yaw angle of the door moves past -Pi rads, it experiences + // a discontinuous jump from -Pi to Pi (vice-versa when the Yaw angle moves past Pi). + // We need to take this jump into account when calculating the relative orientation + // of the door w.r.t to its original orientation. if(door_elem.closed_position > door_elem.open_position) { - // Yaw may go past -Pi rads when opening, and experience discontinuous jump to +Pi. For e.g. when original angle is -(7/8)Pi, opening the door by Pi/2 rads would - // push it past -Pi rads. - // If closed position (0 rads) is larger than open position, then the relative orientation should theoretically never exceed 0, - //which is how we identify when it has experienced a discontinuous jump + // Yaw may go past -Pi rads when opening, and experience discontinuous jump to +Pi. + // For e.g. when original angle is -(7/8)Pi, opening the door by Pi/2 rads would + // push it past -Pi rads. If closed position (0 rads) is larger than open position, + // then the relative orientation should theoretically never exceed 0, + // which is how we identify when it has experienced a discontinuous jump. request.position = curr_rot - orig_rot > eps ? - -3.14 - fmod(curr_rot - orig_rot,3.14) : curr_rot - orig_rot; + -3.14 - fmod(curr_rot - orig_rot, 3.14) : curr_rot - orig_rot; } else // Yaw may go past +180, and experience discontinuous jump to -180 { @@ -226,17 +227,12 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin else if(door_elem.door_type == "SlidingDoor" || door_elem.door_type == "DoubleSlidingDoor") { ignition::math::Vector3d displacement = curr_pos.CoordPositionSub(orig_pos); - //std::cout << "diff: " << displacement.Length() << std::endl; request.position = displacement.Length(); } else { continue; } - if (door_elem.door_type == "DoubleSwingDoor"){ - std::cout << door.first << " pos: " << request.position << std::endl; - } - //std::cout << "result vel: " << vel_cmd << std::endl; request.velocity = vel_cmds[door.first]; } else // Default Physics Engine with Joint Features support @@ -251,7 +247,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin requests.push_back(request); } - // Get and apply motions to the joints + // Get and apply motions to the joints/links auto results = _door_common->update(t, requests); for (const auto& result : results) { @@ -259,7 +255,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin const auto it = _door_common->_doors.find(result.joint_name); assert(it != _door_common->_doors.end()); - //to get door element section + // No joint features support, use velocity commands to mimic joint motion if(is_tpe_plugin(_physics_plugin_name)) { vel_cmds[result.joint_name] = result.velocity; @@ -267,23 +263,38 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin const DoorCommon::DoorElement& door_elem = it->second; if(door_elem.door_type == "SwingDoor" || door_elem.door_type == "DoubleSwingDoor") { - double curr_rot = /*ecm.Component(link)->Data().Yaw()*/ + ecm.Component(_en)->Data().Yaw() - orig_rotations[result.joint_name]; - double x_cmd = cos(curr_rot) * vel_cmd * (door_elem.length / 2.0); - double y_cmd = sin(curr_rot) * vel_cmd * (door_elem.length / 2.0); - + // The reference frame axes of the Linear Velocity Cmds at any point t = axes defined + // by the link's global pose (link yaw + model yaw) at time t0, rotated by the change in + // yaw component of the link since then. + // Note: This is not always equal to the global reference frame's axes rotated by the + // link's global yaw. In some cases, the link may rotate after time t0, but the change in + // its yaw may equal 0 if the change is applied to the parent model instead. + + // Calculate the door link orientation, theta_local, relative to Linear Velocity cmds reference frame + // theta_global = (current_link_yaw + current_model_yaw) - (original_link_yaw + original_model_yaw) + // theta_local = theta_global - (current_link_yaw - original_link_yaw) + // theta_local = current_model_yaw - (original_link_yaw + original_model_yaw) (assuming original link yaw = 0) + double theta_local = ecm.Component(_en)->Data().Yaw() - orig_rotations[result.joint_name]; + + // Given the rotation of the door, calculate a vector parallel to the door length, with magnitude 1/2 * vel_cmd + double x_cmd = cos(theta_local) * vel_cmd * (door_elem.length / 2.0); + double y_cmd = sin(theta_local) * vel_cmd * (door_elem.length / 2.0); + // Rotate the vector to calculate the normal to the door length. The Linear Velocity + // Cmd should be applied normal to the door double x_rot_cmd = -y_cmd; double y_rot_cmd = x_cmd; - /*if (vel_cmd < 0){ + // Flip direction of velocity based on door hinge (left/right). + // Assumes closed position is closest to 0, i.e. along original axis of the joint + if (door_elem.closed_position > door_elem.open_position) + { x_rot_cmd *= -1; y_rot_cmd *= -1; } - if (door_elem.door_type == "DoubleSwingDoor" && std::abs(x_cmd) > 0.00001 || std::abs(y_cmd) > 0.0001){ - std::cout << result.joint_name << " yaw: " << curr_rot << " x cmd: " << x_rot_cmd << " y cmd: " << y_rot_cmd << std::endl; - }*/ + ecm.Component(link)->Data() = ignition::math::Vector3d(x_rot_cmd,y_rot_cmd,0); ecm.Component(link)->Data() = ignition::math::Vector3d(0,0,vel_cmd); } - else if(door_elem.door_type == "SlidingDoor") + else if(door_elem.door_type == "SlidingDoor" || door_elem.door_type == "DoubleSlidingDoor") { ecm.Component(link)->Data() = ignition::math::Vector3d(vel_cmd,0,0); } From 33b54e61b410960b77b9c8c772a90a1649a4b7de Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Thu, 24 Dec 2020 13:09:03 +0800 Subject: [PATCH 4/8] Fix position calculation for sliding door and rename `door_type` to `type` Ensures calculated position can be negative as well --- .../building_ignition_plugins/src/door.cpp | 13 +++++++++---- .../include/building_sim_common/door_common.hpp | 8 ++++---- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index 3a835646..c9d2bb9d 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -159,6 +159,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin return; if(!pos_set){ + // Code later on assumes that each link is originally at its closed position for (const auto& link_entity : _link_entities) { orig_positions[link_entity.first] = @@ -202,7 +203,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin + ecm.Component(_en)->Data(); constexpr double eps = 0.01; - if(door_elem.door_type == "SwingDoor" || door_elem.door_type == "DoubleSwingDoor") + if(door_elem.type == "SwingDoor" || door_elem.type == "DoubleSwingDoor") { // In the event that Yaw angle of the door moves past -Pi rads, it experiences // a discontinuous jump from -Pi to Pi (vice-versa when the Yaw angle moves past Pi). @@ -224,10 +225,14 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin 3.14 + abs(-3.14 - (curr_rot - orig_rot)) : curr_rot - orig_rot; } } - else if(door_elem.door_type == "SlidingDoor" || door_elem.door_type == "DoubleSlidingDoor") + else if(door_elem.type == "SlidingDoor" || door_elem.type == "DoubleSlidingDoor") { ignition::math::Vector3d displacement = curr_pos.CoordPositionSub(orig_pos); request.position = displacement.Length(); + if (door_elem.open_position < door_elem.closed_position){ + // At any point the link's position must be negative relative to the closed pose + request.position *= -1; + } } else { @@ -261,7 +266,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin vel_cmds[result.joint_name] = result.velocity; double vel_cmd = result.velocity; const DoorCommon::DoorElement& door_elem = it->second; - if(door_elem.door_type == "SwingDoor" || door_elem.door_type == "DoubleSwingDoor") + if(door_elem.type == "SwingDoor" || door_elem.type == "DoubleSwingDoor") { // The reference frame axes of the Linear Velocity Cmds at any point t = axes defined // by the link's global pose (link yaw + model yaw) at time t0, rotated by the change in @@ -294,7 +299,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin ecm.Component(link)->Data() = ignition::math::Vector3d(x_rot_cmd,y_rot_cmd,0); ecm.Component(link)->Data() = ignition::math::Vector3d(0,0,vel_cmd); } - else if(door_elem.door_type == "SlidingDoor" || door_elem.door_type == "DoubleSlidingDoor") + else if(door_elem.type == "SlidingDoor" || door_elem.type == "DoubleSlidingDoor") { ecm.Component(link)->Data() = ignition::math::Vector3d(vel_cmd,0,0); } diff --git a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp index 89f97382..f8b220a3 100644 --- a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp +++ b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp @@ -74,7 +74,7 @@ class DoorCommon struct DoorElement { - std::string door_type; + std::string type; std::string link_name; std::string joint_name; double length; @@ -86,14 +86,14 @@ class DoorCommon DoorElement() {} DoorElement( - const std::string& door_type, + const std::string& type, const std::string& link_name, const std::string& joint_name, const double length, const double lower_limit, const double upper_limit, const bool flip_direction = false) - : door_type(door_type), + : type(type), link_name(link_name), joint_name(joint_name), length(length), @@ -243,7 +243,7 @@ std::shared_ptr DoorCommon::make( get_sdf_param_required( element, "child", link_name); - // Get the link's dimensions + // Find link and get its dimensions double length = 0.0; auto link = parent->GetElement("link"); std::string link_search_name; From 805df7e99d093cf33bb9b66493fc67491cebf588 Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Thu, 24 Dec 2020 15:07:45 +0800 Subject: [PATCH 5/8] Combine disparate unordered_map value fields into new DoorElementIgnition struct --- .../building_ignition_plugins/src/door.cpp | 78 +++++++++++-------- 1 file changed, 45 insertions(+), 33 deletions(-) diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index c9d2bb9d..057ec21f 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -33,14 +34,20 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin public ISystemPreUpdate { private: + struct DoorElementIgnition + { + Entity link_entity; + Entity joint_entity; + sdf::JointType joint_type; + ignition::math::Pose3d orig_position; + double orig_rotation; + double vel_cmd; + }; + rclcpp::Node::SharedPtr _ros_node; - std::unordered_map _link_entities; - std::unordered_map _joint_entities; + // Map from joint_name of each door section to ignition properties for that section + std::unordered_map _doors_ign; Entity _en; - - std::unordered_map orig_positions; - std::unordered_map orig_rotations; - std::unordered_map vel_cmds; std::string _physics_plugin_name; bool pos_set = false; @@ -59,6 +66,9 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin if (!ecm.EntityHasComponentType(entity, components::JointVelocityCmd().TypeId())) ecm.CreateComponent(entity, components::JointVelocityCmd({0})); + if (!ecm.EntityHasComponentType(entity, + components::JointType().TypeId())) + ecm.CreateComponent(entity, components::JointType()); } void create_link_components(Entity entity, EntityComponentManager& ecm) @@ -120,6 +130,9 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin for (const auto& door_elem : _door_common->_doors) { + auto door_ign_it = _doors_ign.insert( + {door_elem.second.joint_name, DoorElementIgnition()}).first; + // For Trivial Physics Engine (TPE) const auto link = model.LinkByName(ecm, door_elem.second.link_name); if (link == kNullEntity) @@ -130,7 +143,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin return; } create_link_components(link, ecm); - _link_entities.insert({door_elem.second.joint_name, link}); + door_ign_it->second.link_entity = link; // For default physics engine const auto joint = model.JointByName(ecm, door_elem.second.joint_name); @@ -142,7 +155,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin return; } create_joint_components(joint, ecm); - _joint_entities.insert({door_elem.second.joint_name, joint}); + door_ign_it->second.joint_entity = joint; } _initialized = true; @@ -160,12 +173,12 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin if(!pos_set){ // Code later on assumes that each link is originally at its closed position - for (const auto& link_entity : _link_entities) + for (auto& [door_name, door_ign] : _doors_ign) { - orig_positions[link_entity.first] = - ecm.Component(link_entity.second)->Data() + door_ign.orig_position = + ecm.Component(door_ign.link_entity)->Data() + ecm.Component(_en)->Data(); - orig_rotations[link_entity.first] = orig_positions[link_entity.first].Yaw(); + door_ign.orig_rotation = door_ign.orig_position.Yaw(); } Entity parent = _en; @@ -185,31 +198,31 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin // Determine current angular position and velocity of the door elements std::vector requests; - for (const auto& door : _door_common->_doors) + for (const auto& [door_name, door] : _door_common->_doors) { - Entity link = _link_entities[door.first]; + const DoorElementIgnition& door_ign = _doors_ign[door_name]; + Entity link = door_ign.link_entity; DoorCommon::DoorUpdateRequest request; - request.joint_name = door.first; + request.joint_name = door_name; // No joint features support, look at link pose instead if(is_tpe_plugin(_physics_plugin_name)) { - const DoorCommon::DoorElement& door_elem = door.second; - double orig_rot = orig_rotations[door.first]; - double curr_rot = ecm.Component(link)->Data().Yaw() + const double orig_rot = door_ign.orig_rotation; + const double curr_rot = ecm.Component(link)->Data().Yaw() + ecm.Component(_en)->Data().Yaw(); - ignition::math::Pose3d orig_pos = orig_positions[door.first]; - ignition::math::Pose3d curr_pos = ecm.Component(link)->Data() + const ignition::math::Pose3d orig_pos = door_ign.orig_position; + const ignition::math::Pose3d curr_pos = ecm.Component(link)->Data() + ecm.Component(_en)->Data(); constexpr double eps = 0.01; - if(door_elem.type == "SwingDoor" || door_elem.type == "DoubleSwingDoor") + if(door.type == "SwingDoor" || door.type == "DoubleSwingDoor") { // In the event that Yaw angle of the door moves past -Pi rads, it experiences // a discontinuous jump from -Pi to Pi (vice-versa when the Yaw angle moves past Pi). // We need to take this jump into account when calculating the relative orientation // of the door w.r.t to its original orientation. - if(door_elem.closed_position > door_elem.open_position) + if(door.closed_position > door.open_position) { // Yaw may go past -Pi rads when opening, and experience discontinuous jump to +Pi. // For e.g. when original angle is -(7/8)Pi, opening the door by Pi/2 rads would @@ -225,11 +238,11 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin 3.14 + abs(-3.14 - (curr_rot - orig_rot)) : curr_rot - orig_rot; } } - else if(door_elem.type == "SlidingDoor" || door_elem.type == "DoubleSlidingDoor") + else if(door.type == "SlidingDoor" || door.type == "DoubleSlidingDoor") { ignition::math::Vector3d displacement = curr_pos.CoordPositionSub(orig_pos); request.position = displacement.Length(); - if (door_elem.open_position < door_elem.closed_position){ + if (door.open_position < door.closed_position){ // At any point the link's position must be negative relative to the closed pose request.position *= -1; } @@ -238,15 +251,14 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin { continue; } - request.velocity = vel_cmds[door.first]; + request.velocity = door_ign.vel_cmd; } else // Default Physics Engine with Joint Features support { - Entity joint = _joint_entities[door.first]; request.position = ecm.Component( - joint)->Data()[0]; + door_ign.joint_entity)->Data()[0]; request.velocity = ecm.Component( - joint)->Data()[0]; + door_ign.joint_entity)->Data()[0]; } requests.push_back(request); @@ -256,14 +268,15 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin auto results = _door_common->update(t, requests); for (const auto& result : results) { - Entity link = _link_entities[result.joint_name]; + DoorElementIgnition& door_ign = _doors_ign[result.joint_name]; + const Entity link = door_ign.link_entity; const auto it = _door_common->_doors.find(result.joint_name); assert(it != _door_common->_doors.end()); // No joint features support, use velocity commands to mimic joint motion if(is_tpe_plugin(_physics_plugin_name)) { - vel_cmds[result.joint_name] = result.velocity; + door_ign.vel_cmd = result.velocity; double vel_cmd = result.velocity; const DoorCommon::DoorElement& door_elem = it->second; if(door_elem.type == "SwingDoor" || door_elem.type == "DoubleSwingDoor") @@ -279,7 +292,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin // theta_global = (current_link_yaw + current_model_yaw) - (original_link_yaw + original_model_yaw) // theta_local = theta_global - (current_link_yaw - original_link_yaw) // theta_local = current_model_yaw - (original_link_yaw + original_model_yaw) (assuming original link yaw = 0) - double theta_local = ecm.Component(_en)->Data().Yaw() - orig_rotations[result.joint_name]; + double theta_local = ecm.Component(_en)->Data().Yaw() - door_ign.orig_rotation; // Given the rotation of the door, calculate a vector parallel to the door length, with magnitude 1/2 * vel_cmd double x_cmd = cos(theta_local) * vel_cmd * (door_elem.length / 2.0); @@ -310,9 +323,8 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin } else // Default Physics Engine with Joint Features support { - Entity joint = _joint_entities[result.joint_name]; auto joint_vel_cmd = ecm.Component( - joint); + door_ign.joint_entity); joint_vel_cmd->Data()[0] = result.velocity; } } From 0d994ecf87cfe14c69e57fc7bded19d71aa0daab Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Thu, 24 Dec 2020 15:31:41 +0800 Subject: [PATCH 6/8] Base TPE actions on joint type instead of door type Removes need to save door type field in DoorCommon::DoorElement struct --- .../building_ignition_plugins/src/door.cpp | 9 +++++---- .../include/building_sim_common/door_common.hpp | 14 ++++---------- 2 files changed, 9 insertions(+), 14 deletions(-) diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index 057ec21f..0ee13a16 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -179,6 +179,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin ecm.Component(door_ign.link_entity)->Data() + ecm.Component(_en)->Data(); door_ign.orig_rotation = door_ign.orig_position.Yaw(); + door_ign.joint_type = ecm.Component(door_ign.joint_entity)->Data(); } Entity parent = _en; @@ -216,7 +217,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin + ecm.Component(_en)->Data(); constexpr double eps = 0.01; - if(door.type == "SwingDoor" || door.type == "DoubleSwingDoor") + if (door_ign.joint_type == sdf::JointType::REVOLUTE) { // In the event that Yaw angle of the door moves past -Pi rads, it experiences // a discontinuous jump from -Pi to Pi (vice-versa when the Yaw angle moves past Pi). @@ -238,7 +239,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin 3.14 + abs(-3.14 - (curr_rot - orig_rot)) : curr_rot - orig_rot; } } - else if(door.type == "SlidingDoor" || door.type == "DoubleSlidingDoor") + else if (door_ign.joint_type == sdf::JointType::PRISMATIC) { ignition::math::Vector3d displacement = curr_pos.CoordPositionSub(orig_pos); request.position = displacement.Length(); @@ -279,7 +280,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin door_ign.vel_cmd = result.velocity; double vel_cmd = result.velocity; const DoorCommon::DoorElement& door_elem = it->second; - if(door_elem.type == "SwingDoor" || door_elem.type == "DoubleSwingDoor") + if (door_ign.joint_type == sdf::JointType::REVOLUTE) { // The reference frame axes of the Linear Velocity Cmds at any point t = axes defined // by the link's global pose (link yaw + model yaw) at time t0, rotated by the change in @@ -312,7 +313,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin ecm.Component(link)->Data() = ignition::math::Vector3d(x_rot_cmd,y_rot_cmd,0); ecm.Component(link)->Data() = ignition::math::Vector3d(0,0,vel_cmd); } - else if(door_elem.type == "SlidingDoor" || door_elem.type == "DoubleSlidingDoor") + else if (door_ign.joint_type == sdf::JointType::PRISMATIC) { ecm.Component(link)->Data() = ignition::math::Vector3d(vel_cmd,0,0); } diff --git a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp index f8b220a3..5b9814dc 100644 --- a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp +++ b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp @@ -74,7 +74,6 @@ class DoorCommon struct DoorElement { - std::string type; std::string link_name; std::string joint_name; double length; @@ -86,15 +85,13 @@ class DoorCommon DoorElement() {} DoorElement( - const std::string& type, const std::string& link_name, const std::string& joint_name, const double length, const double lower_limit, const double upper_limit, const bool flip_direction = false) - : type(type), - link_name(link_name), + : link_name(link_name), joint_name(joint_name), length(length), current_position(0.0), @@ -175,16 +172,13 @@ std::shared_ptr DoorCommon::make( auto door_element = sdf_clone; std::string left_door_joint_name; std::string right_door_joint_name; - std::string door_type; // Get the joint names and door type if (!get_element_required(sdf_clone, "door", door_element) || !get_sdf_attribute_required( door_element, "left_joint_name", left_door_joint_name) || !get_sdf_attribute_required( - door_element, "right_joint_name", right_door_joint_name) || - !get_sdf_attribute_required( - door_element, "type", door_type)) + door_element, "right_joint_name", right_door_joint_name)) { RCLCPP_ERROR(node->get_logger(), " -- Missing required parameters for [%s] plugin", @@ -285,9 +279,9 @@ std::shared_ptr DoorCommon::make( DoorCommon::DoorElement door_element; if (joint_name == right_door_joint_name) door_element = - DoorCommon::DoorElement{door_type, link_name, joint_name, length, lower_limit, upper_limit, true}; + DoorCommon::DoorElement{link_name, joint_name, length, lower_limit, upper_limit, true}; else if (joint_name == left_door_joint_name) - door_element = DoorCommon::DoorElement{door_type, link_name, joint_name, length, lower_limit, upper_limit}; + door_element = DoorCommon::DoorElement{link_name, joint_name, length, lower_limit, upper_limit}; doors.insert({joint_name, door_element}); } }; From 539a9c840394599643f7a0f934cacdedc3c8f9b0 Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Thu, 24 Dec 2020 15:52:22 +0800 Subject: [PATCH 7/8] Move Physics Engine Plugin Names to common library --- .../building_ignition_plugins/src/door.cpp | 35 +++++++++++-------- .../building_ignition_plugins/src/lift.cpp | 4 --- .../building_ignition_plugins/src/slotcar.cpp | 4 --- .../include/building_sim_common/utils.hpp | 6 ++++ 4 files changed, 27 insertions(+), 22 deletions(-) diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index 0ee13a16..9d984fa4 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -48,7 +48,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin // Map from joint_name of each door section to ignition properties for that section std::unordered_map _doors_ign; Entity _en; - std::string _physics_plugin_name; + PhysEnginePlugin _phys_plugin = PhysEnginePlugin::DEFAULT; bool pos_set = false; std::shared_ptr _door_common = nullptr; @@ -84,10 +84,24 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin ecm.CreateComponent(entity, components::Pose()); } - bool is_tpe_plugin(const std::string& plugin_name) + void fill_physics_engine(Entity entity, EntityComponentManager& ecm) { - static const std::string tpe_plugin = "ignition-physics-tpe-plugin"; - return plugin_name == tpe_plugin; + Entity parent = entity; + while (ecm.ParentEntity(parent)) + { + parent = ecm.ParentEntity(parent); + } + if (ecm.EntityHasComponentType(parent, + components::PhysicsEnginePlugin().TypeId())) + { + const std::string physics_plugin_name = + ecm.Component(parent)->Data(); + const auto it = plugin_names.find(physics_plugin_name); + if (it != plugin_names.end()) + { + _phys_plugin = it->second; + } + } } public: @@ -182,14 +196,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin door_ign.joint_type = ecm.Component(door_ign.joint_entity)->Data(); } - Entity parent = _en; - while(ecm.ParentEntity(parent)){ - parent = ecm.ParentEntity(parent); - } - if (ecm.EntityHasComponentType(parent, - components::PhysicsEnginePlugin().TypeId())){ - _physics_plugin_name = ecm.Component(parent)->Data(); - } + fill_physics_engine(_en, ecm); pos_set = true; } @@ -207,7 +214,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin request.joint_name = door_name; // No joint features support, look at link pose instead - if(is_tpe_plugin(_physics_plugin_name)) + if(_phys_plugin == PhysEnginePlugin::TPE) { const double orig_rot = door_ign.orig_rotation; const double curr_rot = ecm.Component(link)->Data().Yaw() @@ -275,7 +282,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin assert(it != _door_common->_doors.end()); // No joint features support, use velocity commands to mimic joint motion - if(is_tpe_plugin(_physics_plugin_name)) + if(_phys_plugin == PhysEnginePlugin::TPE) { door_ign.vel_cmd = result.velocity; double vel_cmd = result.velocity; diff --git a/building_sim_plugins/building_ignition_plugins/src/lift.cpp b/building_sim_plugins/building_ignition_plugins/src/lift.cpp index 79b87fae..09c59ee3 100644 --- a/building_sim_plugins/building_ignition_plugins/src/lift.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/lift.cpp @@ -31,10 +31,6 @@ using namespace building_sim_common; namespace building_sim_ign { -enum class PhysEnginePlugin {DEFAULT, TPE}; -std::unordered_map plugin_names { - {"ignition-physics-tpe-plugin", PhysEnginePlugin::TPE}}; - //============================================================================== class IGNITION_GAZEBO_VISIBLE LiftPlugin diff --git a/building_sim_plugins/building_ignition_plugins/src/slotcar.cpp b/building_sim_plugins/building_ignition_plugins/src/slotcar.cpp index aed21bf3..f1873627 100644 --- a/building_sim_plugins/building_ignition_plugins/src/slotcar.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/slotcar.cpp @@ -26,10 +26,6 @@ using namespace ignition::gazebo; using namespace building_sim_common; -enum class PhysEnginePlugin {DEFAULT, TPE}; -std::unordered_map plugin_names { - {"ignition-physics-tpe-plugin", PhysEnginePlugin::TPE}}; - class IGNITION_GAZEBO_VISIBLE SlotcarPlugin : public System, public ISystemConfigure, diff --git a/building_sim_plugins/building_plugins_common/include/building_sim_common/utils.hpp b/building_sim_plugins/building_plugins_common/include/building_sim_common/utils.hpp index c1379fc3..7d435f93 100644 --- a/building_sim_plugins/building_plugins_common/include/building_sim_common/utils.hpp +++ b/building_sim_plugins/building_plugins_common/include/building_sim_common/utils.hpp @@ -3,9 +3,15 @@ #include #include +#include +#include namespace building_sim_common { +enum class PhysEnginePlugin {DEFAULT, TPE}; +std::unordered_map plugin_names { + {"ignition-physics-tpe-plugin", PhysEnginePlugin::TPE}}; + // TODO(MXG): Refactor the use of this function to replace it with // compute_desired_rate_of_change() double compute_ds( From 2123befb698680499433e6e96433cf05f88d968b Mon Sep 17 00:00:00 2001 From: mrushyendra Date: Thu, 24 Dec 2020 16:39:47 +0800 Subject: [PATCH 8/8] Fix code style for ament_uncrustify and describe normal calculation --- .../building_ignition_plugins/src/door.cpp | 72 +++++++++++-------- .../building_sim_common/door_common.hpp | 16 +++-- 2 files changed, 52 insertions(+), 36 deletions(-) diff --git a/building_sim_plugins/building_ignition_plugins/src/door.cpp b/building_sim_plugins/building_ignition_plugins/src/door.cpp index 9d984fa4..d2df96ba 100644 --- a/building_sim_plugins/building_ignition_plugins/src/door.cpp +++ b/building_sim_plugins/building_ignition_plugins/src/door.cpp @@ -80,7 +80,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin components::AngularVelocityCmd().TypeId())) ecm.CreateComponent(entity, components::AngularVelocityCmd()); if (!ecm.EntityHasComponentType(entity, - components::Pose().TypeId())) + components::Pose().TypeId())) ecm.CreateComponent(entity, components::Pose()); } @@ -185,7 +185,8 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin if (!_initialized) return; - if(!pos_set){ + if (!pos_set) + { // Code later on assumes that each link is originally at its closed position for (auto& [door_name, door_ign] : _doors_ign) { @@ -193,7 +194,8 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin ecm.Component(door_ign.link_entity)->Data() + ecm.Component(_en)->Data(); door_ign.orig_rotation = door_ign.orig_position.Yaw(); - door_ign.joint_type = ecm.Component(door_ign.joint_entity)->Data(); + door_ign.joint_type = ecm.Component( + door_ign.joint_entity)->Data(); } fill_physics_engine(_en, ecm); @@ -214,13 +216,15 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin request.joint_name = door_name; // No joint features support, look at link pose instead - if(_phys_plugin == PhysEnginePlugin::TPE) + if (_phys_plugin == PhysEnginePlugin::TPE) { const double orig_rot = door_ign.orig_rotation; - const double curr_rot = ecm.Component(link)->Data().Yaw() + const double curr_rot = + ecm.Component(link)->Data().Yaw() + ecm.Component(_en)->Data().Yaw(); const ignition::math::Pose3d orig_pos = door_ign.orig_position; - const ignition::math::Pose3d curr_pos = ecm.Component(link)->Data() + const ignition::math::Pose3d curr_pos = + ecm.Component(link)->Data() + ecm.Component(_en)->Data(); constexpr double eps = 0.01; @@ -230,7 +234,7 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin // a discontinuous jump from -Pi to Pi (vice-versa when the Yaw angle moves past Pi). // We need to take this jump into account when calculating the relative orientation // of the door w.r.t to its original orientation. - if(door.closed_position > door.open_position) + if (door.closed_position > door.open_position) { // Yaw may go past -Pi rads when opening, and experience discontinuous jump to +Pi. // For e.g. when original angle is -(7/8)Pi, opening the door by Pi/2 rads would @@ -248,12 +252,14 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin } else if (door_ign.joint_type == sdf::JointType::PRISMATIC) { - ignition::math::Vector3d displacement = curr_pos.CoordPositionSub(orig_pos); - request.position = displacement.Length(); - if (door.open_position < door.closed_position){ - // At any point the link's position must be negative relative to the closed pose - request.position *= -1; - } + ignition::math::Vector3d displacement = curr_pos.CoordPositionSub( + orig_pos); + request.position = displacement.Length(); + if (door.open_position < door.closed_position) + { + // At any point the link's position must be negative relative to the closed pose + request.position *= -1; + } } else { @@ -282,25 +288,30 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin assert(it != _door_common->_doors.end()); // No joint features support, use velocity commands to mimic joint motion - if(_phys_plugin == PhysEnginePlugin::TPE) + if (_phys_plugin == PhysEnginePlugin::TPE) { door_ign.vel_cmd = result.velocity; double vel_cmd = result.velocity; const DoorCommon::DoorElement& door_elem = it->second; if (door_ign.joint_type == sdf::JointType::REVOLUTE) { - // The reference frame axes of the Linear Velocity Cmds at any point t = axes defined - // by the link's global pose (link yaw + model yaw) at time t0, rotated by the change in - // yaw component of the link since then. - // Note: This is not always equal to the global reference frame's axes rotated by the - // link's global yaw. In some cases, the link may rotate after time t0, but the change in - // its yaw may equal 0 if the change is applied to the parent model instead. - - // Calculate the door link orientation, theta_local, relative to Linear Velocity cmds reference frame - // theta_global = (current_link_yaw + current_model_yaw) - (original_link_yaw + original_model_yaw) - // theta_local = theta_global - (current_link_yaw - original_link_yaw) - // theta_local = current_model_yaw - (original_link_yaw + original_model_yaw) (assuming original link yaw = 0) - double theta_local = ecm.Component(_en)->Data().Yaw() - door_ign.orig_rotation; + // The reference frame axes of the Linear Velocity Cmds at any time t equals to the + // axes defined by the link's global pose (link yaw + model yaw) at time t0, rotated + // by the change in yaw component of the link since then. + // Note: This does not always simply equal the axes defined by the link's global pose + // at time t. In some cases, the link may rotate after time t0, but the change in + // its yaw component may equal 0 if the change is applied to the parent model instead. + + // Calculate the door link orientation (theta_local) relative to Linear Velocity cmds reference frame + // d_yaw_component = current_link_yaw - original_link_yaw + // reference_frame_axes_rotation = (original_link_yaw + original_model_yaw) + d_yaw_component + // theta_global = current_link_yaw + current_model_yaw + // theta_local = theta_global - reference_frame_rotation_angle + // theta_local = (current_link_yaw + current_model_yaw) - ((original_link_yaw + original_model_yaw) + (current_link_yaw - original_link_yaw)) + // theta_local = current_model_yaw - original_model_yaw + double theta_local = + ecm.Component(_en)->Data().Yaw() + - door_ign.orig_rotation; // Todo: currently assumes orig_rotation = original_model_yaw, i.e. original link yaw = 0 // Given the rotation of the door, calculate a vector parallel to the door length, with magnitude 1/2 * vel_cmd double x_cmd = cos(theta_local) * vel_cmd * (door_elem.length / 2.0); @@ -317,12 +328,15 @@ class IGNITION_GAZEBO_VISIBLE DoorPlugin y_rot_cmd *= -1; } - ecm.Component(link)->Data() = ignition::math::Vector3d(x_rot_cmd,y_rot_cmd,0); - ecm.Component(link)->Data() = ignition::math::Vector3d(0,0,vel_cmd); + ecm.Component(link)->Data() = + ignition::math::Vector3d(x_rot_cmd, y_rot_cmd, 0); + ecm.Component(link)->Data() = + ignition::math::Vector3d(0, 0, vel_cmd); } else if (door_ign.joint_type == sdf::JointType::PRISMATIC) { - ecm.Component(link)->Data() = ignition::math::Vector3d(vel_cmd,0,0); + ecm.Component(link)->Data() = + ignition::math::Vector3d(vel_cmd, 0, 0); } else { diff --git a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp index 5b9814dc..4f878714 100644 --- a/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp +++ b/building_sim_plugins/building_plugins_common/include/building_sim_common/door_common.hpp @@ -235,16 +235,16 @@ std::shared_ptr DoorCommon::make( // Get name of the link associated with the joint std::string link_name; get_sdf_param_required( - element, "child", link_name); + element, "child", link_name); // Find link and get its dimensions double length = 0.0; auto link = parent->GetElement("link"); std::string link_search_name; - while(link && get_sdf_attribute_required( - link, "name", link_search_name)) + while (link && get_sdf_attribute_required( + link, "name", link_search_name)) { - if(link_search_name == link_name) + if (link_search_name == link_name) { auto visual = link->GetElement("visual"); if (visual) @@ -265,7 +265,7 @@ std::shared_ptr DoorCommon::make( { link = link->GetNextElement("link"); } - }; + } // Get joint parameters get_element_required(joint_sdf_clone, "axis", element); @@ -279,9 +279,11 @@ std::shared_ptr DoorCommon::make( DoorCommon::DoorElement door_element; if (joint_name == right_door_joint_name) door_element = - DoorCommon::DoorElement{link_name, joint_name, length, lower_limit, upper_limit, true}; + DoorCommon::DoorElement{link_name, joint_name, length, lower_limit, + upper_limit, true}; else if (joint_name == left_door_joint_name) - door_element = DoorCommon::DoorElement{link_name, joint_name, length, lower_limit, upper_limit}; + door_element = DoorCommon::DoorElement{link_name, joint_name, length, + lower_limit, upper_limit}; doors.insert({joint_name, door_element}); } };