From 2d2bc89a6b48e05566304006750b09d766c1e8ca Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Sun, 27 Mar 2022 01:05:50 +0100 Subject: [PATCH] i should be learning yet i am refactoring this is not a poem pain. --- src/btree_nodes/CMakeLists.txt | 2 +- src/btree_nodes/src/GenerateNewPose.cpp | 5 - src/btree_nodes/src/GenerateNewPose.h | 14 - src/btree_nodes/src/MinimalSubscriber.cpp | 31 -- src/btree_nodes/src/Tree.cpp | 203 +-------- .../src/helpers/MinimalSubscriber.cpp | 6 + .../src/helpers/MinimalSubscriber.h | 46 ++ src/btree_nodes/src/nodes/AmICalled.cpp | 23 + src/btree_nodes/src/nodes/AmICalled.h | 22 + .../src/nodes/GenerateTarget2D.cpp | 88 ++++ src/btree_nodes/src/nodes/GenerateTarget2D.h | 40 ++ .../src/nodes/MoveActorToTarget.cpp | 64 +++ src/btree_nodes/src/nodes/MoveActorToTarget.h | 30 ++ src/btree_nodes/src/nodes/RobotMove.cpp | 5 + src/btree_nodes/src/nodes/RobotMove.h | 22 + .../src/{ => nodes}/WeightedRandomNode.cpp | 7 +- .../src/{ => nodes}/WeightedRandomNode.h | 5 +- src/btree_robot/src/robot_test.cpp | 400 +----------------- src/gaz_simulation/launch/test_launch.py | 42 +- .../launch/lbr_move_group.launch.py | 2 + src/iisy_config/srdf/iisy.srdf | 1 + src/iisy_config/stl/table.stl | Bin 0 -> 1884 bytes src/iisy_config/urdf/iisy.urdf | 20 +- 23 files changed, 411 insertions(+), 667 deletions(-) delete mode 100644 src/btree_nodes/src/GenerateNewPose.cpp delete mode 100644 src/btree_nodes/src/GenerateNewPose.h delete mode 100644 src/btree_nodes/src/MinimalSubscriber.cpp create mode 100644 src/btree_nodes/src/helpers/MinimalSubscriber.cpp create mode 100644 src/btree_nodes/src/helpers/MinimalSubscriber.h create mode 100644 src/btree_nodes/src/nodes/AmICalled.cpp create mode 100644 src/btree_nodes/src/nodes/AmICalled.h create mode 100644 src/btree_nodes/src/nodes/GenerateTarget2D.cpp create mode 100644 src/btree_nodes/src/nodes/GenerateTarget2D.h create mode 100644 src/btree_nodes/src/nodes/MoveActorToTarget.cpp create mode 100644 src/btree_nodes/src/nodes/MoveActorToTarget.h create mode 100644 src/btree_nodes/src/nodes/RobotMove.cpp create mode 100644 src/btree_nodes/src/nodes/RobotMove.h rename src/btree_nodes/src/{ => nodes}/WeightedRandomNode.cpp (95%) rename src/btree_nodes/src/{ => nodes}/WeightedRandomNode.h (87%) create mode 100755 src/iisy_config/stl/table.stl diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index c3ffa0e..49dd0ca 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/src/btree_nodes/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(behaviortree_cpp_v3 REQUIRED) # further dependencies manually. # find_package( REQUIRED) -add_executable(tree src/Tree.cpp src/Extensions.cpp src/WeightedRandomNode.cpp) +add_executable(tree src/Tree.cpp src/Extensions.cpp src/nodes/WeightedRandomNode.cpp src/nodes/AmICalled.cpp src/nodes/GenerateTarget2D.cpp src/nodes/MoveActorToTarget.cpp src/helpers/MinimalSubscriber.cpp src/nodes/RobotMove.cpp) ament_target_dependencies(tree rclcpp geometry_msgs std_msgs behaviortree_cpp_v3) #add_executable(talker src/publisher_member_function.cpp) diff --git a/src/btree_nodes/src/GenerateNewPose.cpp b/src/btree_nodes/src/GenerateNewPose.cpp deleted file mode 100644 index f59981d..0000000 --- a/src/btree_nodes/src/GenerateNewPose.cpp +++ /dev/null @@ -1,5 +0,0 @@ -// -// Created by bastian on 21.02.22. -// - -#include "GenerateNewPose.h" diff --git a/src/btree_nodes/src/GenerateNewPose.h b/src/btree_nodes/src/GenerateNewPose.h deleted file mode 100644 index 318d945..0000000 --- a/src/btree_nodes/src/GenerateNewPose.h +++ /dev/null @@ -1,14 +0,0 @@ -// -// Created by bastian on 21.02.22. -// - -#ifndef BUILD_GENERATENEWPOSE_H -#define BUILD_GENERATENEWPOSE_H - - -class GenerateNewPose { - -}; - - -#endif //BUILD_GENERATENEWPOSE_H diff --git a/src/btree_nodes/src/MinimalSubscriber.cpp b/src/btree_nodes/src/MinimalSubscriber.cpp deleted file mode 100644 index 16892b9..0000000 --- a/src/btree_nodes/src/MinimalSubscriber.cpp +++ /dev/null @@ -1,31 +0,0 @@ -// -// Created by bastian on 22.02.22. -// - -#include -#include -#include "rclcpp/rclcpp.hpp" - -template -class MinimalSubscriber : public rclcpp::Node { -public: - MinimalSubscriber(const std::string &node_name, const std::string &topic_name, std::function callback) : - Node(node_name) { - this->subscription_ = this->create_subscription( - topic_name, 10, [callback](const T result) { - callback(result); - }); - } - -private: - typename rclcpp::Subscription::SharedPtr subscription_; -}; - -template -std::thread spinMinimalSubscriber(const std::string &node_name, const std::string &topic_name, std::function callback) { - auto subscriber = std::make_shared>(node_name, topic_name, callback); - return std::thread([subscriber]() { - rclcpp::spin(subscriber); - rclcpp::shutdown(); - }); -} \ No newline at end of file diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index 12ea005..057ccac 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -1,204 +1,15 @@ -#include #include "Area.h" -#include #include #include #include -#include "MinimalSubscriber.cpp" -#include "WeightedRandomNode.h" - -//------------------------------------------------------------- -// Simple Action to print a number -//------------------------------------------------------------- +#include "nodes/WeightedRandomNode.h" +#include "nodes/AmICalled.h" +#include "nodes/GenerateTarget2D.h" +#include "nodes/MoveActorToTarget.h" +#include "helpers/MinimalSubscriber.h" using namespace BT; -class AmICalled: public ConditionNode -{ -public: - AmICalled(const std::string& name, const NodeConfiguration& config): - ConditionNode(name,config) - {} - - static PortsList providedPorts() - { - return { InputPort("called") }; - } - - NodeStatus tick() override - { - bool called; - if(!getInput("called",called)){ - return NodeStatus::FAILURE; - } - if(called){ - return NodeStatus::SUCCESS; - } - return NodeStatus::FAILURE; - } -}; - -typedef std::chrono::milliseconds Milliseconds; - -class MoveActorToTarget : public StatefulActionNode { -public: - MoveActorToTarget(const std::string &name, const NodeConfiguration &config) - : StatefulActionNode(name, config) {} - - static PortsList providedPorts() { - return { - InputPort("current"), - InputPort("target") - }; - } - - geometry_msgs::msg::Pose target; - - - NodeStatus onStart() override { - std::cout << "started moving" << std::endl; - - rclcpp::Node node("targetPublisherNode"); - auto publisher = node.create_publisher("targetPose", 10); - - auto res = getInput("target", target); - if (!res) { - std::cout << "[ no target available ]" << std::endl; - std::cout << res.error() << std::endl; - return NodeStatus::FAILURE; - } - - publisher->publish(target); - return NodeStatus::RUNNING; - } - - NodeStatus onRunning() override { - geometry_msgs::msg::Pose current; - - auto res = getInput("current", current); - if (!res) { - std::cout << "[ no current position available ]" << std::endl; - std::cout << res.error() << std::endl; - return NodeStatus::FAILURE; - } - - double dX = target.position.x - current.position.x; - double dY = target.position.y - current.position.y; - - auto distance = sqrt(dX * dX + dY * dY); - - if (distance < 0.3) { - return NodeStatus::SUCCESS; - } else { - return NodeStatus::RUNNING; - } - } - - void onHalted() override { - std::cout << "halted move" << std::endl; - rclcpp::Node node("targetPublisherNode"); - auto publisher = node.create_publisher("targetPose", 10); - geometry_msgs::msg::Pose inf; - inf.position.x = HUGE_VAL; - inf.position.y = HUGE_VAL; - publisher->publish(inf); - } -}; - -// This class cannot guarantee valid results with every polygon, triangles are assumed to be a valid triangle-strip. -class GenerateTarget2D : public SyncActionNode { - Area area; - std::vector weights; - std::mt19937_64 rng; - -private: - double getArea(unsigned long id) { - Position2D a = area.triangles[id]; - Position2D b = area.triangles[id + 1]; - Position2D c = area.triangles[id + 2]; - - return 0.5 * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y)); - } - -private: - double getRandom() { - std::uniform_real_distribution zeroToOne(0, 1); - return zeroToOne(rng); - } - -private: - Position2D getRandomPosInTriangle(unsigned long id) { - Position2D a = area.triangles[id]; - Position2D b = area.triangles[id + 1]; - Position2D c = area.triangles[id + 2]; - - Position2D ab = {b.x - a.x, b.y - a.y}; - Position2D ac = {c.x - a.x, c.y - a.y}; - - double u1 = getRandom(); - double u2 = getRandom(); - - if (u1 + u2 > 1) { - u1 = 1 - u1; - u2 = 1 - u2; - } - - return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)}; - } - -public: - GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area area) : - SyncActionNode(name, config) { - this->area = area; - unsigned long triangleCount = area.triangles.size() - 2; - - uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count(); - std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed >> 32)}; - rng.seed(ss); - - std::vector prob(triangleCount); - double sum = 0; - for (unsigned long i = 0; i < triangleCount; ++i) { - double currentArea = getArea(i); - prob[i] = currentArea; - sum += currentArea; - } - for (unsigned long i = 0; i < triangleCount; ++i) { - prob[i] = prob[i] / sum; - } - sum = 0; - for (unsigned long i = 0; i < triangleCount; ++i) { - prob[i] = prob[i] + sum; - sum += prob[i]; - } - - this->weights = prob; - } - - static PortsList providedPorts() { - // Optionally, a port can have a human readable description - const char *description = "Generates a random position in an area defined by a triangle strip"; - return {OutputPort("target", description)}; - } - - - NodeStatus tick() override { - double triangleRandom = getRandom(); - unsigned long i = 0; - while (triangleRandom < weights[i + 1] && i < weights.size() - 1) { - i++; - } - auto pos = getRandomPosInTriangle(i); - - geometry_msgs::msg::Pose randomPose; - randomPose.position.x=pos.x; - randomPose.position.y=pos.y; - - printf("Generated Target: %f %f\n",pos.x,pos.y); - setOutput("target", randomPose); - return NodeStatus::SUCCESS; - } -}; int main(int argc, char **argv) { std::cout << "Registering nodes." << std::endl; @@ -247,14 +58,14 @@ int main(int argc, char **argv) { tree.rootBlackboard()->set("target",init); tree.rootBlackboard()->set("called",false); - spinMinimalSubscriber("tree_get_currentPose", "currentPose", + MinimalSubscriber::createAsThread("tree_get_currentPose", "currentPose", [&tree, &mutex](geometry_msgs::msg::Pose pose) { mutex.lock(); tree.rootBlackboard()->set("current", pose); mutex.unlock(); }).detach(); - spinMinimalSubscriber("tree_get_called", "called", + MinimalSubscriber::createAsThread("tree_get_called", "called", [&tree, &mutex](std_msgs::msg::Bool called) { mutex.lock(); tree.rootBlackboard()->set("called", (bool) called.data); diff --git a/src/btree_nodes/src/helpers/MinimalSubscriber.cpp b/src/btree_nodes/src/helpers/MinimalSubscriber.cpp new file mode 100644 index 0000000..1ad75bc --- /dev/null +++ b/src/btree_nodes/src/helpers/MinimalSubscriber.cpp @@ -0,0 +1,6 @@ +// +// Created by bastian on 26.03.22. +// + +#include "MinimalSubscriber.h" + diff --git a/src/btree_nodes/src/helpers/MinimalSubscriber.h b/src/btree_nodes/src/helpers/MinimalSubscriber.h new file mode 100644 index 0000000..b80baa4 --- /dev/null +++ b/src/btree_nodes/src/helpers/MinimalSubscriber.h @@ -0,0 +1,46 @@ +// +// Created by bastian on 26.03.22. +// + +#ifndef BUILD_MINIMALSUBSCRIBER_H +#define BUILD_MINIMALSUBSCRIBER_H + +#include +#include +#include "rclcpp/rclcpp.hpp" + +template +class MinimalSubscriber : public rclcpp::Node { +public: + MinimalSubscriber(const std::string &node_name, const std::string &topic_name, + std::function callback); + +public: + static std::thread createAsThread(const std::string &node_name, const std::string &topic_name, + std::function callback); + +private: + typename rclcpp::Subscription::SharedPtr subscription_; +}; + +template +MinimalSubscriber::MinimalSubscriber(const std::string &node_name, const std::string &topic_name, + std::function callback) : + Node(node_name) { + this->subscription_ = this->create_subscription( + topic_name, 10, [callback](const T result) { + callback(result); + }); +} + +template +std::thread MinimalSubscriber::createAsThread(const std::string &node_name, const std::string &topic_name, + std::function callback) { + auto subscriber = std::make_shared>(node_name, topic_name, callback); + return std::thread([subscriber]() { + rclcpp::spin(subscriber); + rclcpp::shutdown(); + }); +} + +#endif //BUILD_MINIMALSUBSCRIBER_H diff --git a/src/btree_nodes/src/nodes/AmICalled.cpp b/src/btree_nodes/src/nodes/AmICalled.cpp new file mode 100644 index 0000000..714a474 --- /dev/null +++ b/src/btree_nodes/src/nodes/AmICalled.cpp @@ -0,0 +1,23 @@ +// +// Created by bastian on 26.03.22. +// + +#include "AmICalled.h" + +BT::AmICalled::AmICalled(const std::string &name, const BT::NodeConfiguration &config) : + ConditionNode(name, config) {} + +BT::PortsList BT::AmICalled::providedPorts() { + return {InputPort("called")}; +} + +BT::NodeStatus BT::AmICalled::tick() { + bool called; + if (!getInput("called", called)) { + return NodeStatus::FAILURE; + } + if (called) { + return NodeStatus::SUCCESS; + } + return NodeStatus::FAILURE; +} diff --git a/src/btree_nodes/src/nodes/AmICalled.h b/src/btree_nodes/src/nodes/AmICalled.h new file mode 100644 index 0000000..9daf834 --- /dev/null +++ b/src/btree_nodes/src/nodes/AmICalled.h @@ -0,0 +1,22 @@ +// +// Created by bastian on 26.03.22. +// + +#ifndef BUILD_AMICALLED_H +#define BUILD_AMICALLED_H + +#include + +namespace BT { + + class AmICalled : public ConditionNode { + public: + AmICalled(const std::string &name, const NodeConfiguration &config); + + static PortsList providedPorts(); + + NodeStatus tick() override; + }; + +} +#endif //BUILD_AMICALLED_H diff --git a/src/btree_nodes/src/nodes/GenerateTarget2D.cpp b/src/btree_nodes/src/nodes/GenerateTarget2D.cpp new file mode 100644 index 0000000..01792e9 --- /dev/null +++ b/src/btree_nodes/src/nodes/GenerateTarget2D.cpp @@ -0,0 +1,88 @@ +// +// Created by bastian on 26.03.22. +// + +#include "GenerateTarget2D.h" + +double BT::GenerateTarget2D::getArea(unsigned long id) { + Position2D a = area.triangles[id]; + Position2D b = area.triangles[id + 1]; + Position2D c = area.triangles[id + 2]; + + return 0.5 * ((b.x - a.x) * (c.y - a.y) - (c.x - a.x) * (b.y - a.y)); +} + +double BT::GenerateTarget2D::getRandom() { + std::uniform_real_distribution zeroToOne(0, 1); + return zeroToOne(rng); +} + +Position2D BT::GenerateTarget2D::getRandomPosInTriangle(unsigned long id) { + Position2D a = area.triangles[id]; + Position2D b = area.triangles[id + 1]; + Position2D c = area.triangles[id + 2]; + + Position2D ab = {b.x - a.x, b.y - a.y}; + Position2D ac = {c.x - a.x, c.y - a.y}; + + double u1 = getRandom(); + double u2 = getRandom(); + + if (u1 + u2 > 1) { + u1 = 1 - u1; + u2 = 1 - u2; + } + + return {a.x + (u1 * ab.x + u2 * ac.x), a.y + (u1 * ab.y + u2 * ac.y)}; +} + +BT::GenerateTarget2D::GenerateTarget2D(const std::string &name, const BT::NodeConfiguration &config, const Area area) : + SyncActionNode(name, config) { + this->area = area; + unsigned long triangleCount = area.triangles.size() - 2; + + uint64_t timeSeed = std::chrono::high_resolution_clock::now().time_since_epoch().count(); + std::seed_seq ss{uint32_t(timeSeed & 0xffffffff), uint32_t(timeSeed >> 32)}; + rng.seed(ss); + + std::vector prob(triangleCount); + double sum = 0; + for (unsigned long i = 0; i < triangleCount; ++i) { + double currentArea = getArea(i); + prob[i] = currentArea; + sum += currentArea; + } + for (unsigned long i = 0; i < triangleCount; ++i) { + prob[i] = prob[i] / sum; + } + sum = 0; + for (unsigned long i = 0; i < triangleCount; ++i) { + prob[i] = prob[i] + sum; + sum += prob[i]; + } + + this->weights = prob; +} + +BT::PortsList BT::GenerateTarget2D::providedPorts() { + // Optionally, a port can have a human readable description + const char *description = "Generates a random position in an area defined by a triangle strip"; + return {OutputPort("target", description)}; +} + +BT::NodeStatus BT::GenerateTarget2D::tick() { + double triangleRandom = getRandom(); + unsigned long i = 0; + while (triangleRandom < weights[i + 1] && i < weights.size() - 1) { + i++; + } + auto pos = getRandomPosInTriangle(i); + + geometry_msgs::msg::Pose randomPose; + randomPose.position.x=pos.x; + randomPose.position.y=pos.y; + + printf("Generated Target: %f %f\n",pos.x,pos.y); + setOutput("target", randomPose); + return NodeStatus::SUCCESS; +} diff --git a/src/btree_nodes/src/nodes/GenerateTarget2D.h b/src/btree_nodes/src/nodes/GenerateTarget2D.h new file mode 100644 index 0000000..83360f0 --- /dev/null +++ b/src/btree_nodes/src/nodes/GenerateTarget2D.h @@ -0,0 +1,40 @@ +// +// Created by bastian on 26.03.22. +// + +#ifndef BUILD_GENERATETARGET2D_H +#define BUILD_GENERATETARGET2D_H + +#include +#include "../Area.h" +#include +#include + +namespace BT{ + +// This class cannot guarantee valid results with every polygon, triangles are assumed to be a valid triangle-strip. +class GenerateTarget2D : public SyncActionNode { + Area area; + std::vector weights; + std::mt19937_64 rng; + +private: + double getArea(unsigned long id); + +private: + double getRandom(); + +private: + Position2D getRandomPosInTriangle(unsigned long id); + +public: + GenerateTarget2D(const std::string &name, const NodeConfiguration &config, const Area area); + + static PortsList providedPorts(); + + + NodeStatus tick() override; +}; +} + +#endif //BUILD_GENERATETARGET2D_H diff --git a/src/btree_nodes/src/nodes/MoveActorToTarget.cpp b/src/btree_nodes/src/nodes/MoveActorToTarget.cpp new file mode 100644 index 0000000..4a71de4 --- /dev/null +++ b/src/btree_nodes/src/nodes/MoveActorToTarget.cpp @@ -0,0 +1,64 @@ +// +// Created by bastian on 26.03.22. +// + +#include "MoveActorToTarget.h" + +BT::MoveActorToTarget::MoveActorToTarget(const std::string &name, const BT::NodeConfiguration &config) + : StatefulActionNode(name, config) {} + +BT::PortsList BT::MoveActorToTarget::providedPorts() { + return { + InputPort("current"), + InputPort("target") + }; +} + +BT::NodeStatus BT::MoveActorToTarget::onStart() { + std::cout << "started moving" << std::endl; + + rclcpp::Node node("targetPublisherNode"); + auto publisher = node.create_publisher("targetPose", 10); + + auto res = getInput("target", target); + if (!res) { + std::cout << "[ no target available ]" << std::endl; + std::cout << res.error() << std::endl; + return NodeStatus::FAILURE; + } + + publisher->publish(target); + return NodeStatus::RUNNING; +} + +BT::NodeStatus BT::MoveActorToTarget::onRunning() { + geometry_msgs::msg::Pose current; + + auto res = getInput("current", current); + if (!res) { + std::cout << "[ no current position available ]" << std::endl; + std::cout << res.error() << std::endl; + return NodeStatus::FAILURE; + } + + double dX = target.position.x - current.position.x; + double dY = target.position.y - current.position.y; + + auto distance = sqrt(dX * dX + dY * dY); + + if (distance < 0.3) { + return NodeStatus::SUCCESS; + } else { + return NodeStatus::RUNNING; + } +} + +void BT::MoveActorToTarget::onHalted() { + std::cout << "halted move" << std::endl; + rclcpp::Node node("targetPublisherNode"); + auto publisher = node.create_publisher("targetPose", 10); + geometry_msgs::msg::Pose inf; + inf.position.x = HUGE_VAL; + inf.position.y = HUGE_VAL; + publisher->publish(inf); +} diff --git a/src/btree_nodes/src/nodes/MoveActorToTarget.h b/src/btree_nodes/src/nodes/MoveActorToTarget.h new file mode 100644 index 0000000..704c283 --- /dev/null +++ b/src/btree_nodes/src/nodes/MoveActorToTarget.h @@ -0,0 +1,30 @@ +// +// Created by bastian on 26.03.22. +// + +#ifndef BUILD_MOVEACTORTOTARGET_H +#define BUILD_MOVEACTORTOTARGET_H + +#include +#include +#include + +namespace BT { + class MoveActorToTarget : public StatefulActionNode { + public: + MoveActorToTarget(const std::string &name, const NodeConfiguration &config); + + static PortsList providedPorts(); + + geometry_msgs::msg::Pose target; + + NodeStatus onStart() override; + + NodeStatus onRunning() override; + + void onHalted() override; + }; +} + + +#endif //BUILD_MOVEACTORTOTARGET_H diff --git a/src/btree_nodes/src/nodes/RobotMove.cpp b/src/btree_nodes/src/nodes/RobotMove.cpp new file mode 100644 index 0000000..12a66eb --- /dev/null +++ b/src/btree_nodes/src/nodes/RobotMove.cpp @@ -0,0 +1,5 @@ +// +// Created by bastian on 26.03.22. +// + +#include "RobotMove.h" diff --git a/src/btree_nodes/src/nodes/RobotMove.h b/src/btree_nodes/src/nodes/RobotMove.h new file mode 100644 index 0000000..4fb742e --- /dev/null +++ b/src/btree_nodes/src/nodes/RobotMove.h @@ -0,0 +1,22 @@ +// +// Created by bastian on 26.03.22. +// + +#ifndef BUILD_ROBOTMOVE_H +#define BUILD_ROBOTMOVE_H + +#include +#include + +namespace BT { + + class RobotMove : public StatefulActionNode { + public: + RobotMove(const std::string &name, const BT::NodeConfiguration &config) + : StatefulActionNode(name, config) { + + } + }; +} + +#endif //BUILD_ROBOTMOVE_H diff --git a/src/btree_nodes/src/WeightedRandomNode.cpp b/src/btree_nodes/src/nodes/WeightedRandomNode.cpp similarity index 95% rename from src/btree_nodes/src/WeightedRandomNode.cpp rename to src/btree_nodes/src/nodes/WeightedRandomNode.cpp index 513a38e..a0003d3 100644 --- a/src/btree_nodes/src/WeightedRandomNode.cpp +++ b/src/btree_nodes/src/nodes/WeightedRandomNode.cpp @@ -3,7 +3,6 @@ // #include "WeightedRandomNode.h" -#include WeightedRandomNode::WeightedRandomNode(const std::string &name, const NodeConfiguration &config) : ControlNode::ControlNode(name, config), selected_child_index(0), select_next(true) { @@ -67,4 +66,8 @@ NodeStatus WeightedRandomNode::tick() { } return child_status; -} \ No newline at end of file +} + +PortsList WeightedRandomNode::providedPorts() { + return { InputPort("weights") }; +} diff --git a/src/btree_nodes/src/WeightedRandomNode.h b/src/btree_nodes/src/nodes/WeightedRandomNode.h similarity index 87% rename from src/btree_nodes/src/WeightedRandomNode.h rename to src/btree_nodes/src/nodes/WeightedRandomNode.h index a2f704b..aa7d54c 100644 --- a/src/btree_nodes/src/WeightedRandomNode.h +++ b/src/btree_nodes/src/nodes/WeightedRandomNode.h @@ -8,6 +8,7 @@ #include #include #include +#include using namespace BT; @@ -20,9 +21,7 @@ public: virtual void halt() override; - static PortsList providedPorts(){ - return { InputPort("weights") }; - } + static PortsList providedPorts(); private: size_t selected_child_index; bool select_next{}; diff --git a/src/btree_robot/src/robot_test.cpp b/src/btree_robot/src/robot_test.cpp index 057fcc3..883c98b 100644 --- a/src/btree_robot/src/robot_test.cpp +++ b/src/btree_robot/src/robot_test.cpp @@ -37,14 +37,6 @@ #include #include -#include -#include - -#include -#include - -#include - // All source files that use ROS logging should define a file-specific // static const rclcpp::Logger named LOGGER, located at the top of the file // and inside the namespace with the narrowest scope (if there is one) @@ -87,43 +79,11 @@ int main(int argc, char** argv) const moveit::core::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP); - // Visualization - // ^^^^^^^^^^^^^ - namespace rvt = rviz_visual_tools; - moveit_visual_tools::MoveItVisualTools visual_tools(move_group_node, "base_bottom", "iisy", - move_group.getRobotModel()); - - visual_tools.deleteAllMarkers(); - - /* Remote control is an introspection tool that allows users to step through a high level script */ - /* via buttons and keyboard shortcuts in RViz */ - visual_tools.loadRemoteControl(); - - // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres - Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); - text_pose.translation().z() = 1.0; - visual_tools.publishText(text_pose, "MoveGroupInterface_Demo", rvt::WHITE, rvt::XLARGE); - // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations - visual_tools.trigger(); - - // Getting Basic Information - // ^^^^^^^^^^^^^^^^^^^^^^^^^ - // - // We can print the name of the reference frame for this robot. - RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group.getPlanningFrame().c_str()); - - // We can also print the name of the end-effector link for this group. - RCLCPP_INFO(LOGGER, "End effector link: %s", move_group.getEndEffectorLink().c_str()); - - // We can get a list of all the groups in the robot: - RCLCPP_INFO(LOGGER, "Available Planning Groups:"); - std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), - std::ostream_iterator(std::cout, ", ")); // Start the demo // ^^^^^^^^^^^^^^^^^^^^^^^^^ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); + // visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); // .. _move_group_interface-planning-to-pose-goal: // @@ -132,10 +92,12 @@ int main(int argc, char** argv) // We can plan a motion for this group to a desired pose for the // end-effector. geometry_msgs::msg::Pose target_pose1; - target_pose1.orientation.w = 1.0; - target_pose1.position.x = 0.28; + target_pose1.orientation.w = 0; + target_pose1.orientation.y = 1; + + target_pose1.position.x = 0.2; target_pose1.position.y = -0.2; - target_pose1.position.z = 0.5; + target_pose1.position.z = 1.2; move_group.setPoseTarget(target_pose1); // Now, we call the planner to compute the plan and visualize it. @@ -147,354 +109,8 @@ int main(int argc, char** argv) RCLCPP_INFO(LOGGER, "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); - // Visualizing plans - // ^^^^^^^^^^^^^^^^^ - // We can also visualize the plan as a line with markers in RViz. - RCLCPP_INFO(LOGGER, "Visualizing plan 1 as trajectory line"); - visual_tools.publishAxisLabeled(target_pose1, "pose1"); - visual_tools.publishText(text_pose, "Pose_Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - - // Moving to a pose goal - // ^^^^^^^^^^^^^^^^^^^^^ - // - // Moving to a pose goal is similar to the step above - // except we now use the ``move()`` function. Note that - // the pose goal we had set earlier is still active - // and so the robot will try to move to that goal. We will - // not use that function in this tutorial since it is - // a blocking function and requires a controller to be active - // and report success on execution of a trajectory. - - /* Uncomment below line when working with a real robot */ - /* move_group.move(); */ - - // Planning to a joint-space goal - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // - // Let's set a joint space goal and move towards it. This will replace the - // pose target we set above. - // - // To start, we'll create an pointer that references the current robot's state. - // RobotState is the object that contains all the current position/velocity/acceleration data. - moveit::core::RobotStatePtr current_state = move_group.getCurrentState(10); - // - // Next get the current set of joint values for the group. - std::vector joint_group_positions; - current_state->copyJointGroupPositions(joint_model_group, joint_group_positions); - - // Now, let's modify one of the joints, plan to the new joint space goal, and visualize the plan. - joint_group_positions[0] = -1.0; // radians - move_group.setJointValueTarget(joint_group_positions); - - // We lower the allowed maximum velocity and acceleration to 5% of their maximum. - // The default values are 10% (0.1). - // Set your preferred defaults in the joint_limits.yaml file of your robot's moveit_config - // or set explicit factors in your code if you need your robot to move faster. - move_group.setMaxVelocityScalingFactor(0.05); - move_group.setMaxAccelerationScalingFactor(0.05); - - success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(LOGGER, "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED"); - - // Visualize the plan in RViz: - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Joint_Space_Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - - // Planning with Path Constraints - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // - // Path constraints can easily be specified for a link on the robot. - // Let's specify a path constraint and a pose goal for our group. - // First define the path constraint. - moveit_msgs::msg::OrientationConstraint ocm; - ocm.link_name = "link3_top"; - ocm.header.frame_id = "base_bottom"; - ocm.orientation.w = 1.0; - ocm.absolute_x_axis_tolerance = 0.1; - ocm.absolute_y_axis_tolerance = 0.1; - ocm.absolute_z_axis_tolerance = 0.1; - ocm.weight = 1.0; - - // Now, set it as the path constraint for the group. - moveit_msgs::msg::Constraints test_constraints; - test_constraints.orientation_constraints.push_back(ocm); - move_group.setPathConstraints(test_constraints); - - // Enforce Planning in Joint Space - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // - // Depending on the planning problem MoveIt chooses between - // ``joint space`` and ``cartesian space`` for problem representation. - // Setting the group parameter ``enforce_joint_model_state_space:true`` in - // the ompl_planning.yaml file enforces the use of ``joint space`` for all plans. - // - // By default, planning requests with orientation path constraints - // are sampled in ``cartesian space`` so that invoking IK serves as a - // generative sampler. - // - // By enforcing ``joint space``, the planning process will use rejection - // sampling to find valid requests. Please note that this might - // increase planning time considerably. - // - // We will reuse the old goal that we had and plan to it. - // Note that this will only work if the current state already - // satisfies the path constraints. So we need to set the start - // state to a new pose. - moveit::core::RobotState start_state(*move_group.getCurrentState()); - geometry_msgs::msg::Pose start_pose2; - start_pose2.orientation.w = 1.0; - start_pose2.position.x = 0.55; - start_pose2.position.y = -0.05; - start_pose2.position.z = 0.8; - start_state.setFromIK(joint_model_group, start_pose2); - move_group.setStartState(start_state); - - // Now, we will plan to the earlier pose target from the new - // start state that we just created. - move_group.setPoseTarget(target_pose1); - - // Planning with constraints can be slow because every sample must call an inverse kinematics solver. - // Let's increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed. - move_group.setPlanningTime(10.0); - - success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(LOGGER, "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED"); - - // Visualize the plan in RViz: - visual_tools.deleteAllMarkers(); - visual_tools.publishAxisLabeled(start_pose2, "start"); - visual_tools.publishAxisLabeled(target_pose1, "goal"); - visual_tools.publishText(text_pose, "Constrained_Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - - // When done with the path constraint, be sure to clear it. - move_group.clearPathConstraints(); - - // Cartesian Paths - // ^^^^^^^^^^^^^^^ - // You can plan a Cartesian path directly by specifying a list of waypoints - // for the end-effector to go through. Note that we are starting - // from the new start state above. The initial pose (start state) does not - // need to be added to the waypoint list but adding it can help with visualizations - std::vector waypoints; - waypoints.push_back(start_pose2); - - geometry_msgs::msg::Pose target_pose3 = start_pose2; - - target_pose3.position.z -= 0.2; - waypoints.push_back(target_pose3); // down - - target_pose3.position.y -= 0.2; - waypoints.push_back(target_pose3); // right - - target_pose3.position.z += 0.2; - target_pose3.position.y += 0.2; - target_pose3.position.x -= 0.2; - waypoints.push_back(target_pose3); // up and left - - // We want the Cartesian path to be interpolated at a resolution of 1 cm - // which is why we will specify 0.01 as the max step in Cartesian - // translation. We will specify the jump threshold as 0.0, effectively disabling it. - // Warning - disabling the jump threshold while operating real hardware can cause - // large unpredictable motions of redundant joints and could be a safety issue - moveit_msgs::msg::RobotTrajectory trajectory; - const double jump_threshold = 0.0; - const double eef_step = 0.01; - double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); - RCLCPP_INFO(LOGGER, "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0); - - // Visualize the plan in RViz - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Cartesian_Path", rvt::WHITE, rvt::XLARGE); - visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL); - for (std::size_t i = 0; i < waypoints.size(); ++i) - visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - - // Cartesian motions should often be slow, e.g. when approaching objects. The speed of Cartesian - // plans cannot currently be set through the maxVelocityScalingFactor, but requires you to time - // the trajectory manually, as described `here `_. - // Pull requests are welcome. - // - // You can execute a trajectory like this. - /* move_group.execute(trajectory); */ - - // Adding objects to the environment - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // - // First, let's plan to another simple goal with no objects in the way. - move_group.setStartState(*move_group.getCurrentState()); - geometry_msgs::msg::Pose another_pose; - another_pose.orientation.w = 0; - another_pose.orientation.x = -1.0; - another_pose.position.x = 0.7; - another_pose.position.y = 0.0; - another_pose.position.z = 0.59; - move_group.setPoseTarget(another_pose); - - success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(LOGGER, "Visualizing plan 5 (with no obstacles) %s", success ? "" : "FAILED"); - - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Clear_Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishAxisLabeled(another_pose, "goal"); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); - - // The result may look like this: - // - // .. image:: ./move_group_interface_tutorial_clear_path.gif - // :alt: animation showing the arm moving relatively straight toward the goal - // - // Now, let's define a collision object ROS message for the robot to avoid. - moveit_msgs::msg::CollisionObject collision_object; - collision_object.header.frame_id = move_group.getPlanningFrame(); - - // The id of the object is used to identify it. - collision_object.id = "box1"; - - // Define a box to add to the world. - shape_msgs::msg::SolidPrimitive primitive; - primitive.type = primitive.BOX; - primitive.dimensions.resize(3); - primitive.dimensions[primitive.BOX_X] = 0.1; - primitive.dimensions[primitive.BOX_Y] = 1.5; - primitive.dimensions[primitive.BOX_Z] = 0.5; - - // Define a pose for the box (specified relative to frame_id). - geometry_msgs::msg::Pose box_pose; - box_pose.orientation.w = 1.0; - box_pose.position.x = 0.48; - box_pose.position.y = 0.0; - box_pose.position.z = 0.25; - - collision_object.primitives.push_back(primitive); - collision_object.primitive_poses.push_back(box_pose); - collision_object.operation = collision_object.ADD; - - std::vector collision_objects; - collision_objects.push_back(collision_object); - - // Now, let's add the collision object into the world - // (using a vector that could contain additional objects) - RCLCPP_INFO(LOGGER, "Add an object into the world"); - planning_scene_interface.addCollisionObjects(collision_objects); - - // Show text in RViz of status and wait for MoveGroup to receive and process the collision object message - visual_tools.publishText(text_pose, "Add_object", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz"); - - // Now, when we plan a trajectory it will avoid the obstacle. - success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(LOGGER, "Visualizing plan 6 (pose goal move around cuboid) %s", success ? "" : "FAILED"); - visual_tools.publishText(text_pose, "Obstacle_Goal", rvt::WHITE, rvt::XLARGE); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); - - // The result may look like this: - // - // .. image:: ./move_group_interface_tutorial_avoid_path.gif - // :alt: animation showing the arm moving avoiding the new obstacle - // - // Attaching objects to the robot - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // - // You can attach an object to the robot, so that it moves with the robot geometry. - // This simulates picking up the object for the purpose of manipulating it. - // The motion planning should avoid collisions between objects as well. - moveit_msgs::msg::CollisionObject object_to_attach; - object_to_attach.id = "cylinder1"; - - shape_msgs::msg::SolidPrimitive cylinder_primitive; - cylinder_primitive.type = primitive.CYLINDER; - cylinder_primitive.dimensions.resize(2); - cylinder_primitive.dimensions[primitive.CYLINDER_HEIGHT] = 0.20; - cylinder_primitive.dimensions[primitive.CYLINDER_RADIUS] = 0.04; - - // We define the frame/pose for this cylinder so that it appears in the gripper. - object_to_attach.header.frame_id = move_group.getEndEffectorLink(); - geometry_msgs::msg::Pose grab_pose; - grab_pose.orientation.w = 1.0; - grab_pose.position.z = 0.2; - - // First, we add the object to the world (without using a vector). - object_to_attach.primitives.push_back(cylinder_primitive); - object_to_attach.primitive_poses.push_back(grab_pose); - object_to_attach.operation = object_to_attach.ADD; - planning_scene_interface.applyCollisionObject(object_to_attach); - - // Then, we "attach" the object to the robot. It uses the frame_id to determine which robot link it is attached to. - // We also need to tell MoveIt that the object is allowed to be in collision with the finger links of the gripper. - // You could also use applyAttachedCollisionObject to attach an object to the robot directly. - RCLCPP_INFO(LOGGER, "Attach the object to the robot"); - std::vector touch_links; - touch_links.push_back("panda_rightfinger"); - touch_links.push_back("panda_leftfinger"); - move_group.attachObject(object_to_attach.id, "panda_hand", touch_links); - - visual_tools.publishText(text_pose, "Object_attached_to_robot", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - - /* Wait for MoveGroup to receive and process the attached collision object message */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is attached to the robot"); - - // Replan, but now with the object in hand. - move_group.setStartStateToCurrentState(); - success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); - RCLCPP_INFO(LOGGER, "Visualizing plan 7 (move around cuboid with cylinder) %s", success ? "" : "FAILED"); - visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group); - visual_tools.trigger(); - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the plan is complete"); - - // The result may look something like this: - // - // .. image:: ./move_group_interface_tutorial_attached_object.gif - // :alt: animation showing the arm moving differently once the object is attached - // - // Detaching and Removing Objects - // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - // - // Now, let's detach the cylinder from the robot's gripper. - RCLCPP_INFO(LOGGER, "Detach the object from the robot"); - move_group.detachObject(object_to_attach.id); - - // Show text in RViz of status - visual_tools.deleteAllMarkers(); - visual_tools.publishText(text_pose, "Object_detached_from_robot", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - - /* Wait for MoveGroup to receive and process the attached collision object message */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window once the new object is detached from the robot"); - - // Now, let's remove the objects from the world. - RCLCPP_INFO(LOGGER, "Remove the objects from the world"); - std::vector object_ids; - object_ids.push_back(collision_object.id); - object_ids.push_back(object_to_attach.id); - planning_scene_interface.removeCollisionObjects(object_ids); - - // Show text in RViz of status - visual_tools.publishText(text_pose, "Objects_removed", rvt::WHITE, rvt::XLARGE); - visual_tools.trigger(); - - /* Wait for MoveGroup to receive and process the attached collision object message */ - visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disappears"); - - // END_TUTORIAL - visual_tools.deleteAllMarkers(); - visual_tools.trigger(); + move_group.asyncExecute(my_plan.trajectory_); + move_group.execute(my_plan); rclcpp::shutdown(); return 0; diff --git a/src/gaz_simulation/launch/test_launch.py b/src/gaz_simulation/launch/test_launch.py index 8b39d6e..57c7a95 100644 --- a/src/gaz_simulation/launch/test_launch.py +++ b/src/gaz_simulation/launch/test_launch.py @@ -42,30 +42,30 @@ def generate_launch_description(): name='lidar_2_broadcaster', arguments=["-1.9", "1", "1", "0", "0", "0", "map", "lidar_2_link"] ), - Node( - package='gazebo_ros', - executable='spawn_entity.py', - arguments=['-entity', robot_name, - '-topic', 'robot_description', - '-x', '-1', - '-y', '-1', - '-z', '1', - '-Y', '0'], # Yaw - output='screen' - ), + #Node( + # package='gazebo_ros', + # executable='spawn_entity.py', + # arguments=['-entity', robot_name, + # '-topic', 'robot_description', + # '-x', '-1', + # '-y', '-1', + # '-z', '1', + # '-Y', '0'], # Yaw + # output='screen' + #), # Subscribe to the joint states of the robot, and publish the 3D pose of each link. - Node( - package='robot_state_publisher', - executable='robot_state_publisher', - parameters=[{'robot_description': Command(['xacro ', urdf_path])}] - ), + #Node( + # package='robot_state_publisher', + # executable='robot_state_publisher', + # parameters=[{'robot_description': Command(['xacro ', urdf_path])}] + #), # Publish the joint states of the robot - Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher' - ) + #Node( + # package='joint_state_publisher', + # executable='joint_state_publisher', + # name='joint_state_publisher' + #) # , # Node( diff --git a/src/iisy_config/launch/lbr_move_group.launch.py b/src/iisy_config/launch/lbr_move_group.launch.py index 95f2470..0028a3c 100644 --- a/src/iisy_config/launch/lbr_move_group.launch.py +++ b/src/iisy_config/launch/lbr_move_group.launch.py @@ -149,6 +149,8 @@ def launch_setup(context, *args, **kwargs): robot_description, robot_description_semantic, kinematics_yaml, + planning, + use_sim_time ], ) diff --git a/src/iisy_config/srdf/iisy.srdf b/src/iisy_config/srdf/iisy.srdf index 4bf3602..620e842 100644 --- a/src/iisy_config/srdf/iisy.srdf +++ b/src/iisy_config/srdf/iisy.srdf @@ -27,6 +27,7 @@ + diff --git a/src/iisy_config/stl/table.stl b/src/iisy_config/stl/table.stl new file mode 100755 index 0000000000000000000000000000000000000000..e71fc4af9efa4f80dc1e2e20b99a9e94528fc504 GIT binary patch literal 1884 zcmbW0Jx;?=5QKdVx)iiTAbLka;wFhJAjqPi!^Z#)V;?Q-?4Re^FNuUDD~sQJGrMc^ z{P}+DKMwm%*B@UuPcMhJ{h_~m+&*mYoBv$51vKn_e{NdbO{4X=s7E|!q)MpDoeE-@ zz|JRJXWn%JT}6i1)|2^rMxH>uAljJYBB3g$r7o{w;uEP`+@6hkEcKFDBUI&1Nz6O0 z_)11@T?o@&Bvj>2nSt(%@^tal_kmqWof%r=3|E!#$(eAqzHh6W>05leuEY1EwY!N1 zuI4^y-UAdF|Cpm;4 zsfUT)IsS$dGREr!sg&T!)kia+iVPxlR6|GK6%nk`Ix^B9D4!Gr<_qWZ1gT{D+z6cK zeesFb?hAk1^31N>u&QEmQYjI07!%*sa&lL(lc&@OWN_Xp13mJ7CVB_j70=P+cXdXk HT^aEQz!9Os literal 0 HcmV?d00001 diff --git a/src/iisy_config/urdf/iisy.urdf b/src/iisy_config/urdf/iisy.urdf index 5c3173f..f440bf2 100644 --- a/src/iisy_config/urdf/iisy.urdf +++ b/src/iisy_config/urdf/iisy.urdf @@ -5,6 +5,18 @@ + + + + + + + + + + + + @@ -22,7 +34,6 @@ - @@ -138,8 +149,8 @@ - + @@ -148,6 +159,11 @@ + + + + +