From 9d5c131eac009ddf4f38c04facd24c0a6f26491e Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 13 Feb 2024 18:45:51 -0500 Subject: [PATCH 01/54] Add initial click ik node --- CMakeLists.txt | 3 +++ src/perception/click_ik/click_ik.cpp | 15 +++++++++++ src/perception/click_ik/click_ik.hpp | 22 +++++++++++++++ src/perception/click_ik/main.cpp | 22 +++++++++++++++ src/perception/click_ik/pch.hpp | 28 ++++++++++++++++++++ src/perception/tag_detector/tag_detector.hpp | 7 ++--- 6 files changed, 94 insertions(+), 3 deletions(-) create mode 100644 src/perception/click_ik/click_ik.cpp create mode 100644 src/perception/click_ik/click_ik.hpp create mode 100644 src/perception/click_ik/main.cpp create mode 100644 src/perception/click_ik/pch.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 65406ed51..a286cf5c0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -189,6 +189,9 @@ endif () mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie) +mrover_add_nodelet(click_ik src/perception/click_ik/*.cpp src/perception/click_ik src/perception/click_ik/pch.hpp) +mrover_nodelet_link_libraries(click_ik lie) + if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) # target_link_libraries(streaming PUBLIC opencv_core opencv_cudacodec) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp new file mode 100644 index 000000000..a702e50f0 --- /dev/null +++ b/src/perception/click_ik/click_ik.cpp @@ -0,0 +1,15 @@ +#include "click_ik.hpp" + +namespace mrover { + + auto ClickIkNodelet::onInit() -> void { + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + + mPcSub = mNh.subscribe("camera/left/points", 1, &ClickIkNodelet::pointCloudCallback, this); + } + + auto ClickIkNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { + } + +} // namespace mrover diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp new file mode 100644 index 000000000..f671e7839 --- /dev/null +++ b/src/perception/click_ik/click_ik.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include "pch.hpp" + +namespace mrover { + + class ClickIkNodelet final : public nodelet::Nodelet { + ros::NodeHandle mNh, mPnh; + + ros::Subscriber mPcSub; + + public: + ClickIkNodelet() = default; + + ~ClickIkNodelet() override = default; + + void onInit() override; + + auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; + }; + +} // namespace mrover diff --git a/src/perception/click_ik/main.cpp b/src/perception/click_ik/main.cpp new file mode 100644 index 000000000..1bef59b30 --- /dev/null +++ b/src/perception/click_ik/main.cpp @@ -0,0 +1,22 @@ +#include "click_ik.hpp" + +#ifdef MROVER_IS_NODELET + +#include +PLUGINLIB_EXPORT_CLASS(mrover::ClickIkNodelet, nodelet::Nodelet) + +#else + +auto main(int argc, char** argv) -> int { + ros::init(argc, argv, "click_ik"); + + // Start the ZED Nodelet + nodelet::Loader nodelet; + nodelet.load(ros::this_node::getName(), "mrover/ClickIkNodelet", ros::names::getRemappings(), {}); + + ros::spin(); + + return EXIT_SUCCESS; +} + +#endif diff --git a/src/perception/click_ik/pch.hpp b/src/perception/click_ik/pch.hpp new file mode 100644 index 000000000..eff7b3fc4 --- /dev/null +++ b/src/perception/click_ik/pch.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index b14541478..5652dcc81 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -1,3 +1,5 @@ +#pragma once + #include "pch.hpp" namespace mrover { @@ -10,7 +12,6 @@ namespace mrover { }; class TagDetectorNodelet : public nodelet::Nodelet { - private: ros::NodeHandle mNh, mPnh; ros::Publisher mImgPub; @@ -44,8 +45,8 @@ namespace mrover { std::vector> mImmediateCorners; std::vector mImmediateIds; std::unordered_map mTags; - dynamic_reconfigure::Server mConfigServer; - dynamic_reconfigure::Server::CallbackType mCallbackType; + dynamic_reconfigure::Server mConfigServer; + dynamic_reconfigure::Server::CallbackType mCallbackType; LoopProfiler mProfiler{"Tag Detector"}; From b81546f46833fd01ca970f05718d092f073322af Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 13 Feb 2024 18:54:55 -0500 Subject: [PATCH 02/54] Add action --- CMakeLists.txt | 12 +++++++++--- action/ClickIk.action | 6 ++++++ src/perception/click_ik/pch.hpp | 2 ++ 3 files changed, 17 insertions(+), 3 deletions(-) create mode 100644 action/ClickIk.action diff --git a/CMakeLists.txt b/CMakeLists.txt index a286cf5c0..2e239ce5b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,15 +54,19 @@ set(MROVER_ROS_DEPENDENCIES tf2 tf2_ros tf2_geometry_msgs + actionlib_msgs ) extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) +extract_filenames(action/*.action MROVER_ACTION_FILES) + set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs + actionlib_msgs ) set(MROVER_PARAMETERS @@ -122,14 +126,16 @@ endforeach () set(BUILD_SHARED_LIBS ON) -### ======== ### -### Messages ### -### ======== ### +### ================================ ### +### Messages & Actions & Reconfigure ### +### ================================ ### add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) + generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) generate_dynamic_reconfigure_options(${MROVER_PARAMETERS}) diff --git a/action/ClickIk.action b/action/ClickIk.action new file mode 100644 index 000000000..d17ea39ff --- /dev/null +++ b/action/ClickIk.action @@ -0,0 +1,6 @@ +uint32 pointInImageX +uint32 pointInImageY +--- +bool success +--- +float32 distance diff --git a/src/perception/click_ik/pch.hpp b/src/perception/click_ik/pch.hpp index eff7b3fc4..90117288b 100644 --- a/src/perception/click_ik/pch.hpp +++ b/src/perception/click_ik/pch.hpp @@ -26,3 +26,5 @@ #include #include #include + +#include From c65b102d93f29fbd7b79c721456a162b07abaeef Mon Sep 17 00:00:00 2001 From: Charlie Zhong Date: Tue, 13 Feb 2024 19:53:53 -0500 Subject: [PATCH 03/54] Save point cloud, create IK publisher --- src/perception/click_ik/click_ik.cpp | 9 +++++++++ src/perception/click_ik/click_ik.hpp | 7 +++++++ 2 files changed, 16 insertions(+) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index a702e50f0..b0a7bc634 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -1,4 +1,5 @@ #include "click_ik.hpp" +#include "mrover/IK.h" namespace mrover { @@ -7,9 +8,17 @@ namespace mrover { mPnh = getMTPrivateNodeHandle(); mPcSub = mNh.subscribe("camera/left/points", 1, &ClickIkNodelet::pointCloudCallback, this); + // IK Publisher + mPcPub = mNh.advertise("/arm_ik", 1); } auto ClickIkNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { + // Save points from point cloud + points = reinterpret_cast(msg->data.data()); + } + + void executeGoal(auto g) { + } } // namespace mrover diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index f671e7839..ecaf07c87 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -9,6 +9,11 @@ namespace mrover { ros::Subscriber mPcSub; + // IK publisher + ros::Publisher mPcPub; + + const Point* points{}; + public: ClickIkNodelet() = default; @@ -17,6 +22,8 @@ namespace mrover { void onInit() override; auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; + + void executeGoal(auto g); }; } // namespace mrover From 1771f20ff1101fcea91be9fe32805febfed18ebf Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 14 Feb 2024 12:00:47 -0500 Subject: [PATCH 04/54] Add plugin files so the nodelet can get loaded --- package.xml | 1 + plugins/click_ik.xml | 6 ++++++ 2 files changed, 7 insertions(+) create mode 100644 plugins/click_ik.xml diff --git a/package.xml b/package.xml index 0839b8d8f..fb371b46b 100644 --- a/package.xml +++ b/package.xml @@ -84,5 +84,6 @@ + \ No newline at end of file diff --git a/plugins/click_ik.xml b/plugins/click_ik.xml new file mode 100644 index 000000000..ffed8d6b4 --- /dev/null +++ b/plugins/click_ik.xml @@ -0,0 +1,6 @@ + + + + From b63073f54be169517d53d5fe13516cfd0129261e Mon Sep 17 00:00:00 2001 From: Charlie Zhong Date: Thu, 15 Feb 2024 18:34:12 -0500 Subject: [PATCH 05/54] Initialize action server --- src/perception/click_ik/click_ik.cpp | 5 +---- src/perception/click_ik/main.cpp | 6 +++++- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index b0a7bc634..5a937de19 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -17,8 +17,5 @@ namespace mrover { points = reinterpret_cast(msg->data.data()); } - void executeGoal(auto g) { - - } - + } // namespace mrover diff --git a/src/perception/click_ik/main.cpp b/src/perception/click_ik/main.cpp index 1bef59b30..d4a2689d9 100644 --- a/src/perception/click_ik/main.cpp +++ b/src/perception/click_ik/main.cpp @@ -5,7 +5,7 @@ #include PLUGINLIB_EXPORT_CLASS(mrover::ClickIkNodelet, nodelet::Nodelet) -#else +// #else auto main(int argc, char** argv) -> int { ros::init(argc, argv, "click_ik"); @@ -13,6 +13,10 @@ auto main(int argc, char** argv) -> int { // Start the ZED Nodelet nodelet::Loader nodelet; nodelet.load(ros::this_node::getName(), "mrover/ClickIkNodelet", ros::names::getRemappings(), {}); + // Start ActionServer + ros::NodeHandle n; + Server server(n, "do_click_ik", boost::bind(&execute, _1, &server), false); + server.start(); ros::spin(); From 1278bcff5079fa56f219318bb68f1198875d4838 Mon Sep 17 00:00:00 2001 From: Charlie Zhong Date: Thu, 15 Feb 2024 19:33:10 -0500 Subject: [PATCH 06/54] Add action client script --- scripts/debug_click_ik.py | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) create mode 100644 scripts/debug_click_ik.py diff --git a/scripts/debug_click_ik.py b/scripts/debug_click_ik.py new file mode 100644 index 000000000..8865819a3 --- /dev/null +++ b/scripts/debug_click_ik.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 + +import rospy +from mrover.msg import ClickIkAction, ClickIkActionGoal, ClickIkActionFeedback, ClickIkActionResult +import actionlib +import sys + +def click_ik_client(): + # Creates the SimpleActionClient, passing the type of the action + # (FibonacciAction) to the constructor. + client = actionlib.SimpleActionClient('click-ik', ClickIkAction) + # Waits until the action server has started up and started + # listening for goals. + client.wait_for_server() + # Creates a goal to send to the action server. + goal = ClickIkActionGoal(pointInImageX=0, pointInImageY=0) + # Sends the goal to the action server. + client.send_goal(goal) + # Waits for the server to finish performing the action. + client.wait_for_result() + # Prints out the result of executing the action + return client.get_result() # A FibonacciResult + +if __name__ == "__main__": + try: + # Initializes a rospy node so that the SimpleActionClient can + # publish and subscribe over ROS. + rospy.init_node('debug_click_ik') + result = click_ik_client() + print("Result:", ', '.join([str(n) for n in result.sequence])) + except rospy.ROSInterruptException: + print("program interrupted before completion", file=sys.stderr) \ No newline at end of file From be76d77e1359f374a0468688aae7e68bdc6d76f4 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 18 Feb 2024 12:54:55 -0500 Subject: [PATCH 07/54] working on limits --- src/simulator/simulator.cpp | 2 +- .../arm_controller/arm_controller.cpp | 35 +++++++++++++++++-- 2 files changed, 34 insertions(+), 3 deletions(-) diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index f3a536b2a..562ecadf3 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -69,7 +69,7 @@ namespace mrover { #else glfwPollEvents(); #endif - // glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED); + glfwSetInputMode(mWindow.get(), GLFW_CURSOR, mInGui ? GLFW_CURSOR_NORMAL : GLFW_CURSOR_DISABLED); mLoopProfiler.measureEvent("GLFW Events"); userControls(dt); diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index 986e17644..a72cff8dd 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -7,8 +7,19 @@ namespace mrover { // From: arm.urdf.xacro constexpr double LINK_AB = 0.58; constexpr double LINK_BC = 0.55; + constexpr double LINK_CD = 0.142; constexpr double TAU = std::numbers::pi * 2; double const OFFSET = std::atan2(0.09, LINK_BC); + constexpr double JOINT_A_MIN = -1; + constexpr double JOINT_A_MAX = 1; + constexpr double JOINT_B_MIN = -0.5 * std::numbers::pi; + constexpr double JOINT_B_MAX = 0; + constexpr double JOINT_C_MIN = 0; + constexpr double JOINT_C_MAX = std::numbers::pi; + constexpr double JOINT_DE_PITCH_MIN = -1 * std::numbers::pi; + constexpr double JOINT_DE_PITCH_MAX = std::numbers::pi; + constexpr double JOINT_DE_ROLL_MIN = -1 * std::numbers::pi; + constexpr double JOINT_DE_ROLL_MAX = std::numbers::pi; // Subscribers @@ -47,12 +58,32 @@ namespace mrover { double C = x * x + z * z - LINK_AB * LINK_AB - LINK_BC * LINK_BC; double q2 = std::acos(C / (2 * LINK_AB * LINK_BC)); double q1 = std::atan2(z, x) - std::atan2(LINK_BC * std::sin(q2), LINK_AB + LINK_BC * std::cos(q2)); - q1 = std::clamp(q1, -TAU / 8, 0.0); + // q1 = std::clamp(q1, -TAU / 8, 0.0); double q3 = -(q1 + q2); // ROS_INFO("x: %f, y: %f, z: %f, q1: %f, q2: %f, q3: %f", x, y, z, q1, q2, q3); - if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3)) { + // double gamma = 0; + // double x3 = x - LINK_CD * cos(gamma); + // double z3 = z - LINK_CD * sin(gamma); + + // double C = sqrt(x3 * x3 + z3 * z3); + // double alpha = acos((LINK_AB * LINK_AB + LINK_BC * LINK_BC - C * C) / (2 * LINK_AB * LINK_BC)); + // double beta = acos((LINK_AB * LINK_AB + C * C - LINK_BC * LINK_BC) / (2 * LINK_AB * C)); + // double thetaA = atan(z3 / x3) + beta; + // double thetaB = -1 * (std::numbers::pi - alpha); + // double thetaC = gamma - thetaA - thetaB; + + // double q1 = -thetaA; + // double q2 = -thetaB; + // double q3 = -thetaC; + + if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3) && + y >= JOINT_A_MIN && y <= JOINT_A_MAX && + q1 >= JOINT_B_MIN && q1 <= JOINT_B_MAX && + q2 >= JOINT_C_MIN && q2 <= JOINT_C_MAX && + q3 >= JOINT_DE_PITCH_MIN && q3 <= JOINT_DE_PITCH_MAX + ) { positions.positions[0] = static_cast(y); positions.positions[1] = static_cast(q1); positions.positions[2] = static_cast(q2); From f04aebcf7ae8f0e4fb27a18f4d07d37b57e48929 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 18 Feb 2024 14:14:08 -0500 Subject: [PATCH 08/54] added arm_targeet to tf tree --- CMakeLists.txt | 5 +- src/perception/click_ik/main.cpp | 4 +- src/perception/click_ik/pch.hpp | 2 +- .../arm_controller/arm_controller.cpp | 70 +++++++++++-------- .../arm_controller/arm_controller.hpp | 19 ++++- src/teleoperation/arm_controller/pch.hpp | 1 + 6 files changed, 64 insertions(+), 37 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 26bc53e74..e58a2c801 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -196,8 +196,8 @@ endif () mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif) -mrover_add_nodelet(click_ik src/perception/click_ik/*.cpp src/perception/click_ik src/perception/click_ik/pch.hpp) -mrover_nodelet_link_libraries(click_ik lie) +# mrover_add_nodelet(click_ik src/perception/click_ik/*.cpp src/perception/click_ik src/perception/click_ik/pch.hpp) +# mrover_nodelet_link_libraries(click_ik lie) if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) @@ -220,6 +220,7 @@ endif () ## Teleoperation mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp) +target_link_libraries(arm_controller PRIVATE lie) ## Simulator diff --git a/src/perception/click_ik/main.cpp b/src/perception/click_ik/main.cpp index d4a2689d9..2d7ec6f7b 100644 --- a/src/perception/click_ik/main.cpp +++ b/src/perception/click_ik/main.cpp @@ -15,8 +15,8 @@ auto main(int argc, char** argv) -> int { nodelet.load(ros::this_node::getName(), "mrover/ClickIkNodelet", ros::names::getRemappings(), {}); // Start ActionServer ros::NodeHandle n; - Server server(n, "do_click_ik", boost::bind(&execute, _1, &server), false); - server.start(); + // Server server(n, "do_click_ik", boost::bind(&execute, _1, &server), false); + // server.start(); ros::spin(); diff --git a/src/perception/click_ik/pch.hpp b/src/perception/click_ik/pch.hpp index 90117288b..bc5c2b45e 100644 --- a/src/perception/click_ik/pch.hpp +++ b/src/perception/click_ik/pch.hpp @@ -25,6 +25,6 @@ #include #include -#include +#include #include diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index a72cff8dd..971b32e45 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -1,4 +1,7 @@ #include "arm_controller.hpp" +#include +#include +#include namespace mrover { @@ -23,30 +26,29 @@ namespace mrover { // Subscribers - [[maybe_unused]] ros::Subscriber ik_subscriber; // Publishers - ros::Publisher position_publisher; + // Private state - auto run(int argc, char** argv) -> int { - ros::init(argc, argv, "arm_controller"); - ros::NodeHandle nh; + // auto ArmController::run(int argc, char** argv) -> int { + // ros::init(argc, argv, "arm_controller"); + // ros::NodeHandle nh; - double frequency{}; - nh.param("/frequency", frequency, 100); + // double frequency{}; + // nh.param("/frequency", frequency, 100); - ik_subscriber = nh.subscribe("arm_ik", 1, ik_callback); - position_publisher = nh.advertise("arm_position_cmd", 1); + // ik_subscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); + // position_publisher = nh.advertise("arm_position_cmd", 1); - ros::spin(); + // ros::spin(); - return EXIT_SUCCESS; - } + // return EXIT_SUCCESS; + // } - auto ik_callback(IK const& ik_target) -> void { + auto ArmController::ik_callback(IK const& ik_target) -> void { Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; positions.positions.resize(positions.names.size(), 0.f); @@ -54,29 +56,31 @@ namespace mrover { double x = ik_target.pose.position.x; double y = ik_target.pose.position.y; double z = ik_target.pose.position.z; + SE3d pos{{x, y, z}, SO3d::Identity()}; + SE3Conversions::pushToTfTree(tfBroadcaster, "arm_target", "chassis_link", pos); - double C = x * x + z * z - LINK_AB * LINK_AB - LINK_BC * LINK_BC; - double q2 = std::acos(C / (2 * LINK_AB * LINK_BC)); - double q1 = std::atan2(z, x) - std::atan2(LINK_BC * std::sin(q2), LINK_AB + LINK_BC * std::cos(q2)); - // q1 = std::clamp(q1, -TAU / 8, 0.0); - double q3 = -(q1 + q2); + // double C = x * x + z * z - LINK_AB * LINK_AB - LINK_BC * LINK_BC; + // double q2 = std::acos(C / (2 * LINK_AB * LINK_BC)); + // double q1 = std::atan2(z, x) - std::atan2(LINK_BC * std::sin(q2), LINK_AB + LINK_BC * std::cos(q2)); + // // q1 = std::clamp(q1, -TAU / 8, 0.0); + // double q3 = -(q1 + q2); // ROS_INFO("x: %f, y: %f, z: %f, q1: %f, q2: %f, q3: %f", x, y, z, q1, q2, q3); - // double gamma = 0; - // double x3 = x - LINK_CD * cos(gamma); - // double z3 = z - LINK_CD * sin(gamma); + double gamma = 0; + double x3 = x - LINK_CD * cos(gamma); + double z3 = z - LINK_CD * sin(gamma); - // double C = sqrt(x3 * x3 + z3 * z3); - // double alpha = acos((LINK_AB * LINK_AB + LINK_BC * LINK_BC - C * C) / (2 * LINK_AB * LINK_BC)); - // double beta = acos((LINK_AB * LINK_AB + C * C - LINK_BC * LINK_BC) / (2 * LINK_AB * C)); - // double thetaA = atan(z3 / x3) + beta; - // double thetaB = -1 * (std::numbers::pi - alpha); - // double thetaC = gamma - thetaA - thetaB; + double C = sqrt(x3 * x3 + z3 * z3); + double alpha = acos((LINK_AB * LINK_AB + LINK_BC * LINK_BC - C * C) / (2 * LINK_AB * LINK_BC)); + double beta = acos((LINK_AB * LINK_AB + C * C - LINK_BC * LINK_BC) / (2 * LINK_AB * C)); + double thetaA = atan(z3 / x3) + beta; + double thetaB = -1 * (std::numbers::pi - alpha); + double thetaC = gamma - thetaA - thetaB; - // double q1 = -thetaA; - // double q2 = -thetaB; - // double q3 = -thetaC; + double q1 = -thetaA; + double q2 = -thetaB; + double q3 = -thetaC; if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3) && y >= JOINT_A_MIN && y <= JOINT_A_MAX && @@ -96,5 +100,9 @@ namespace mrover { } // namespace mrover auto main(int argc, char** argv) -> int { - return mrover::run(argc, argv); + ros::init(argc, argv, "arm_controller"); + mrover::ArmController arm_controller{}; + + ros::spin(); + // return arm_controller.run(argc, argv); } diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index e58f95979..c2baf9c94 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -1,9 +1,26 @@ #pragma once #include "pch.hpp" +#include namespace mrover { - void ik_callback(IK const& new_ik_target); + class ArmController { + [[maybe_unused]] ros::Subscriber ik_subscriber; + ros::Publisher position_publisher; + tf2_ros::TransformBroadcaster tfBroadcaster; + + public: + ArmController() : tfBroadcaster() { + ros::NodeHandle nh; + double frequency{}; + nh.param("/frequency", frequency, 100); + ik_subscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); + position_publisher = nh.advertise("arm_position_cmd", 1); + } + void ik_callback(IK const& new_ik_target); + // auto run(int argc, char** argv) -> int; + }; + // void ik_callback(IK const& new_ik_target); } // namespace mrover diff --git a/src/teleoperation/arm_controller/pch.hpp b/src/teleoperation/arm_controller/pch.hpp index 5acfb2242..20e0454a1 100644 --- a/src/teleoperation/arm_controller/pch.hpp +++ b/src/teleoperation/arm_controller/pch.hpp @@ -10,3 +10,4 @@ #include #include +#include From 43e7d2b586c4ae1379fa4fee38aa46e8eae9a997 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 18 Feb 2024 14:55:39 -0500 Subject: [PATCH 09/54] worked on stuff --- src/teleoperation/arm_controller/arm_controller.cpp | 11 ++++++++--- src/teleoperation/arm_controller/arm_controller.hpp | 4 ++++ 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index 971b32e45..b5894b620 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -8,9 +8,9 @@ namespace mrover { // Constants // From: arm.urdf.xacro - constexpr double LINK_AB = 0.58; - constexpr double LINK_BC = 0.55; - constexpr double LINK_CD = 0.142; + constexpr double LINK_AB = 0.534; + constexpr double LINK_BC = 0.553; + constexpr double LINK_CD = 0.044886000454425812; constexpr double TAU = std::numbers::pi * 2; double const OFFSET = std::atan2(0.09, LINK_BC); constexpr double JOINT_A_MIN = -1; @@ -59,6 +59,11 @@ namespace mrover { SE3d pos{{x, y, z}, SO3d::Identity()}; SE3Conversions::pushToTfTree(tfBroadcaster, "arm_target", "chassis_link", pos); + // SE3d link_ab = SE3Conversions::fromTfTree(buffer, "arm_a_link", "arm_b_link"); + // SE3d link_bc = SE3Conversions::fromTfTree(buffer, "arm_b_link", "arm_c_link"); + // SE3d link_cd = SE3Conversions::fromTfTree(buffer, "arm_c_link", "arm_d_link"); + // SE3d link_de = SE3Conversions::fromTfTree(buffer, "arm_d_link", "arm_e_link"); + // double C = x * x + z * z - LINK_AB * LINK_AB - LINK_BC * LINK_BC; // double q2 = std::acos(C / (2 * LINK_AB * LINK_BC)); // double q1 = std::atan2(z, x) - std::atan2(LINK_BC * std::sin(q2), LINK_AB + LINK_BC * std::cos(q2)); diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index c2baf9c94..c7f2e20f5 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -2,6 +2,7 @@ #include "pch.hpp" #include +#include namespace mrover { @@ -9,6 +10,9 @@ namespace mrover { [[maybe_unused]] ros::Subscriber ik_subscriber; ros::Publisher position_publisher; tf2_ros::TransformBroadcaster tfBroadcaster; + tf2_ros::Buffer buffer{}; + tf2_ros::TransformListener mTfListener{buffer}; + public: ArmController() : tfBroadcaster() { From 92495b7a78efdb5d0ab404e51981f6a10e5b39e4 Mon Sep 17 00:00:00 2001 From: RobbieGM Date: Thu, 15 Feb 2024 19:48:16 -0500 Subject: [PATCH 10/54] Start work on click ik action server --- src/perception/click_ik/click_ik.cpp | 17 +++++++++++++---- src/perception/click_ik/click_ik.hpp | 9 ++++++--- src/perception/click_ik/main.cpp | 9 ++------- 3 files changed, 21 insertions(+), 14 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 5a937de19..d015f2a27 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -1,20 +1,29 @@ +#include #include "click_ik.hpp" #include "mrover/IK.h" namespace mrover { - auto ClickIkNodelet::onInit() -> void { + void ClickIkNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); mPcSub = mNh.subscribe("camera/left/points", 1, &ClickIkNodelet::pointCloudCallback, this); // IK Publisher - mPcPub = mNh.advertise("/arm_ik", 1); + mIkPub = mNh.advertise("/arm_ik", 1); + + // Start ActionServer + actionlib::SimpleActionServer server(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) { + // How to pass server to callback? It will go out of scope and can't be assigned to a member variable because actionlib::SimpleActionServer is non-copyable + }, false); + server.start(); + } - auto ClickIkNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void { + void ClickIkNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { // Save points from point cloud - points = reinterpret_cast(msg->data.data()); + mPoints = reinterpret_cast(msg->data.data()); + mNumPoints = msg->width * msg->height; } diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index ecaf07c87..e6df8d2ba 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -1,6 +1,7 @@ #pragma once #include "pch.hpp" +#include namespace mrover { @@ -10,9 +11,11 @@ namespace mrover { ros::Subscriber mPcSub; // IK publisher - ros::Publisher mPcPub; + ros::Publisher mIkPub; - const Point* points{}; + const Point* mPoints{}; + + std::size_t mNumPoints{}; public: ClickIkNodelet() = default; @@ -23,7 +26,7 @@ namespace mrover { auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; - void executeGoal(auto g); + void executeGoal(); }; } // namespace mrover diff --git a/src/perception/click_ik/main.cpp b/src/perception/click_ik/main.cpp index 2d7ec6f7b..176cd6250 100644 --- a/src/perception/click_ik/main.cpp +++ b/src/perception/click_ik/main.cpp @@ -5,19 +5,14 @@ #include PLUGINLIB_EXPORT_CLASS(mrover::ClickIkNodelet, nodelet::Nodelet) -// #else +#else auto main(int argc, char** argv) -> int { ros::init(argc, argv, "click_ik"); - // Start the ZED Nodelet + // Start the ClickIK Nodelet nodelet::Loader nodelet; nodelet.load(ros::this_node::getName(), "mrover/ClickIkNodelet", ros::names::getRemappings(), {}); - // Start ActionServer - ros::NodeHandle n; - // Server server(n, "do_click_ik", boost::bind(&execute, _1, &server), false); - // server.start(); - ros::spin(); return EXIT_SUCCESS; From 70f22ba4f1bff39c12170c0e07aa60dca5c2cc56 Mon Sep 17 00:00:00 2001 From: RobbieGM Date: Tue, 20 Feb 2024 18:47:12 -0500 Subject: [PATCH 11/54] Fix some limits and rename stuff in arm_controller.cpp --- .../arm_controller/arm_controller.cpp | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index b5894b620..724fdeed8 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -7,22 +7,22 @@ namespace mrover { // Constants - // From: arm.urdf.xacro - constexpr double LINK_AB = 0.534; - constexpr double LINK_BC = 0.553; - constexpr double LINK_CD = 0.044886000454425812; - constexpr double TAU = std::numbers::pi * 2; - double const OFFSET = std::atan2(0.09, LINK_BC); - constexpr double JOINT_A_MIN = -1; - constexpr double JOINT_A_MAX = 1; - constexpr double JOINT_B_MIN = -0.5 * std::numbers::pi; + // From: rover.urdf.xacro + // A is the prismatic joint, B is the first revolute joint, C is the second revolute joint + constexpr double LINK_BC = 0.534; + constexpr double LINK_CD = 0.553; + constexpr double LINK_DE = 0.044886000454425812; + double const OFFSET = std::atan2(0.09, LINK_CD); + constexpr double JOINT_A_MIN = -0.45; + constexpr double JOINT_A_MAX = 0; + constexpr double JOINT_B_MIN = -0.25 * std::numbers::pi; constexpr double JOINT_B_MAX = 0; - constexpr double JOINT_C_MIN = 0; - constexpr double JOINT_C_MAX = std::numbers::pi; - constexpr double JOINT_DE_PITCH_MIN = -1 * std::numbers::pi; - constexpr double JOINT_DE_PITCH_MAX = std::numbers::pi; - constexpr double JOINT_DE_ROLL_MIN = -1 * std::numbers::pi; - constexpr double JOINT_DE_ROLL_MAX = std::numbers::pi; + constexpr double JOINT_C_MIN = -0.959931; + constexpr double JOINT_C_MAX = 2.87979; + constexpr double JOINT_DE_PITCH_MIN = -0.75 * std::numbers::pi; + constexpr double JOINT_DE_PITCH_MAX = 0.75 * std::numbers::pi; + // constexpr double JOINT_DE_ROLL_MIN = -0.75 * std::numbers::pi; + // constexpr double JOINT_DE_ROLL_MAX = 0.75 * std::numbers::pi; // Subscribers @@ -73,12 +73,12 @@ namespace mrover { // ROS_INFO("x: %f, y: %f, z: %f, q1: %f, q2: %f, q3: %f", x, y, z, q1, q2, q3); double gamma = 0; - double x3 = x - LINK_CD * cos(gamma); - double z3 = z - LINK_CD * sin(gamma); + double x3 = x - LINK_DE * cos(gamma); + double z3 = z - LINK_DE * sin(gamma); double C = sqrt(x3 * x3 + z3 * z3); - double alpha = acos((LINK_AB * LINK_AB + LINK_BC * LINK_BC - C * C) / (2 * LINK_AB * LINK_BC)); - double beta = acos((LINK_AB * LINK_AB + C * C - LINK_BC * LINK_BC) / (2 * LINK_AB * C)); + double alpha = acos((LINK_BC * LINK_BC + LINK_CD * LINK_CD - C * C) / (2 * LINK_BC * LINK_CD)); + double beta = acos((LINK_BC * LINK_BC + C * C - LINK_CD * LINK_CD) / (2 * LINK_BC * C)); double thetaA = atan(z3 / x3) + beta; double thetaB = -1 * (std::numbers::pi - alpha); double thetaC = gamma - thetaA - thetaB; From 659318dceb0b0837a4b58548a5c6f2487087ff37 Mon Sep 17 00:00:00 2001 From: Charlie Zhong Date: Thu, 22 Feb 2024 18:31:37 -0500 Subject: [PATCH 12/54] server client --- CMakeLists.txt | 4 ++-- scripts/debug_click_ik.py | 9 +++------ src/perception/click_ik/click_ik.cpp | 17 ++++++++++++++--- 3 files changed, 19 insertions(+), 11 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e58a2c801..53a224637 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -196,8 +196,8 @@ endif () mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif) -# mrover_add_nodelet(click_ik src/perception/click_ik/*.cpp src/perception/click_ik src/perception/click_ik/pch.hpp) -# mrover_nodelet_link_libraries(click_ik lie) +mrover_add_nodelet(click_ik src/perception/click_ik/*.cpp src/perception/click_ik src/perception/click_ik/pch.hpp) +mrover_nodelet_link_libraries(click_ik lie) if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) diff --git a/scripts/debug_click_ik.py b/scripts/debug_click_ik.py index 8865819a3..8892b7cec 100644 --- a/scripts/debug_click_ik.py +++ b/scripts/debug_click_ik.py @@ -8,18 +8,15 @@ def click_ik_client(): # Creates the SimpleActionClient, passing the type of the action # (FibonacciAction) to the constructor. - client = actionlib.SimpleActionClient('click-ik', ClickIkAction) + client = actionlib.SimpleActionClient('do_click_ik', ClickIkAction) # Waits until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. - goal = ClickIkActionGoal(pointInImageX=0, pointInImageY=0) + goal = ClickIkActionGoal() # Sends the goal to the action server. client.send_goal(goal) - # Waits for the server to finish performing the action. - client.wait_for_result() - # Prints out the result of executing the action - return client.get_result() # A FibonacciResult + client.wait_for_result(rospy.Duration.from_sec(5.0)) if __name__ == "__main__": try: diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index d015f2a27..7426ef648 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -1,9 +1,16 @@ #include #include "click_ik.hpp" +#include "mrover/ClickIkAction.h" +#include "mrover/ClickIkGoal.h" #include "mrover/IK.h" namespace mrover { + typedef actionlib::SimpleActionServer Server; + void execute(const mrover::ClickIkGoalConstPtr& goal, Server* as) { + as->setSucceeded(); + } + void ClickIkNodelet::onInit() { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); @@ -13,9 +20,12 @@ namespace mrover { mIkPub = mNh.advertise("/arm_ik", 1); // Start ActionServer - actionlib::SimpleActionServer server(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) { - // How to pass server to callback? It will go out of scope and can't be assigned to a member variable because actionlib::SimpleActionServer is non-copyable - }, false); + // actionlib::SimpleActionServer server(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) { + // // How to pass server to callback? It will go out of scope and can't be assigned to a member variable because actionlib::SimpleActionServer is non-copyable + // }, false); + // server.start();[capture0 = &server](auto && PH1) { return execute(std::forward(PH1), capture0); } + // Server server(mNh, "do_click_ik", [capture0 = &server](auto && PH1) { return execute(std::forward(PH1), capture0); }, false); + Server server(mNh, "do_click_ik", [&server](auto && PH1) { return execute(std::forward(PH1), server); }, false); server.start(); } @@ -26,5 +36,6 @@ namespace mrover { mNumPoints = msg->width * msg->height; } + } // namespace mrover From 24cea9e4481a12e4de6b151f7ad3c455f16b71fd Mon Sep 17 00:00:00 2001 From: Charlie Zhong Date: Thu, 22 Feb 2024 18:51:04 -0500 Subject: [PATCH 13/54] fixed compile bug --- src/perception/click_ik/click_ik.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 7426ef648..9e109e3bc 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -24,8 +24,8 @@ namespace mrover { // // How to pass server to callback? It will go out of scope and can't be assigned to a member variable because actionlib::SimpleActionServer is non-copyable // }, false); // server.start();[capture0 = &server](auto && PH1) { return execute(std::forward(PH1), capture0); } - // Server server(mNh, "do_click_ik", [capture0 = &server](auto && PH1) { return execute(std::forward(PH1), capture0); }, false); - Server server(mNh, "do_click_ik", [&server](auto && PH1) { return execute(std::forward(PH1), server); }, false); + Server server(mNh, "do_click_ik", [capture0 = &server](auto && PH1) { return execute(std::forward(PH1), capture0); }, false); + // Server server(mNh, "do_click_ik", [&server](auto && PH1) { return execute(std::forward(PH1), server); }, false); server.start(); } From 16aece7ccf65e67cff10de90df4cfa3808f91169 Mon Sep 17 00:00:00 2001 From: RobbieGM Date: Thu, 22 Feb 2024 19:31:23 -0500 Subject: [PATCH 14/54] click ik action server --- src/perception/click_ik/click_ik.cpp | 13 +++---------- src/perception/click_ik/click_ik.hpp | 6 +++++- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 9e109e3bc..20f1ce2c6 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -1,3 +1,4 @@ +#include #include #include "click_ik.hpp" #include "mrover/ClickIkAction.h" @@ -5,10 +6,9 @@ #include "mrover/IK.h" namespace mrover { - typedef actionlib::SimpleActionServer Server; - void execute(const mrover::ClickIkGoalConstPtr& goal, Server* as) { - as->setSucceeded(); + void ClickIkNodelet::execute(const mrover::ClickIkGoalConstPtr& goal) { + ROS_INFO("Executing goal"); } void ClickIkNodelet::onInit() { @@ -19,13 +19,6 @@ namespace mrover { // IK Publisher mIkPub = mNh.advertise("/arm_ik", 1); - // Start ActionServer - // actionlib::SimpleActionServer server(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) { - // // How to pass server to callback? It will go out of scope and can't be assigned to a member variable because actionlib::SimpleActionServer is non-copyable - // }, false); - // server.start();[capture0 = &server](auto && PH1) { return execute(std::forward(PH1), capture0); } - Server server(mNh, "do_click_ik", [capture0 = &server](auto && PH1) { return execute(std::forward(PH1), capture0); }, false); - // Server server(mNh, "do_click_ik", [&server](auto && PH1) { return execute(std::forward(PH1), server); }, false); server.start(); } diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index e6df8d2ba..7a951fabc 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -1,5 +1,6 @@ #pragma once +#include "mrover/ClickIkAction.h" #include "pch.hpp" #include @@ -13,6 +14,9 @@ namespace mrover { // IK publisher ros::Publisher mIkPub; + actionlib::SimpleActionServer server = actionlib::SimpleActionServer(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) { + execute(goal); + }, false); const Point* mPoints{}; std::size_t mNumPoints{}; @@ -26,7 +30,7 @@ namespace mrover { auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; - void executeGoal(); + void execute(const mrover::ClickIkGoalConstPtr& goal); }; } // namespace mrover From 1f126038a96e884212aef180603a0de1f542b099 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Thu, 22 Feb 2024 21:30:10 -0500 Subject: [PATCH 15/54] fixed ik --- .../arm_controller/arm_controller.cpp | 38 +++++++++++++------ 1 file changed, 27 insertions(+), 11 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index 724fdeed8..f5a445084 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -1,5 +1,7 @@ #include "arm_controller.hpp" +#include #include +#include #include #include @@ -9,10 +11,10 @@ namespace mrover { // From: rover.urdf.xacro // A is the prismatic joint, B is the first revolute joint, C is the second revolute joint - constexpr double LINK_BC = 0.534; - constexpr double LINK_CD = 0.553; + constexpr double LINK_BC = 0.5344417294; + constexpr double LINK_CD = 0.5531735368; constexpr double LINK_DE = 0.044886000454425812; - double const OFFSET = std::atan2(0.09, LINK_CD); + // double const OFFSET = std::atan2(0.09, LINK_CD); constexpr double JOINT_A_MIN = -0.45; constexpr double JOINT_A_MAX = 0; constexpr double JOINT_B_MIN = -0.25 * std::numbers::pi; @@ -56,17 +58,21 @@ namespace mrover { double x = ik_target.pose.position.x; double y = ik_target.pose.position.y; double z = ik_target.pose.position.z; - SE3d pos{{x, y, z}, SO3d::Identity()}; - SE3Conversions::pushToTfTree(tfBroadcaster, "arm_target", "chassis_link", pos); + SE3d pos{{x + 0.034346, 0, z + 0.049024}, SO3d::Identity()}; + SE3Conversions::pushToTfTree(tfBroadcaster, "arm_target", "arm_a_link", pos); // SE3d link_ab = SE3Conversions::fromTfTree(buffer, "arm_a_link", "arm_b_link"); // SE3d link_bc = SE3Conversions::fromTfTree(buffer, "arm_b_link", "arm_c_link"); // SE3d link_cd = SE3Conversions::fromTfTree(buffer, "arm_c_link", "arm_d_link"); // SE3d link_de = SE3Conversions::fromTfTree(buffer, "arm_d_link", "arm_e_link"); - // double C = x * x + z * z - LINK_AB * LINK_AB - LINK_BC * LINK_BC; - // double q2 = std::acos(C / (2 * LINK_AB * LINK_BC)); - // double q1 = std::atan2(z, x) - std::atan2(LINK_BC * std::sin(q2), LINK_AB + LINK_BC * std::cos(q2)); + // SE3d offset = SE3Conversions::fromTfTree(buffer, "chassis_link", "arm_e_link").inverse() * pos; + // ROS_INFO("x: %f, y: %f, z: %f", offset.translation().x(), offset.translation().y(), offset.translation().z()); + // SE3d thing = SE3Conversions::fromTfTree(buffer, "arm_c_link", "arm_d_link"); + + // double C = x * x + z * z - LINK_BC * LINK_BC - LINK_CD * LINK_CD; + // double q2 = std::acos(C / (2 * LINK_BC * LINK_CD)); + // double q1 = std::atan2(z, x) - std::atan2(LINK_BC * std::sin(q2), LINK_BC + LINK_CD * std::cos(q2)); // // q1 = std::clamp(q1, -TAU / 8, 0.0); // double q3 = -(q1 + q2); @@ -75,6 +81,7 @@ namespace mrover { double gamma = 0; double x3 = x - LINK_DE * cos(gamma); double z3 = z - LINK_DE * sin(gamma); + // SE3d joint_e_pos{{x3, 0, z3}, SO3d::Identity()}; double C = sqrt(x3 * x3 + z3 * z3); double alpha = acos((LINK_BC * LINK_BC + LINK_CD * LINK_CD - C * C) / (2 * LINK_BC * LINK_CD)); @@ -83,10 +90,19 @@ namespace mrover { double thetaB = -1 * (std::numbers::pi - alpha); double thetaC = gamma - thetaA - thetaB; - double q1 = -thetaA; - double q2 = -thetaB; - double q3 = -thetaC; + SE3d joint_b_pos{{0.034346, 0, 0.049024}, SO3d::Identity()}; + SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, SO3d{0, -thetaA, 0}}; + SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, SO3d{0, -thetaB, 0}}; + SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, SO3d{0, -thetaC, 0}}; + SE3Conversions::pushToTfTree(tfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); + SE3Conversions::pushToTfTree(tfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); + SE3Conversions::pushToTfTree(tfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); + SE3Conversions::pushToTfTree(tfBroadcaster, "arm_e_target", "arm_d_target", joint_e_pos); + double q1 = -thetaA; + double q2 = -thetaB + 0.1608485915; + double q3 = -thetaC - 0.1608485915; + if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3) && y >= JOINT_A_MIN && y <= JOINT_A_MAX && q1 >= JOINT_B_MIN && q1 <= JOINT_B_MAX && From aa9c2f30a0018c590eb11b3ebf593770384c3e14 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Fri, 23 Feb 2024 00:38:24 -0500 Subject: [PATCH 16/54] Minor updateS --- .../arm_controller/arm_controller.cpp | 43 ++++++++++--------- .../arm_controller/arm_controller.hpp | 23 +++------- src/teleoperation/arm_controller/pch.hpp | 2 + 3 files changed, 31 insertions(+), 37 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index f5a445084..aef3285f2 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -1,9 +1,4 @@ #include "arm_controller.hpp" -#include -#include -#include -#include -#include namespace mrover { @@ -31,8 +26,6 @@ namespace mrover { // Publishers - - // Private state // auto ArmController::run(int argc, char** argv) -> int { @@ -50,6 +43,14 @@ namespace mrover { // return EXIT_SUCCESS; // } + ArmController::ArmController() { + ros::NodeHandle nh; + double frequency{}; + nh.param("/frequency", frequency, 100); + mIkSubscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); + mPositionPublisher = nh.advertise("arm_position_cmd", 1); + } + auto ArmController::ik_callback(IK const& ik_target) -> void { Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; @@ -59,7 +60,7 @@ namespace mrover { double y = ik_target.pose.position.y; double z = ik_target.pose.position.z; SE3d pos{{x + 0.034346, 0, z + 0.049024}, SO3d::Identity()}; - SE3Conversions::pushToTfTree(tfBroadcaster, "arm_target", "arm_a_link", pos); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_a_link", pos); // SE3d link_ab = SE3Conversions::fromTfTree(buffer, "arm_a_link", "arm_b_link"); // SE3d link_bc = SE3Conversions::fromTfTree(buffer, "arm_b_link", "arm_c_link"); @@ -79,14 +80,14 @@ namespace mrover { // ROS_INFO("x: %f, y: %f, z: %f, q1: %f, q2: %f, q3: %f", x, y, z, q1, q2, q3); double gamma = 0; - double x3 = x - LINK_DE * cos(gamma); - double z3 = z - LINK_DE * sin(gamma); + double x3 = x - LINK_DE * std::cos(gamma); + double z3 = z - LINK_DE * std::sin(gamma); // SE3d joint_e_pos{{x3, 0, z3}, SO3d::Identity()}; - double C = sqrt(x3 * x3 + z3 * z3); - double alpha = acos((LINK_BC * LINK_BC + LINK_CD * LINK_CD - C * C) / (2 * LINK_BC * LINK_CD)); - double beta = acos((LINK_BC * LINK_BC + C * C - LINK_CD * LINK_CD) / (2 * LINK_BC * C)); - double thetaA = atan(z3 / x3) + beta; + double C = std::sqrt(x3 * x3 + z3 * z3); + double alpha = std::acos((LINK_BC * LINK_BC + LINK_CD * LINK_CD - C * C) / (2 * LINK_BC * LINK_CD)); + double beta = std::acos((LINK_BC * LINK_BC + C * C - LINK_CD * LINK_CD) / (2 * LINK_BC * C)); + double thetaA = std::atan(z3 / x3) + beta; double thetaB = -1 * (std::numbers::pi - alpha); double thetaC = gamma - thetaA - thetaB; @@ -94,10 +95,10 @@ namespace mrover { SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, SO3d{0, -thetaA, 0}}; SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, SO3d{0, -thetaB, 0}}; SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, SO3d{0, -thetaC, 0}}; - SE3Conversions::pushToTfTree(tfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); - SE3Conversions::pushToTfTree(tfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); - SE3Conversions::pushToTfTree(tfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); - SE3Conversions::pushToTfTree(tfBroadcaster, "arm_e_target", "arm_d_target", joint_e_pos); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_e_target", "arm_d_target", joint_e_pos); double q1 = -thetaA; double q2 = -thetaB + 0.1608485915; @@ -113,7 +114,7 @@ namespace mrover { positions.positions[1] = static_cast(q1); positions.positions[2] = static_cast(q2); positions.positions[3] = static_cast(q3); - position_publisher.publish(positions); + mPositionPublisher.publish(positions); } else { } } @@ -122,8 +123,8 @@ namespace mrover { auto main(int argc, char** argv) -> int { ros::init(argc, argv, "arm_controller"); - mrover::ArmController arm_controller{}; + + mrover::ArmController armController; ros::spin(); - // return arm_controller.run(argc, argv); } diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index c7f2e20f5..2b31c35db 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -1,30 +1,21 @@ #pragma once #include "pch.hpp" -#include -#include namespace mrover { class ArmController { - [[maybe_unused]] ros::Subscriber ik_subscriber; - ros::Publisher position_publisher; - tf2_ros::TransformBroadcaster tfBroadcaster; - tf2_ros::Buffer buffer{}; - tf2_ros::TransformListener mTfListener{buffer}; + [[maybe_unused]] ros::Subscriber mIkSubscriber; + ros::Publisher mPositionPublisher; + tf2_ros::TransformBroadcaster mTfBroadcaster; + tf2_ros::Buffer mTfBuffer{}; + tf2_ros::TransformListener mTfListener{mTfBuffer}; public: - ArmController() : tfBroadcaster() { - ros::NodeHandle nh; - double frequency{}; - nh.param("/frequency", frequency, 100); - ik_subscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); - position_publisher = nh.advertise("arm_position_cmd", 1); - } + ArmController(); + void ik_callback(IK const& new_ik_target); - // auto run(int argc, char** argv) -> int; }; - // void ik_callback(IK const& new_ik_target); } // namespace mrover diff --git a/src/teleoperation/arm_controller/pch.hpp b/src/teleoperation/arm_controller/pch.hpp index 20e0454a1..b827f9f71 100644 --- a/src/teleoperation/arm_controller/pch.hpp +++ b/src/teleoperation/arm_controller/pch.hpp @@ -4,9 +4,11 @@ #include #include +#include #include #include +#include #include #include From 831f54727e3a1ee8a1144ed2edb09b614e8b4299 Mon Sep 17 00:00:00 2001 From: mvielmetti Date: Tue, 5 Mar 2024 19:09:18 -0500 Subject: [PATCH 17/54] updated comment --- src/perception/click_ik/click_ik.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 20f1ce2c6..f630de4e3 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -24,7 +24,7 @@ namespace mrover { } void ClickIkNodelet::pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) { - // Save points from point cloud + // Update current pointer to pointcloud data mPoints = reinterpret_cast(msg->data.data()); mNumPoints = msg->width * msg->height; } From ad58d09631f300c9d7b798fef14be0f32038ef7e Mon Sep 17 00:00:00 2001 From: mvielmetti Date: Tue, 5 Mar 2024 19:38:35 -0500 Subject: [PATCH 18/54] added spiral search --- src/perception/click_ik/click_ik.cpp | 43 ++++++++++++++++++++++++++++ src/perception/click_ik/click_ik.hpp | 10 +++++++ src/perception/click_ik/pch.hpp | 2 ++ 3 files changed, 55 insertions(+) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index f630de4e3..6acd6b38d 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -5,6 +5,8 @@ #include "mrover/ClickIkGoal.h" #include "mrover/IK.h" +#include + namespace mrover { void ClickIkNodelet::execute(const mrover::ClickIkGoalConstPtr& goal) { @@ -29,6 +31,47 @@ namespace mrover { mNumPoints = msg->width * msg->height; } + auto ClickIkNodelet::spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional { + std::size_t currX = xCenter; + std::size_t currY = yCenter; + std::size_t radius = 0; + int t = 0; + constexpr int numPts = 16; + bool isPointInvalid = true; + Point point{}; + + // Find the smaller of the two box dimensions so we know the max spiral radius + std::size_t smallDim = std::min(mPointCloudWidth / 2, mPointCloudHeight / 2); + + while (isPointInvalid) { + // This is the parametric equation to spiral around the center pnt + currX = static_cast(static_cast(xCenter) + std::cos(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); + currY = static_cast(static_cast(yCenter) + std::sin(t * 1.0 / numPts * 2 * M_PI) * static_cast(radius)); + + // Grab the point from the pntCloud and determine if its a finite pnt + point = mPoints[currX + currY * mPointCloudWidth]; + + isPointInvalid = !std::isfinite(point.x) || !std::isfinite(point.y) || !std::isfinite(point.z); + if (isPointInvalid) + NODELET_WARN("Tag center point not finite: [%f %f %f]", point.x, point.y, point.z); + + // After a full circle increase the radius + if (t % numPts == 0) { + radius++; + } + + // Increase the parameter + t++; + + // If we reach the edge of the box we stop spiraling + if (radius >= smallDim) { + return std::nullopt; + } + } + + return std::make_optional(R3{point.x, point.y, point.z}, SO3d::Identity()); + } + } // namespace mrover diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index 7a951fabc..e72962a6e 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -4,6 +4,7 @@ #include "pch.hpp" #include + namespace mrover { class ClickIkNodelet final : public nodelet::Nodelet { @@ -20,6 +21,12 @@ namespace mrover { const Point* mPoints{}; std::size_t mNumPoints{}; + + std::size_t mPointCloudWidth{}; + std::size_t mPointCloudHeight{}; + + //TODO - convert to ROS PARAM + const uin32_t MAX_RADIUS = 10; public: ClickIkNodelet() = default; @@ -31,6 +38,9 @@ namespace mrover { auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; void execute(const mrover::ClickIkGoalConstPtr& goal); + + //Taken line for line from percep object detection code + auto spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional; }; } // namespace mrover diff --git a/src/perception/click_ik/pch.hpp b/src/perception/click_ik/pch.hpp index bc5c2b45e..ea77ca119 100644 --- a/src/perception/click_ik/pch.hpp +++ b/src/perception/click_ik/pch.hpp @@ -27,4 +27,6 @@ #include #include +#include + #include From e2a4bb81d12e9fce75975f103c589f34ddf6edd2 Mon Sep 17 00:00:00 2001 From: mvielmetti Date: Tue, 5 Mar 2024 19:55:44 -0500 Subject: [PATCH 19/54] fixing poitncloud callback --- src/perception/click_ik/click_ik.cpp | 17 +++++++++++++++++ src/perception/click_ik/click_ik.hpp | 5 ++++- src/perception/click_ik/pch.hpp | 1 - 3 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 6acd6b38d..522e60ef9 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -6,11 +6,28 @@ #include "mrover/IK.h" #include +#include namespace mrover { void ClickIkNodelet::execute(const mrover::ClickIkGoalConstPtr& goal) { ROS_INFO("Executing goal"); + + auto target_point = spiralSearchInImg(static_cast(goal.pointInImageX), static_cast(goal.pointInImageY)); + + //Check if optional has value + if (!std::optional::has_value(target_point)) { + //Handle gracefully + return; + } + + //Convert target_point (SE3) to correct frame + auto inverse_transform = SE3Conversions::fromTfTree(buffer, "chassis_link", "zed2i_left_camera_frame"); + + auto desired_transform = SE3Conversions::inverse_transform(); + + + } void ClickIkNodelet::onInit() { diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index e72962a6e..10548a9d0 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -25,6 +25,9 @@ namespace mrover { std::size_t mPointCloudWidth{}; std::size_t mPointCloudHeight{}; + tf2_ros::Buffer mTfBuffer{}; + tf2_ros::TransformListener mTfListener{mTfBuffer}; + //TODO - convert to ROS PARAM const uin32_t MAX_RADIUS = 10; @@ -35,7 +38,7 @@ namespace mrover { void onInit() override; - auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; + auto pointCloudCallback(std::sensor_msgs::PointCloud2ConstPtr const& msg) -> void; void execute(const mrover::ClickIkGoalConstPtr& goal); diff --git a/src/perception/click_ik/pch.hpp b/src/perception/click_ik/pch.hpp index ea77ca119..64d3cde92 100644 --- a/src/perception/click_ik/pch.hpp +++ b/src/perception/click_ik/pch.hpp @@ -27,6 +27,5 @@ #include #include -#include #include From 61448fa768217ac7848e2c2b73e2661e8ab8efa8 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 5 Mar 2024 19:56:30 -0500 Subject: [PATCH 20/54] cleaned up arm controller a bit --- .../arm_controller/arm_controller.cpp | 58 +++---------------- .../arm_controller/arm_controller.hpp | 17 ++++++ 2 files changed, 25 insertions(+), 50 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index aef3285f2..bbc0dd7ae 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -1,48 +1,6 @@ #include "arm_controller.hpp" namespace mrover { - - // Constants - - // From: rover.urdf.xacro - // A is the prismatic joint, B is the first revolute joint, C is the second revolute joint - constexpr double LINK_BC = 0.5344417294; - constexpr double LINK_CD = 0.5531735368; - constexpr double LINK_DE = 0.044886000454425812; - // double const OFFSET = std::atan2(0.09, LINK_CD); - constexpr double JOINT_A_MIN = -0.45; - constexpr double JOINT_A_MAX = 0; - constexpr double JOINT_B_MIN = -0.25 * std::numbers::pi; - constexpr double JOINT_B_MAX = 0; - constexpr double JOINT_C_MIN = -0.959931; - constexpr double JOINT_C_MAX = 2.87979; - constexpr double JOINT_DE_PITCH_MIN = -0.75 * std::numbers::pi; - constexpr double JOINT_DE_PITCH_MAX = 0.75 * std::numbers::pi; - // constexpr double JOINT_DE_ROLL_MIN = -0.75 * std::numbers::pi; - // constexpr double JOINT_DE_ROLL_MAX = 0.75 * std::numbers::pi; - - // Subscribers - - - // Publishers - - // Private state - - // auto ArmController::run(int argc, char** argv) -> int { - // ros::init(argc, argv, "arm_controller"); - // ros::NodeHandle nh; - - // double frequency{}; - // nh.param("/frequency", frequency, 100); - - // ik_subscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); - // position_publisher = nh.advertise("arm_position_cmd", 1); - - // ros::spin(); - - // return EXIT_SUCCESS; - // } - ArmController::ArmController() { ros::NodeHandle nh; double frequency{}; @@ -91,14 +49,14 @@ namespace mrover { double thetaB = -1 * (std::numbers::pi - alpha); double thetaC = gamma - thetaA - thetaB; - SE3d joint_b_pos{{0.034346, 0, 0.049024}, SO3d::Identity()}; - SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, SO3d{0, -thetaA, 0}}; - SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, SO3d{0, -thetaB, 0}}; - SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, SO3d{0, -thetaC, 0}}; - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_e_target", "arm_d_target", joint_e_pos); + // SE3d joint_b_pos{{0.034346, 0, 0.049024}, SO3d::Identity()}; + // SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, SO3d{0, -thetaA, 0}}; + // SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, SO3d{0, -thetaB, 0}}; + // SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, SO3d{0, -thetaC, 0}}; + // SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); + // SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); + // SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); + // SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_e_target", "arm_d_target", joint_e_pos); double q1 = -thetaA; double q2 = -thetaB + 0.1608485915; diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index 2b31c35db..e8d729501 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -12,6 +12,23 @@ namespace mrover { tf2_ros::Buffer mTfBuffer{}; tf2_ros::TransformListener mTfListener{mTfBuffer}; + // From: rover.urdf.xacro + // A is the prismatic joint, B is the first revolute joint, C is the second revolute joint + static constexpr double LINK_BC = 0.5344417294; + static constexpr double LINK_CD = 0.5531735368; + static constexpr double LINK_DE = 0.044886000454425812; + // double const OFFSET = std::atan2(0.09, LINK_CD); + static constexpr double JOINT_A_MIN = -0.45; + static constexpr double JOINT_A_MAX = 0; + static constexpr double JOINT_B_MIN = -0.25 * std::numbers::pi; + static constexpr double JOINT_B_MAX = 0; + static constexpr double JOINT_C_MIN = -0.959931; + static constexpr double JOINT_C_MAX = 2.87979; + static constexpr double JOINT_DE_PITCH_MIN = -0.75 * std::numbers::pi; + static constexpr double JOINT_DE_PITCH_MAX = 0.75 * std::numbers::pi; + // constexpr double JOINT_DE_ROLL_MIN = -0.75 * std::numbers::pi; + // constexpr double JOINT_DE_ROLL_MAX = 0.75 * std::numbers::pi; + public: ArmController(); From 450c73d15cb7b1f93467f16e0696e0a826c065d5 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 5 Mar 2024 20:08:44 -0500 Subject: [PATCH 21/54] worked on click ik transformations --- src/perception/click_ik/click_ik.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 522e60ef9..2ba58a8e2 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -13,21 +13,26 @@ namespace mrover { void ClickIkNodelet::execute(const mrover::ClickIkGoalConstPtr& goal) { ROS_INFO("Executing goal"); - auto target_point = spiralSearchInImg(static_cast(goal.pointInImageX), static_cast(goal.pointInImageY)); + auto target_point = spiralSearchInImg(static_cast(goal->pointInImageX), static_cast(goal->pointInImageY)); //Check if optional has value - if (!std::optional::has_value(target_point)) { + if (!target_point.has_value()) { //Handle gracefully return; } //Convert target_point (SE3) to correct frame - auto inverse_transform = SE3Conversions::fromTfTree(buffer, "chassis_link", "zed2i_left_camera_frame"); + SE3d inverse_transform = SE3Conversions::fromTfTree(mTfBuffer, "chassis_link", "zed2i_left_camera_frame"); - auto desired_transform = SE3Conversions::inverse_transform(); + auto desired_transform = inverse_transform.inverse(); + auto point_in_chassis = desired_transform * target_point.value(); + double y = point_in_chassis.y(); + SE3d point_in_arm_b = SE3Conversions::fromTfTree(mTfBuffer, "chassis_link", "arm_b_link") * point_in_chassis; + + mIkPub.publish(); } void ClickIkNodelet::onInit() { From 2acf605d42ebdeac4729a4a5f077238d7d6749cb Mon Sep 17 00:00:00 2001 From: Charlie Zhong Date: Thu, 7 Mar 2024 16:28:07 -0500 Subject: [PATCH 22/54] Added action server client --- scripts/debug_click_ik.py | 20 ++++++++++++++------ src/perception/click_ik/click_ik.cpp | 6 +++++- src/perception/click_ik/click_ik.hpp | 7 +++---- 3 files changed, 22 insertions(+), 11 deletions(-) diff --git a/scripts/debug_click_ik.py b/scripts/debug_click_ik.py index 8892b7cec..34251f622 100644 --- a/scripts/debug_click_ik.py +++ b/scripts/debug_click_ik.py @@ -1,22 +1,30 @@ #!/usr/bin/env python3 import rospy -from mrover.msg import ClickIkAction, ClickIkActionGoal, ClickIkActionFeedback, ClickIkActionResult +from mrover.msg import ClickIkAction, ClickIkGoal import actionlib import sys def click_ik_client(): # Creates the SimpleActionClient, passing the type of the action - # (FibonacciAction) to the constructor. client = actionlib.SimpleActionClient('do_click_ik', ClickIkAction) + print('created click ik client') # Waits until the action server has started up and started # listening for goals. client.wait_for_server() + print('Found server') # Creates a goal to send to the action server. - goal = ClickIkActionGoal() + goal = ClickIkGoal() # Sends the goal to the action server. - client.send_goal(goal) - client.wait_for_result(rospy.Duration.from_sec(5.0)) + goal.pointInImageX = 0 + goal.pointInImageY = 0 + client.send_goal(goal, feedback_cb=feedback) + client.wait_for_result() + result = client.get_result() + return result + +def feedback(msg): + print(msg) if __name__ == "__main__": try: @@ -24,6 +32,6 @@ def click_ik_client(): # publish and subscribe over ROS. rospy.init_node('debug_click_ik') result = click_ik_client() - print("Result:", ', '.join([str(n) for n in result.sequence])) + print('result: ', result) except rospy.ROSInterruptException: print("program interrupted before completion", file=sys.stderr) \ No newline at end of file diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 522e60ef9..2079b62f5 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -38,7 +38,11 @@ namespace mrover { // IK Publisher mIkPub = mNh.advertise("/arm_ik", 1); - server.start(); + ROS_INFO("Starting action server"); + auto * new_server = new actionlib::SimpleActionServer(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) {execute(goal);}, false); + server = std::unique_ptr>(new_server); + server->start(); + ROS_INFO("Action server started"); } diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index 10548a9d0..80b99357a 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -15,9 +15,8 @@ namespace mrover { // IK publisher ros::Publisher mIkPub; - actionlib::SimpleActionServer server = actionlib::SimpleActionServer(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) { - execute(goal); - }, false); + std::unique_ptr> server{}; + const Point* mPoints{}; std::size_t mNumPoints{}; @@ -29,7 +28,7 @@ namespace mrover { tf2_ros::TransformListener mTfListener{mTfBuffer}; //TODO - convert to ROS PARAM - const uin32_t MAX_RADIUS = 10; + const uint32_t MAX_RADIUS = 10; public: ClickIkNodelet() = default; From f7846204014a147d560835efdcad3d38c2b6c25e Mon Sep 17 00:00:00 2001 From: Charlie Zhong Date: Thu, 7 Mar 2024 20:02:18 -0500 Subject: [PATCH 23/54] Published IK from action client --- scripts/debug_click_ik.py | 4 ++-- src/perception/click_ik/click_ik.cpp | 32 ++++++++++++++++++---------- src/perception/click_ik/click_ik.hpp | 4 ++-- 3 files changed, 25 insertions(+), 15 deletions(-) diff --git a/scripts/debug_click_ik.py b/scripts/debug_click_ik.py index 34251f622..cafaaae64 100644 --- a/scripts/debug_click_ik.py +++ b/scripts/debug_click_ik.py @@ -16,8 +16,8 @@ def click_ik_client(): # Creates a goal to send to the action server. goal = ClickIkGoal() # Sends the goal to the action server. - goal.pointInImageX = 0 - goal.pointInImageY = 0 + goal.pointInImageX = 320 + goal.pointInImageY = 240 client.send_goal(goal, feedback_cb=feedback) client.wait_for_result() result = client.get_result() diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 2079b62f5..9f23028f7 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -1,6 +1,7 @@ #include #include #include "click_ik.hpp" +#include "lie.hpp" #include "mrover/ClickIkAction.h" #include "mrover/ClickIkGoal.h" #include "mrover/IK.h" @@ -13,21 +14,30 @@ namespace mrover { void ClickIkNodelet::execute(const mrover::ClickIkGoalConstPtr& goal) { ROS_INFO("Executing goal"); - auto target_point = spiralSearchInImg(static_cast(goal.pointInImageX), static_cast(goal.pointInImageY)); + auto target_point = spiralSearchInImg(static_cast(goal->pointInImageX), static_cast(goal->pointInImageY)); //Check if optional has value - if (!std::optional::has_value(target_point)) { + if (!target_point.has_value()) { //Handle gracefully + ROS_WARN("Target point does not exist."); return; } //Convert target_point (SE3) to correct frame - auto inverse_transform = SE3Conversions::fromTfTree(buffer, "chassis_link", "zed2i_left_camera_frame"); - - auto desired_transform = SE3Conversions::inverse_transform(); - - - + auto inverse_transform = SE3Conversions::fromTfTree(mTfBuffer, "chassis_link", "zed2i_left_camera_frame"); + auto desired_transform = inverse_transform.inverse(); + SE3d arm_target = desired_transform * target_point.value(); + geometry_msgs::Pose pose; + pose.position.x = arm_target.x(); + pose.position.y = arm_target.y(); + pose.position.z = arm_target.z(); + pose.orientation.w = arm_target.quat().w(); + pose.orientation.x = arm_target.quat().x(); + pose.orientation.y = arm_target.quat().y(); + pose.orientation.z = arm_target.quat().z(); + IK message; + message.pose = pose; + mIkPub.publish(message); } void ClickIkNodelet::onInit() { @@ -39,9 +49,7 @@ namespace mrover { mIkPub = mNh.advertise("/arm_ik", 1); ROS_INFO("Starting action server"); - auto * new_server = new actionlib::SimpleActionServer(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) {execute(goal);}, false); - server = std::unique_ptr>(new_server); - server->start(); + server.start(); ROS_INFO("Action server started"); } @@ -50,6 +58,8 @@ namespace mrover { // Update current pointer to pointcloud data mPoints = reinterpret_cast(msg->data.data()); mNumPoints = msg->width * msg->height; + mPointCloudWidth = msg->width; + mPointCloudHeight = msg->height; } auto ClickIkNodelet::spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional { diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index 80b99357a..bd4d2ffcc 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -15,7 +15,7 @@ namespace mrover { // IK publisher ros::Publisher mIkPub; - std::unique_ptr> server{}; + actionlib::SimpleActionServer server = actionlib::SimpleActionServer(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) {execute(goal);}, false); const Point* mPoints{}; @@ -37,7 +37,7 @@ namespace mrover { void onInit() override; - auto pointCloudCallback(std::sensor_msgs::PointCloud2ConstPtr const& msg) -> void; + auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; void execute(const mrover::ClickIkGoalConstPtr& goal); From 1f2a4a2dbd65dac963dbb574d8c71836588bcf12 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Mar 2024 20:20:32 -0500 Subject: [PATCH 24/54] More stable so3d creation --- src/teleoperation/arm_controller/arm_controller.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index aef3285f2..b751a9c02 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -92,9 +92,9 @@ namespace mrover { double thetaC = gamma - thetaA - thetaB; SE3d joint_b_pos{{0.034346, 0, 0.049024}, SO3d::Identity()}; - SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, SO3d{0, -thetaA, 0}}; - SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, SO3d{0, -thetaB, 0}}; - SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, SO3d{0, -thetaC, 0}}; + SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, SO3d{Eigen::AngleAxisd{-thetaA, R3::UnitY()}}}; + SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, SO3d{Eigen::AngleAxisd{-thetaB, R3::UnitY()}}}; + SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, SO3d{Eigen::AngleAxisd{-thetaC, R3::UnitY()}}}; SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); From d54d366f5c8c839ec58f7935e150ae62eddf86c2 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Mar 2024 20:28:19 -0500 Subject: [PATCH 25/54] Actually more stable --- .../arm_controller/arm_controller.cpp | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index b751a9c02..1bf67a86c 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -51,6 +51,11 @@ namespace mrover { mPositionPublisher = nh.advertise("arm_position_cmd", 1); } + auto yawSo3(double r) -> SO3d { + auto q = Eigen::Quaterniond{Eigen::AngleAxisd{r, R3::UnitY()}}; + return {q.normalized()}; + } + auto ArmController::ik_callback(IK const& ik_target) -> void { Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; @@ -70,7 +75,7 @@ namespace mrover { // SE3d offset = SE3Conversions::fromTfTree(buffer, "chassis_link", "arm_e_link").inverse() * pos; // ROS_INFO("x: %f, y: %f, z: %f", offset.translation().x(), offset.translation().y(), offset.translation().z()); // SE3d thing = SE3Conversions::fromTfTree(buffer, "arm_c_link", "arm_d_link"); - + // double C = x * x + z * z - LINK_BC * LINK_BC - LINK_CD * LINK_CD; // double q2 = std::acos(C / (2 * LINK_BC * LINK_CD)); // double q1 = std::atan2(z, x) - std::atan2(LINK_BC * std::sin(q2), LINK_BC + LINK_CD * std::cos(q2)); @@ -92,9 +97,9 @@ namespace mrover { double thetaC = gamma - thetaA - thetaB; SE3d joint_b_pos{{0.034346, 0, 0.049024}, SO3d::Identity()}; - SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, SO3d{Eigen::AngleAxisd{-thetaA, R3::UnitY()}}}; - SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, SO3d{Eigen::AngleAxisd{-thetaB, R3::UnitY()}}}; - SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, SO3d{Eigen::AngleAxisd{-thetaC, R3::UnitY()}}}; + SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, yawSo3(-thetaA)}; + SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, yawSo3(-thetaB)}; + SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, yawSo3(-thetaC)}; SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); @@ -103,13 +108,12 @@ namespace mrover { double q1 = -thetaA; double q2 = -thetaB + 0.1608485915; double q3 = -thetaC - 0.1608485915; - - if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3) && + + if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3) && y >= JOINT_A_MIN && y <= JOINT_A_MAX && q1 >= JOINT_B_MIN && q1 <= JOINT_B_MAX && q2 >= JOINT_C_MIN && q2 <= JOINT_C_MAX && - q3 >= JOINT_DE_PITCH_MIN && q3 <= JOINT_DE_PITCH_MAX - ) { + q3 >= JOINT_DE_PITCH_MIN && q3 <= JOINT_DE_PITCH_MAX) { positions.positions[0] = static_cast(y); positions.positions[1] = static_cast(q1); positions.positions[2] = static_cast(q2); From 912260a324edcf7bd5ebaa2273cdac4da0b81d3f Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 7 Mar 2024 20:30:25 -0500 Subject: [PATCH 26/54] Sensible default --- src/simulator/simulator.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index f8a4a20a9..d80d5bcd3 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -221,7 +221,7 @@ namespace mrover { tf2_ros::TransformBroadcaster mTfBroadcaster; bool mPublishIk = true; - Eigen::Vector3f mIkTarget{0.125, 0.1, 0}; + Eigen::Vector3f mIkTarget{0.35, 0, -0.25}; ros::Publisher mIkTargetPub; R3 mGpsLinearizationReferencePoint{}; From 786ae17b9abd445688092650e8f37ecb4366a2db Mon Sep 17 00:00:00 2001 From: mvielmetti Date: Sun, 10 Mar 2024 12:49:34 -0400 Subject: [PATCH 27/54] removed ros param --- src/perception/click_ik/click_ik.cpp | 2 ++ src/perception/click_ik/click_ik.hpp | 3 --- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 9f23028f7..726a46a66 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -28,9 +28,11 @@ namespace mrover { auto desired_transform = inverse_transform.inverse(); SE3d arm_target = desired_transform * target_point.value(); geometry_msgs::Pose pose; + pose.position.x = arm_target.x(); pose.position.y = arm_target.y(); pose.position.z = arm_target.z(); + pose.orientation.w = arm_target.quat().w(); pose.orientation.x = arm_target.quat().x(); pose.orientation.y = arm_target.quat().y(); diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index bd4d2ffcc..10ae186b3 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -26,9 +26,6 @@ namespace mrover { tf2_ros::Buffer mTfBuffer{}; tf2_ros::TransformListener mTfListener{mTfBuffer}; - - //TODO - convert to ROS PARAM - const uint32_t MAX_RADIUS = 10; public: ClickIkNodelet() = default; From 0e57d8ffe25a7587ffeae5062afdd43dfee510b1 Mon Sep 17 00:00:00 2001 From: mvielmetti Date: Sun, 10 Mar 2024 13:02:35 -0400 Subject: [PATCH 28/54] changed ik to use stamped pose --- msg/IK.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/msg/IK.msg b/msg/IK.msg index e6803ee8b..a1a837b34 100644 --- a/msg/IK.msg +++ b/msg/IK.msg @@ -1 +1 @@ -geometry_msgs/Pose pose \ No newline at end of file +geometry_msgs/PoseStamped pose \ No newline at end of file From 7424ccac37660e3e6bd2ed4253992e6444ff9429 Mon Sep 17 00:00:00 2001 From: mvielmetti Date: Sun, 10 Mar 2024 13:10:41 -0400 Subject: [PATCH 29/54] added static transforms --- launch/simulator.launch | 31 ++++++++++++------- .../arm_controller/arm_controller.cpp | 2 ++ 2 files changed, 21 insertions(+), 12 deletions(-) diff --git a/launch/simulator.launch b/launch/simulator.launch index 23230736f..bdabd8bb6 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -1,26 +1,33 @@ - - + + - + - - + + + args="manager" output="screen" /> - + args="load mrover/SimulatorNodelet perception_nodelet_manager" output="screen"> + - + + + + + + - - + + - + \ No newline at end of file diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index d026e5846..ce1bdcc9d 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -22,6 +22,8 @@ namespace mrover { double x = ik_target.pose.position.x; double y = ik_target.pose.position.y; double z = ik_target.pose.position.z; + + //Offset between static_pose_a and b_link SE3d pos{{x + 0.034346, 0, z + 0.049024}, SO3d::Identity()}; SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_a_link", pos); From 726bac7c438fb34ca87b9b15a48b8782f773d238 Mon Sep 17 00:00:00 2001 From: mvielmetti Date: Sun, 10 Mar 2024 13:12:23 -0400 Subject: [PATCH 30/54] fixed broadcast rate --- launch/simulator.launch | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/simulator.launch b/launch/simulator.launch index bdabd8bb6..9c6afb629 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -20,9 +20,9 @@ + args="0.164882 0.200235 0.491172 0 0 0 base_link joint_a_static 100" /> + args="0.034346 0.0 0.049024 0 0 0 joint_a_static joint_b_static 100" /> From 88bb2e71d68ef54ab0f066738e708f51ddbd2e7c Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 10 Mar 2024 14:13:05 -0400 Subject: [PATCH 31/54] refactored arm ik to use less magic numbers --- msg/IK.msg | 2 +- src/perception/click_ik/click_ik.cpp | 25 ++++++++++--------- src/simulator/simulator.controls.cpp | 7 +++--- .../arm_controller/arm_controller.cpp | 25 +++++++++++++------ 4 files changed, 36 insertions(+), 23 deletions(-) diff --git a/msg/IK.msg b/msg/IK.msg index a1a837b34..2dd6ec8a5 100644 --- a/msg/IK.msg +++ b/msg/IK.msg @@ -1 +1 @@ -geometry_msgs/PoseStamped pose \ No newline at end of file +geometry_msgs/PoseStamped target \ No newline at end of file diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 726a46a66..e2762c715 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -24,21 +24,22 @@ namespace mrover { } //Convert target_point (SE3) to correct frame - auto inverse_transform = SE3Conversions::fromTfTree(mTfBuffer, "chassis_link", "zed2i_left_camera_frame"); - auto desired_transform = inverse_transform.inverse(); - SE3d arm_target = desired_transform * target_point.value(); + // auto inverse_transform = SE3Conversions::fromTfTree(mTfBuffer, "chassis_link", "zed2i_left_camera_frame"); + // auto desired_transform = inverse_transform.inverse(); + // SE3d arm_target = desired_transform * target_point.value(); geometry_msgs::Pose pose; - pose.position.x = arm_target.x(); - pose.position.y = arm_target.y(); - pose.position.z = arm_target.z(); - - pose.orientation.w = arm_target.quat().w(); - pose.orientation.x = arm_target.quat().x(); - pose.orientation.y = arm_target.quat().y(); - pose.orientation.z = arm_target.quat().z(); + pose.position.x = target_point.value().x(); + pose.position.y = target_point.value().y(); + pose.position.z = target_point.value().z(); + + pose.orientation.w = target_point.value().quat().w(); + pose.orientation.x = target_point.value().quat().x(); + pose.orientation.y = target_point.value().quat().y(); + pose.orientation.z = target_point.value().quat().z(); IK message; - message.pose = pose; + message.target.pose = pose; + message.target.header.frame_id = "zed2i_left_camera_frame"; mIkPub.publish(message); } diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index 05e3b89dd..9b6ba02e4 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -155,9 +155,10 @@ namespace mrover { auto SimulatorNodelet::userControls(Clock::duration dt) -> void { if (mPublishIk) { IK ik; - ik.pose.position.x = mIkTarget.x(); - ik.pose.position.y = mIkTarget.y(); - ik.pose.position.z = mIkTarget.z(); + ik.target.pose.position.x = mIkTarget.x(); + ik.target.pose.position.y = mIkTarget.y(); + ik.target.pose.position.z = mIkTarget.z(); + ik.target.header.frame_id = "joint_a_static"; mIkTargetPub.publish(ik); } diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index ce1bdcc9d..48969d981 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -1,4 +1,6 @@ #include "arm_controller.hpp" +#include "lie.hpp" +#include namespace mrover { ArmController::ArmController() { @@ -19,13 +21,22 @@ namespace mrover { positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; positions.positions.resize(positions.names.size(), 0.f); - double x = ik_target.pose.position.x; - double y = ik_target.pose.position.y; - double z = ik_target.pose.position.z; - - //Offset between static_pose_a and b_link - SE3d pos{{x + 0.034346, 0, z + 0.049024}, SO3d::Identity()}; - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_a_link", pos); + SE3d target_frame_to_arm_a_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "joint_a_static"); + SE3d target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "joint_b_static"); + + Eigen::Vector4d target{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z, 1}; + // double x = ik_target.target.pose.position.x; + // double y = ik_target.target.pose.position.y; + // double z = ik_target.target.pose.position.z; + // maybe just add offset instead of doing multiplication (we don't do any rotation?) + Eigen::Vector4d target_in_arm_b_static = target_frame_to_arm_b_static.transform() * target; + Eigen::Vector4d target_in_arm_a_static = target_frame_to_arm_a_static.transform() * target; + + double x = target_in_arm_b_static.x(); + double z = target_in_arm_b_static.z(); + double y = target_in_arm_a_static.y(); + SE3d pos{{target_in_arm_a_static.x(), target_in_arm_a_static.y(), target_in_arm_a_static.z()}, SO3d::Identity()}; + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "joint_a_static", pos); // SE3d link_ab = SE3Conversions::fromTfTree(buffer, "arm_a_link", "arm_b_link"); // SE3d link_bc = SE3Conversions::fromTfTree(buffer, "arm_b_link", "arm_c_link"); From 09a3cf6f658d5276e49f8efb7b85f492f4d09d50 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 12 Mar 2024 20:05:51 -0400 Subject: [PATCH 32/54] worked on click_ik --- src/perception/click_ik/click_ik.cpp | 29 ++++++++++++------- src/perception/click_ik/click_ik.hpp | 4 ++- .../arm_controller/arm_controller.cpp | 4 ++- .../arm_controller/arm_controller.hpp | 1 + 4 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index e2762c715..986d0c95b 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -29,18 +29,25 @@ namespace mrover { // SE3d arm_target = desired_transform * target_point.value(); geometry_msgs::Pose pose; - pose.position.x = target_point.value().x(); - pose.position.y = target_point.value().y(); - pose.position.z = target_point.value().z(); - - pose.orientation.w = target_point.value().quat().w(); - pose.orientation.x = target_point.value().quat().x(); - pose.orientation.y = target_point.value().quat().y(); - pose.orientation.z = target_point.value().quat().z(); + double offset = 0.1; // make sure we don't collide by moving back a little from the target + pose.position.x = target_point.value().x - offset; + pose.position.y = target_point.value().y;// - normal_scale * target_point.value().normal_y; + pose.position.z = target_point.value().z;// - normal_scale * target_point.value().normal_z; + + // pose.orientation.w = target_point.value().quat().w(); + // pose.orientation.x = target_point.value().quat().x(); + // pose.orientation.y = target_point.value().quat().y(); + // pose.orientation.z = target_point.value().quat().z(); IK message; message.target.pose = pose; - message.target.header.frame_id = "zed2i_left_camera_frame"; + message.target.header.frame_id = "zed_left_camera_frame"; mIkPub.publish(message); + + SE3d target_adjusted{{message.target.pose.position.x, message.target.pose.position.y, message.target.pose.position.z}, SO3d::Identity()}; + SE3d target_raw{{target_point.value().x, target_point.value().y, target_point.value().z}, SO3d::Identity()}; + SE3Conversions::pushToTfTree(mTfBroadcaster, "click_ik_target", "zed_left_camera_frame", target_adjusted); + SE3Conversions::pushToTfTree(mTfBroadcaster, "click_ik_target_raw", "zed_left_camera_frame", target_raw); + server.setSucceeded(); } void ClickIkNodelet::onInit() { @@ -65,7 +72,7 @@ namespace mrover { mPointCloudHeight = msg->height; } - auto ClickIkNodelet::spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional { + auto ClickIkNodelet::spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional { std::size_t currX = xCenter; std::size_t currY = yCenter; std::size_t radius = 0; @@ -103,7 +110,7 @@ namespace mrover { } } - return std::make_optional(R3{point.x, point.y, point.z}, SO3d::Identity()); + return std::make_optional(point); } diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index 10ae186b3..65dc28496 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -26,6 +26,8 @@ namespace mrover { tf2_ros::Buffer mTfBuffer{}; tf2_ros::TransformListener mTfListener{mTfBuffer}; + + tf2_ros::TransformBroadcaster mTfBroadcaster; public: ClickIkNodelet() = default; @@ -39,7 +41,7 @@ namespace mrover { void execute(const mrover::ClickIkGoalConstPtr& goal); //Taken line for line from percep object detection code - auto spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional; + auto spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional; }; } // namespace mrover diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index 48969d981..875abe3d0 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -32,9 +32,10 @@ namespace mrover { Eigen::Vector4d target_in_arm_b_static = target_frame_to_arm_b_static.transform() * target; Eigen::Vector4d target_in_arm_a_static = target_frame_to_arm_a_static.transform() * target; - double x = target_in_arm_b_static.x(); + double x = target_in_arm_b_static.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector double z = target_in_arm_b_static.z(); double y = target_in_arm_a_static.y(); + // ROS_INFO("x: %f, y: %f, z: %f", x, y, z); SE3d pos{{target_in_arm_a_static.x(), target_in_arm_a_static.y(), target_in_arm_a_static.z()}, SO3d::Identity()}; SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "joint_a_static", pos); @@ -91,6 +92,7 @@ namespace mrover { positions.positions[3] = static_cast(q3); mPositionPublisher.publish(positions); } else { + ROS_INFO("Can't reach arm target!"); } } diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index e8d729501..1652ee2c0 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -26,6 +26,7 @@ namespace mrover { static constexpr double JOINT_C_MAX = 2.87979; static constexpr double JOINT_DE_PITCH_MIN = -0.75 * std::numbers::pi; static constexpr double JOINT_DE_PITCH_MAX = 0.75 * std::numbers::pi; + static constexpr double END_EFFECTOR_LENGTH = 0.13; // measured from blender // constexpr double JOINT_DE_ROLL_MIN = -0.75 * std::numbers::pi; // constexpr double JOINT_DE_ROLL_MAX = 0.75 * std::numbers::pi; From 1cfad33f6e6bfaa78d47a0edc0866c9e8017e3e5 Mon Sep 17 00:00:00 2001 From: jnnanni Date: Sun, 17 Mar 2024 13:13:58 -0400 Subject: [PATCH 33/54] Add event handler for clicking within a camera stream, implementation of click ik interface still needs to be done --- .../frontend/src/components/CameraFeed.vue | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index 41ebbe0f1..d19f4c934 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -1,6 +1,6 @@ @@ -21,15 +21,23 @@ export default defineComponent({ } }, data() { - return {} + return { + } }, mounted: function () { this.$nextTick(() => { - const canvas: HTMLCanvasElement = document.getElementById('canvas') as HTMLCanvasElement + const canvas: HTMLCanvasElement = document.getElementById('canvas'+this.id) as HTMLCanvasElement const context = canvas.getContext('2d') ?? new CanvasRenderingContext2D() context.fillStyle = 'black' context.fillRect(0, 0, canvas.width, canvas.height) }) + }, + methods: + { + handleClick: function(event: MouseEvent) { + console.log('offsetX', event.offsetX) + console.log('offsetY', event.offsetY) + } } }) From c20aec94d8b076afbfba5b51cbc035fb40ad8ab0 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Sun, 17 Mar 2024 14:21:15 -0400 Subject: [PATCH 34/54] added motor timeout to sim --- src/simulator/simulator.hpp | 4 +++- src/simulator/simulator.physics.cpp | 13 +++++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index d80d5bcd3..b01531cb7 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -84,6 +84,7 @@ namespace mrover { int index{}; boost::container::static_vector, 2> visualUniforms; boost::container::static_vector, 2> collisionUniforms; + Clock::time_point lastUpdate = Clock::now(); }; std::string name; @@ -414,7 +415,7 @@ namespace mrover { template auto forEachWithMotor(N const& names, V const& values, F&& function) -> void { if (auto it = mUrdfs.find("rover"); it != mUrdfs.end()) { - URDF const& rover = it->second; + URDF & rover = it->second; for (auto const& combined: boost::combine(names, values)) { std::string const& name = boost::get<0>(combined); @@ -424,6 +425,7 @@ namespace mrover { std::string const& name = urdfName.value(); int linkIndex = rover.linkNameToMeta.at(name).index; + rover.linkNameToMeta.at(name).lastUpdate = Clock::now(); auto* motor = std::bit_cast(rover.physics->getLink(linkIndex).m_userPtr); assert(motor); diff --git a/src/simulator/simulator.physics.cpp b/src/simulator/simulator.physics.cpp index bf3f6084e..12623e0ca 100644 --- a/src/simulator/simulator.physics.cpp +++ b/src/simulator/simulator.physics.cpp @@ -41,6 +41,19 @@ namespace mrover { motor->setMaxAppliedImpulse(0.5); motor->setPositionTarget(0); } + // check if arm motor commands have expired + // TODO(quintin): fix hard-coded names? + for (auto const& name : {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { + bool expired = std::chrono::duration_cast(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > 20; + if (expired) { + int linkIndex = rover.linkNameToMeta.at(name).index; + auto* motor = std::bit_cast(rover.physics->getLink(linkIndex).m_userPtr); + assert(motor); + motor->setVelocityTarget(0, 1); + // set p gain to 0 to stop position control + motor->setPositionTarget(0, 0); + } + } } // TODO(quintin): clean this up From 0b71a1ea06465b90a611461d6a2b0cfdcbb777a1 Mon Sep 17 00:00:00 2001 From: RobbieGM Date: Sun, 17 Mar 2024 14:38:42 -0400 Subject: [PATCH 35/54] Click IK periodic publishing to arm IK, update consumers.py for click IK --- src/perception/click_ik/click_ik.cpp | 58 ++++++++++--------- src/perception/click_ik/click_ik.hpp | 16 ++--- src/perception/click_ik/pch.hpp | 9 ++- .../arm_controller/arm_controller.hpp | 2 +- src/teleoperation/backend/consumers.py | 20 +++++++ 5 files changed, 64 insertions(+), 41 deletions(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 986d0c95b..214edb9eb 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -1,19 +1,11 @@ -#include -#include #include "click_ik.hpp" -#include "lie.hpp" -#include "mrover/ClickIkAction.h" -#include "mrover/ClickIkGoal.h" -#include "mrover/IK.h" - -#include -#include namespace mrover { - void ClickIkNodelet::execute(const mrover::ClickIkGoalConstPtr& goal) { + void ClickIkNodelet::startClickIk() { ROS_INFO("Executing goal"); + const mrover::ClickIkGoalConstPtr& goal = server.acceptNewGoal(); auto target_point = spiralSearchInImg(static_cast(goal->pointInImageX), static_cast(goal->pointInImageY)); //Check if optional has value @@ -23,31 +15,39 @@ namespace mrover { return; } - //Convert target_point (SE3) to correct frame - // auto inverse_transform = SE3Conversions::fromTfTree(mTfBuffer, "chassis_link", "zed2i_left_camera_frame"); - // auto desired_transform = inverse_transform.inverse(); - // SE3d arm_target = desired_transform * target_point.value(); geometry_msgs::Pose pose; double offset = 0.1; // make sure we don't collide by moving back a little from the target pose.position.x = target_point.value().x - offset; - pose.position.y = target_point.value().y;// - normal_scale * target_point.value().normal_y; - pose.position.z = target_point.value().z;// - normal_scale * target_point.value().normal_z; - - // pose.orientation.w = target_point.value().quat().w(); - // pose.orientation.x = target_point.value().quat().x(); - // pose.orientation.y = target_point.value().quat().y(); - // pose.orientation.z = target_point.value().quat().z(); - IK message; + pose.position.y = target_point.value().y; + pose.position.z = target_point.value().z; + message.target.pose = pose; message.target.header.frame_id = "zed_left_camera_frame"; - mIkPub.publish(message); + timer = mNh.createTimer(ros::Duration(20 / 1000.0), [&](const ros::TimerEvent) { + if (server.isPreemptRequested()) { + timer.stop(); + return; + } + // Check if done + const float tolerance = 0.02; // 2 cm + SE3d arm_position = SE3Conversions::fromTfTree(mTfBuffer, "arm_e_link", "zed_left_camera_frame"); + double distance = pow(pow(arm_position.x() + ArmController::END_EFFECTOR_LENGTH - pose.position.x, 2) + pow(arm_position.y() - pose.position.y, 2) + pow(arm_position.z() - pose.position.z, 2), 0.5); + ROS_INFO("Distance to target: %f", distance); + if (distance < tolerance) { + timer.stop(); + mrover::ClickIkResult result; + result.success = true; + server.setSucceeded(result); + return; + } + // Otherwise publish message + mIkPub.publish(message); + }); + } - SE3d target_adjusted{{message.target.pose.position.x, message.target.pose.position.y, message.target.pose.position.z}, SO3d::Identity()}; - SE3d target_raw{{target_point.value().x, target_point.value().y, target_point.value().z}, SO3d::Identity()}; - SE3Conversions::pushToTfTree(mTfBroadcaster, "click_ik_target", "zed_left_camera_frame", target_adjusted); - SE3Conversions::pushToTfTree(mTfBroadcaster, "click_ik_target_raw", "zed_left_camera_frame", target_raw); - server.setSucceeded(); + void ClickIkNodelet::cancelClickIk() { + timer.stop(); } void ClickIkNodelet::onInit() { @@ -59,6 +59,8 @@ namespace mrover { mIkPub = mNh.advertise("/arm_ik", 1); ROS_INFO("Starting action server"); + server.registerGoalCallback([this]() { startClickIk(); }); + server.registerPreemptCallback([this] { cancelClickIk(); }); server.start(); ROS_INFO("Action server started"); diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index 65dc28496..186b07063 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -1,32 +1,25 @@ #pragma once -#include "mrover/ClickIkAction.h" #include "pch.hpp" -#include - namespace mrover { class ClickIkNodelet final : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; - ros::Subscriber mPcSub; - - // IK publisher ros::Publisher mIkPub; + actionlib::SimpleActionServer server = actionlib::SimpleActionServer(mNh, "do_click_ik", false); - actionlib::SimpleActionServer server = actionlib::SimpleActionServer(mNh, "do_click_ik", [&](const mrover::ClickIkGoalConstPtr& goal) {execute(goal);}, false); + IK message; + ros::Timer timer; const Point* mPoints{}; - std::size_t mNumPoints{}; - std::size_t mPointCloudWidth{}; std::size_t mPointCloudHeight{}; tf2_ros::Buffer mTfBuffer{}; tf2_ros::TransformListener mTfListener{mTfBuffer}; - tf2_ros::TransformBroadcaster mTfBroadcaster; public: @@ -38,7 +31,8 @@ namespace mrover { auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; - void execute(const mrover::ClickIkGoalConstPtr& goal); + void startClickIk(); + void cancelClickIk(); //Taken line for line from percep object detection code auto spiralSearchInImg(size_t xCenter, size_t yCenter) -> std::optional; diff --git a/src/perception/click_ik/pch.hpp b/src/perception/click_ik/pch.hpp index 64d3cde92..68fa57cac 100644 --- a/src/perception/click_ik/pch.hpp +++ b/src/perception/click_ik/pch.hpp @@ -11,6 +11,8 @@ #include #include #include +#include +#include #include #include @@ -22,10 +24,15 @@ #include #include #include +#include #include #include #include -#include +#include "mrover/ClickIkGoal.h" +#include "mrover/ClickIkAction.h" +#include "mrover/IK.h" +#include "../teleoperation/arm_controller/arm_controller.hpp" + diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index 1652ee2c0..ed115506d 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -12,6 +12,7 @@ namespace mrover { tf2_ros::Buffer mTfBuffer{}; tf2_ros::TransformListener mTfListener{mTfBuffer}; + public: // From: rover.urdf.xacro // A is the prismatic joint, B is the first revolute joint, C is the second revolute joint static constexpr double LINK_BC = 0.5344417294; @@ -30,7 +31,6 @@ namespace mrover { // constexpr double JOINT_DE_ROLL_MIN = -0.75 * std::numbers::pi; // constexpr double JOINT_DE_ROLL_MAX = 0.75 * std::numbers::pi; - public: ArmController(); void ik_callback(IK const& new_ik_target); diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 50676539e..645580b28 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -25,7 +25,10 @@ Velocity, Position, IK, + ClickIkAction, + ClickIkGoal ) +import actionlib from mrover.srv import EnableAuton, ChangeCameras, CapturePanorama from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, Image from mrover.srv import EnableAuton @@ -91,6 +94,9 @@ def connect(self): self.sa_humidity_data = rospy.Subscriber( "/sa_humidity_data", RelativeHumidity, self.sa_humidity_data_callback ) + + # Action clients + self.click_ik_client = actionlib.SimpleActionClient('do_click_ik', ClickIkAction) # Services self.laser_service = rospy.ServiceProxy("enable_arm_laser", SetBool) @@ -180,6 +186,10 @@ def receive(self, text_data): self.save_basic_waypoint_list(message) elif message["type"] == "get_basic_waypoint_list": self.get_basic_waypoint_list(message) + elif message["type"] == "start_click_ik": + self.start_click_ik(message) + elif message["type"] == "cancel_click_ik": + self.cancel_click_ik(message) except Exception as e: rospy.logerr(e) @@ -713,3 +723,13 @@ def flight_attitude_listener(self): self.send(text_data=json.dumps({"type": "flight_attitude", "pitch": pitch, "roll": roll})) rate.sleep() + + def start_click_ik(self, msg) -> None: + self.click_ik_client.wait_for_server() + goal = ClickIkGoal() + goal.pointInImageX = msg["x"] + goal.pointInImageY = msg["y"] + self.click_ik_client.send_goal(goal) + + def cancel_click_ik(self, msg) -> None: + self.click_ik_client.cancel_all_goals() From 8e62a1d8fbfa9e89fa914f7d7dab863128fcfe0b Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Tue, 19 Mar 2024 19:03:01 -0400 Subject: [PATCH 36/54] fixed click_ik issues --- src/perception/click_ik/click_ik.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index 214edb9eb..b4a8de5dd 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -24,7 +24,7 @@ namespace mrover { message.target.pose = pose; message.target.header.frame_id = "zed_left_camera_frame"; - timer = mNh.createTimer(ros::Duration(20 / 1000.0), [&](const ros::TimerEvent) { + timer = mNh.createTimer(ros::Duration(0.010), [&, pose](const ros::TimerEvent) { if (server.isPreemptRequested()) { timer.stop(); return; From 895873a358682eb2bb4046f021745b3faf2527f7 Mon Sep 17 00:00:00 2001 From: Charlie Zhong Date: Tue, 19 Mar 2024 19:09:36 -0400 Subject: [PATCH 37/54] publish feedback --- src/perception/click_ik/click_ik.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index b4a8de5dd..af8a715fb 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -34,6 +34,10 @@ namespace mrover { SE3d arm_position = SE3Conversions::fromTfTree(mTfBuffer, "arm_e_link", "zed_left_camera_frame"); double distance = pow(pow(arm_position.x() + ArmController::END_EFFECTOR_LENGTH - pose.position.x, 2) + pow(arm_position.y() - pose.position.y, 2) + pow(arm_position.z() - pose.position.z, 2), 0.5); ROS_INFO("Distance to target: %f", distance); + mrover::ClickIkFeedback feedback; + feedback.distance = static_cast(distance); + server.publishFeedback(feedback); + if (distance < tolerance) { timer.stop(); mrover::ClickIkResult result; From c98f2df6381535f2b252935b123896af15c9b0b8 Mon Sep 17 00:00:00 2001 From: jnnanni Date: Tue, 19 Mar 2024 20:40:31 -0400 Subject: [PATCH 38/54] Add frontend handlers for click ik, remove capacity and set to always be 4 --- .../frontend/src/components/CameraFeed.vue | 27 ++++++++++++++----- .../frontend/src/components/Cameras.vue | 8 +----- .../frontend/src/components/DMESTask.vue | 10 ++++++- 3 files changed, 30 insertions(+), 15 deletions(-) diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index d19f4c934..f0d6af1b5 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -1,12 +1,15 @@ From 1d6a9e613f80c2f9f2aa1c3e8e98f63d35f12f9f Mon Sep 17 00:00:00 2001 From: RobbieGM Date: Thu, 21 Mar 2024 18:57:28 -0400 Subject: [PATCH 39/54] Fix consumers.py click ik --- src/teleoperation/backend/consumers.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 645580b28..bb12bd2f1 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -725,10 +725,9 @@ def flight_attitude_listener(self): rate.sleep() def start_click_ik(self, msg) -> None: - self.click_ik_client.wait_for_server() goal = ClickIkGoal() - goal.pointInImageX = msg["x"] - goal.pointInImageY = msg["y"] + goal.pointInImageX = msg["data"]["x"] + goal.pointInImageY = msg["data"]["y"] self.click_ik_client.send_goal(goal) def cancel_click_ik(self, msg) -> None: From 02eb0ad65e911e5af862cedba5c34094c5e5bd83 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Thu, 21 Mar 2024 20:02:31 -0400 Subject: [PATCH 40/54] frontend video fixes --- src/teleoperation/backend/consumers.py | 8 +++++--- src/teleoperation/frontend/src/components/CameraFeed.vue | 4 ++++ 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 645580b28..7ec949a80 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -725,10 +725,12 @@ def flight_attitude_listener(self): rate.sleep() def start_click_ik(self, msg) -> None: - self.click_ik_client.wait_for_server() + rospy.logerr("before server") + # self.click_ik_client.wait_for_server() + rospy.logerr("after server") goal = ClickIkGoal() - goal.pointInImageX = msg["x"] - goal.pointInImageY = msg["y"] + goal.pointInImageX = msg["data"]["x"] + goal.pointInImageY = msg["data"]["y"] self.click_ik_client.send_goal(goal) def cancel_click_ik(self, msg) -> None: diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index f0d6af1b5..edfa89af4 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -59,4 +59,8 @@ export default defineComponent({ .wrap { width: fit-content; } +canvas { + width: 640px; + height: 480px; +} From ed5c6a2e68eb961dd115906a3f414c14d23a2093 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Thu, 21 Mar 2024 20:03:33 -0400 Subject: [PATCH 41/54] switch arm base frame and static transform error debugging --- launch/simulator.launch | 6 ++-- .../arm_controller/arm_controller.cpp | 30 ++++++++++++------- urdf/rover/rover.urdf.xacro | 8 +++++ 3 files changed, 31 insertions(+), 13 deletions(-) diff --git a/launch/simulator.launch b/launch/simulator.launch index 9c6afb629..c318c1088 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -19,10 +19,10 @@ - + diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index 875abe3d0..9cfaffd59 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -1,10 +1,13 @@ #include "arm_controller.hpp" #include "lie.hpp" #include +#include +#include namespace mrover { ArmController::ArmController() { ros::NodeHandle nh; + // sleep(2); double frequency{}; nh.param("/frequency", frequency, 100); mIkSubscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); @@ -20,24 +23,31 @@ namespace mrover { Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; positions.positions.resize(positions.names.size(), 0.f); - - SE3d target_frame_to_arm_a_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "joint_a_static"); - SE3d target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "joint_b_static"); - + SE3d target_frame_to_arm_b_static; + while (ros::ok()) { + try { + // ROS_INFO("%d", mTfBuffer._frameExists("joint_b_static")); + // target_frame_to_arm_a_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "joint_a_static"); + target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); + // ROS_INFO("Found tf"); + break; + } catch (...) { + ROS_INFO("Failed to grab tf"); + continue; + } + } Eigen::Vector4d target{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z, 1}; // double x = ik_target.target.pose.position.x; // double y = ik_target.target.pose.position.y; // double z = ik_target.target.pose.position.z; // maybe just add offset instead of doing multiplication (we don't do any rotation?) Eigen::Vector4d target_in_arm_b_static = target_frame_to_arm_b_static.transform() * target; - Eigen::Vector4d target_in_arm_a_static = target_frame_to_arm_a_static.transform() * target; - double x = target_in_arm_b_static.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector double z = target_in_arm_b_static.z(); - double y = target_in_arm_a_static.y(); + double y = target_in_arm_b_static.y(); // ROS_INFO("x: %f, y: %f, z: %f", x, y, z); - SE3d pos{{target_in_arm_a_static.x(), target_in_arm_a_static.y(), target_in_arm_a_static.z()}, SO3d::Identity()}; - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "joint_a_static", pos); + // SE3d pos{{target_in_arm_a_static.x(), target_in_arm_a_static.y(), target_in_arm_a_static.z()}, SO3d::Identity()}; + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", target_frame_to_arm_b_static); // SE3d link_ab = SE3Conversions::fromTfTree(buffer, "arm_a_link", "arm_b_link"); // SE3d link_bc = SE3Conversions::fromTfTree(buffer, "arm_b_link", "arm_c_link"); @@ -92,7 +102,7 @@ namespace mrover { positions.positions[3] = static_cast(q3); mPositionPublisher.publish(positions); } else { - ROS_INFO("Can't reach arm target!"); + // ROS_INFO("Can't reach arm target!"); } } diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index ba31ebf24..d4346336c 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -201,6 +201,14 @@ + + + + + + + + From 94e141bad19a99a743ed7171cfa9ef087e039015 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Fri, 22 Mar 2024 14:07:59 -0400 Subject: [PATCH 42/54] modify simulator ik base link --- src/simulator/simulator.controls.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index 9b6ba02e4..5d7933c64 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -158,7 +158,7 @@ namespace mrover { ik.target.pose.position.x = mIkTarget.x(); ik.target.pose.position.y = mIkTarget.y(); ik.target.pose.position.z = mIkTarget.z(); - ik.target.header.frame_id = "joint_a_static"; + ik.target.header.frame_id = "arm_base_link"; mIkTargetPub.publish(ik); } From 30e1da859cfb0bafc3bc8ee959d3f05009ad2a3b Mon Sep 17 00:00:00 2001 From: RobbieGM Date: Sun, 24 Mar 2024 13:53:43 -0400 Subject: [PATCH 43/54] add click ik feedback in consumers.py --- src/teleoperation/backend/consumers.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index bb12bd2f1..24be7417d 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -26,7 +26,8 @@ Position, IK, ClickIkAction, - ClickIkGoal + ClickIkGoal, + ClickIkFeedback, ) import actionlib from mrover.srv import EnableAuton, ChangeCameras, CapturePanorama @@ -728,7 +729,9 @@ def start_click_ik(self, msg) -> None: goal = ClickIkGoal() goal.pointInImageX = msg["data"]["x"] goal.pointInImageY = msg["data"]["y"] - self.click_ik_client.send_goal(goal) + def feedback_cb(feedback: ClickIkFeedback) -> None: + self.send(text_data=json.dumps({"type": "click_ik_feedback", "distance": feedback.distance})) + self.click_ik_client.send_goal(goal, feedback_cb=feedback_cb) def cancel_click_ik(self, msg) -> None: self.click_ik_client.cancel_all_goals() From b9eda52b2521695556ae91d56d0b7044e4b49964 Mon Sep 17 00:00:00 2001 From: jnnanni Date: Sun, 24 Mar 2024 13:54:07 -0400 Subject: [PATCH 44/54] Set up different camera feed types for each GUI --- .../frontend/src/components/AutonTask.vue | 7 +- .../frontend/src/components/CameraDisplay.vue | 18 ++- .../frontend/src/components/CameraFeed.vue | 109 ++++++++---------- .../frontend/src/components/Cameras.vue | 50 ++++++-- .../frontend/src/components/DMESTask.vue | 26 ++--- .../frontend/src/components/IKCameraFeed.vue | 62 ++++++++++ .../frontend/src/components/ISHTask.vue | 2 +- .../frontend/src/components/SACameraFeed.vue | 95 +++++++++++++++ .../frontend/src/components/SATask.vue | 10 +- 9 files changed, 280 insertions(+), 99 deletions(-) create mode 100644 src/teleoperation/frontend/src/components/IKCameraFeed.vue create mode 100644 src/teleoperation/frontend/src/components/SACameraFeed.vue diff --git a/src/teleoperation/frontend/src/components/AutonTask.vue b/src/teleoperation/frontend/src/components/AutonTask.vue index 39e0957b0..394b9a16b 100644 --- a/src/teleoperation/frontend/src/components/AutonTask.vue +++ b/src/teleoperation/frontend/src/components/AutonTask.vue @@ -16,9 +16,6 @@

Nav State: {{ navState }}

-

Joystick Values

@@ -47,7 +44,7 @@
- +
@@ -61,7 +58,6 @@ import { mapActions, mapState, mapGetters } from 'vuex' import DriveMoteusStateTable from './DriveMoteusStateTable.vue' import AutonRoverMap from './AutonRoverMap.vue' import AutonWaypointEditor from './AutonWaypointEditor.vue' -import CameraFeed from './CameraFeed.vue' import Cameras from './Cameras.vue' import MotorsStatusTable from './MotorsStatusTable.vue' import OdometryReading from './OdometryReading.vue' @@ -78,7 +74,6 @@ export default defineComponent({ DriveMoteusStateTable, AutonRoverMap, AutonWaypointEditor, - CameraFeed, Cameras, MotorsStatusTable, OdometryReading, diff --git a/src/teleoperation/frontend/src/components/CameraDisplay.vue b/src/teleoperation/frontend/src/components/CameraDisplay.vue index b44cea90b..83d7632e5 100644 --- a/src/teleoperation/frontend/src/components/CameraDisplay.vue +++ b/src/teleoperation/frontend/src/components/CameraDisplay.vue @@ -1,11 +1,15 @@ @@ -57,6 +83,10 @@ export default { isSA: { type: Boolean, required: true + }, + mission: { + type: String, // {'sa', 'ik', 'other'} + required: true } }, data() { diff --git a/src/teleoperation/frontend/src/components/DMESTask.vue b/src/teleoperation/frontend/src/components/DMESTask.vue index edb586ff3..b83e5ff9b 100644 --- a/src/teleoperation/frontend/src/components/DMESTask.vue +++ b/src/teleoperation/frontend/src/components/DMESTask.vue @@ -22,7 +22,7 @@
- +
@@ -83,8 +83,8 @@ export default defineComponent({ MotorsStatusTable, OdometryReading, DriveControls, - MastGimbalControls, -}, + MastGimbalControls + }, props: { type: { @@ -107,7 +107,8 @@ export default defineComponent({ name: [] as string[], error: [] as string[], state: [] as string[], - limit_hit: [] as boolean[] /* Each motor stores an array of 4 indicating which limit switches are hit */ + limit_hit: + [] as boolean[] /* Each motor stores an array of 4 indicating which limit switches are hit */ }, motorData: { @@ -139,8 +140,7 @@ export default defineComponent({ this.moteusDrive.state = msg.state this.moteusDrive.error = msg.error this.moteusDrive.limit_hit = msg.limit_hit - } - else if (msg.type == "center_map") { + } else if (msg.type == 'center_map') { this.odom.latitude_deg = msg.latitude this.odom.longitude_deg = msg.longitude } @@ -149,20 +149,20 @@ export default defineComponent({ methods: { ...mapActions('websocket', ['sendMessage']), - cancelIK: function() { - this.sendMessage({ "type": "cancel_click_ik" }); + cancelIK: function () { + this.sendMessage({ type: 'cancel_click_ik' }) } }, created: function () { window.setTimeout(() => { - this.sendMessage({ "type": "center_map" }); + this.sendMessage({ type: 'center_map' }) }, 250) window.addEventListener('keydown', (event: KeyboardEvent) => { - if (event.key === ' ') { - this.cancelIK(event) - } - }) + if (event.key === ' ') { + this.cancelIK(event) + } + }) } }) diff --git a/src/teleoperation/frontend/src/components/IKCameraFeed.vue b/src/teleoperation/frontend/src/components/IKCameraFeed.vue new file mode 100644 index 000000000..2fab8ac1a --- /dev/null +++ b/src/teleoperation/frontend/src/components/IKCameraFeed.vue @@ -0,0 +1,62 @@ + + + + + + diff --git a/src/teleoperation/frontend/src/components/ISHTask.vue b/src/teleoperation/frontend/src/components/ISHTask.vue index 1a42a8357..d1f561ffe 100644 --- a/src/teleoperation/frontend/src/components/ISHTask.vue +++ b/src/teleoperation/frontend/src/components/ISHTask.vue @@ -22,7 +22,7 @@
-->
- +
+ + + + diff --git a/src/teleoperation/frontend/src/components/SATask.vue b/src/teleoperation/frontend/src/components/SATask.vue index 87e041229..45af1e6fa 100644 --- a/src/teleoperation/frontend/src/components/SATask.vue +++ b/src/teleoperation/frontend/src/components/SATask.vue @@ -28,7 +28,7 @@
- +
@@ -164,7 +164,7 @@ export default { }, computed: { - ...mapState('websocket', ['message']), + ...mapState('websocket', ['message']) }, watch: { @@ -181,13 +181,11 @@ export default { this.moteusState.state = msg.state this.moteusState.error = msg.error this.moteusState.limit_hit = msg.limit_hit - } + } } }, - created: function () { - - } + created: function () {} } From 1b393636c652138ad474af3f5e437ff7e8479d3d Mon Sep 17 00:00:00 2001 From: jnnanni Date: Sun, 24 Mar 2024 14:34:24 -0400 Subject: [PATCH 45/54] Set up camera feed types with new camera code --- .../frontend/src/components/CameraDisplay.vue | 132 +++++----- .../frontend/src/components/CameraFeed.vue | 234 ++++++++++++------ .../frontend/src/components/Cameras.vue | 6 +- .../frontend/src/components/IKCameraFeed.vue | 62 ----- .../frontend/src/components/ISHTask.vue | 45 ++-- .../frontend/src/components/SACameraFeed.vue | 95 ------- 6 files changed, 245 insertions(+), 329 deletions(-) delete mode 100644 src/teleoperation/frontend/src/components/IKCameraFeed.vue delete mode 100644 src/teleoperation/frontend/src/components/SACameraFeed.vue diff --git a/src/teleoperation/frontend/src/components/CameraDisplay.vue b/src/teleoperation/frontend/src/components/CameraDisplay.vue index 230f76db3..c4c8c1860 100644 --- a/src/teleoperation/frontend/src/components/CameraDisplay.vue +++ b/src/teleoperation/frontend/src/components/CameraDisplay.vue @@ -1,76 +1,72 @@ - - - - \ No newline at end of file + } +}) + + + diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index 3dbc26a23..52d18462d 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -1,11 +1,23 @@ - + diff --git a/src/teleoperation/frontend/src/components/Cameras.vue b/src/teleoperation/frontend/src/components/Cameras.vue index 1e1b80e78..85dee5acb 100644 --- a/src/teleoperation/frontend/src/components/Cameras.vue +++ b/src/teleoperation/frontend/src/components/Cameras.vue @@ -135,8 +135,8 @@ export default { ...mapActions('websocket', ['sendMessage']), setCamIndex: function (index: number) { - console.log(typeof index) - console.log(this.camsEnabled[index]) + // console.log(typeof index) + // console.log(this.camsEnabled[index]) // every time a button is pressed, it changes cam status and adds/removes from stream this.camsEnabled[index] = !this.camsEnabled[index] if (this.camsEnabled[index]) this.qualities[index] = 2 //if enabling camera, turn on medium quality @@ -184,7 +184,7 @@ export default { }, takePanorama() { - this.sendMessage({ type: "takePanorama" }); + this.sendMessage({ type: 'takePanorama' }) } } } diff --git a/src/teleoperation/frontend/src/components/IKCameraFeed.vue b/src/teleoperation/frontend/src/components/IKCameraFeed.vue deleted file mode 100644 index 2fab8ac1a..000000000 --- a/src/teleoperation/frontend/src/components/IKCameraFeed.vue +++ /dev/null @@ -1,62 +0,0 @@ - - - - - - diff --git a/src/teleoperation/frontend/src/components/ISHTask.vue b/src/teleoperation/frontend/src/components/ISHTask.vue index e4f0a7be0..8be0a6a1c 100644 --- a/src/teleoperation/frontend/src/components/ISHTask.vue +++ b/src/teleoperation/frontend/src/components/ISHTask.vue @@ -9,41 +9,45 @@ Help
-
- Joystick +
+ Joystick
- +
- +
- -
-
- +
- +
- -
+ +
- +
- - From c3b98c9a614f4461a0482182eb211890f4821a3f Mon Sep 17 00:00:00 2001 From: Sarah Shapin Date: Tue, 26 Mar 2024 20:49:41 -0400 Subject: [PATCH 46/54] Moved a lot of CameraInfo stuff to Cameras and CameraDisplay --- .../frontend/src/components/AminoBenedict.vue | 5 -- .../frontend/src/components/CameraDisplay.vue | 16 ++++++- .../frontend/src/components/CameraFeed.vue | 47 +++++++++++++++---- .../frontend/src/components/CameraInfo.vue | 2 +- .../frontend/src/components/Cameras.vue | 2 +- 5 files changed, 54 insertions(+), 18 deletions(-) diff --git a/src/teleoperation/frontend/src/components/AminoBenedict.vue b/src/teleoperation/frontend/src/components/AminoBenedict.vue index 9d73155d7..a55e288ee 100644 --- a/src/teleoperation/frontend/src/components/AminoBenedict.vue +++ b/src/teleoperation/frontend/src/components/AminoBenedict.vue @@ -37,11 +37,6 @@ :show_name="true" /> -
-
- -
-
diff --git a/src/teleoperation/frontend/src/components/CameraDisplay.vue b/src/teleoperation/frontend/src/components/CameraDisplay.vue index c4c8c1860..5cfdc4d78 100644 --- a/src/teleoperation/frontend/src/components/CameraDisplay.vue +++ b/src/teleoperation/frontend/src/components/CameraDisplay.vue @@ -3,7 +3,7 @@
- +
@@ -19,6 +19,14 @@ export default defineComponent({ CameraFeed }, props: { + name: { + type: Array, + required: true + }, + id: { + type: Array, + required: true + }, streamOrder: { //array of camera indices to stream. -1 indicates not used type: Array, @@ -35,6 +43,12 @@ export default defineComponent({ } }, + computed: { + getID(i: number) { + return this.streamOrder[i - 1] + } + }, + watch: { streamOrder: { handler: function () { diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index 52d18462d..c7dc898df 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -1,16 +1,23 @@ @@ -21,6 +28,10 @@ import Checkbox from './Checkbox.vue' export default defineComponent({ props: { + name: { + type: String, + required: true + }, id: { type: Number, required: true @@ -33,6 +44,13 @@ export default defineComponent({ components: { Checkbox }, + + computed: { + isPrimary() { + return (this.mission === 'sa') + } + }, + data() { return { ws: null as WebSocket | null, @@ -42,7 +60,8 @@ export default defineComponent({ IKCam: false, // SA/ISH Mode - site: 'A' + site: 'A', + quality: 2 } }, @@ -93,6 +112,14 @@ export default defineComponent({ this.sendMessage({ type: 'start_click_ik', data: { x: event.offsetX, y: event.offsetY } }) } }, + changeQuality: function () { + this.sendMessage({ + type: 'sendCameras', + primary: this.isPrimary(), + device: this.id, + resolution: this.quality + }) + }, toggleIKMode: function () { this.IKCam = !this.IKCam }, diff --git a/src/teleoperation/frontend/src/components/CameraInfo.vue b/src/teleoperation/frontend/src/components/CameraInfo.vue index 645cc7e3d..d15c34441 100644 --- a/src/teleoperation/frontend/src/components/CameraInfo.vue +++ b/src/teleoperation/frontend/src/components/CameraInfo.vue @@ -1,6 +1,6 @@ From 1d688a22ee252d1bfe6e76b94b6fd189630c29e8 Mon Sep 17 00:00:00 2001 From: Neven Johnson Date: Thu, 28 Mar 2024 19:52:38 -0400 Subject: [PATCH 47/54] added arm controller status publishing --- msg/ArmStatus.msg | 1 + src/perception/click_ik/click_ik.cpp | 13 ++- src/perception/click_ik/click_ik.hpp | 2 + .../arm_controller/arm_controller.cpp | 109 ++++++++---------- .../arm_controller/arm_controller.hpp | 1 + src/teleoperation/arm_controller/pch.hpp | 1 + 6 files changed, 64 insertions(+), 63 deletions(-) create mode 100644 msg/ArmStatus.msg diff --git a/msg/ArmStatus.msg b/msg/ArmStatus.msg new file mode 100644 index 000000000..4f47d908d --- /dev/null +++ b/msg/ArmStatus.msg @@ -0,0 +1 @@ +bool status \ No newline at end of file diff --git a/src/perception/click_ik/click_ik.cpp b/src/perception/click_ik/click_ik.cpp index af8a715fb..f9b57dc9a 100644 --- a/src/perception/click_ik/click_ik.cpp +++ b/src/perception/click_ik/click_ik.cpp @@ -62,6 +62,9 @@ namespace mrover { // IK Publisher mIkPub = mNh.advertise("/arm_ik", 1); + // ArmStatus subscriber + mStatusSub = mNh.subscribe("arm_status", 1, &ClickIkNodelet::statusCallback, this); + ROS_INFO("Starting action server"); server.registerGoalCallback([this]() { startClickIk(); }); server.registerPreemptCallback([this] { cancelClickIk(); }); @@ -119,6 +122,14 @@ namespace mrover { return std::make_optional(point); } - + void ClickIkNodelet::statusCallback(ArmStatus const& status) { + if (!status.status) { + cancelClickIk(); + ClickIkResult result; + result.success = false; + server.setAborted(result, "Arm position unreachable"); + NODELET_WARN("Arm position unreachable"); + } + } } // namespace mrover diff --git a/src/perception/click_ik/click_ik.hpp b/src/perception/click_ik/click_ik.hpp index 186b07063..224aad184 100644 --- a/src/perception/click_ik/click_ik.hpp +++ b/src/perception/click_ik/click_ik.hpp @@ -8,6 +8,7 @@ namespace mrover { ros::NodeHandle mNh, mPnh; ros::Subscriber mPcSub; ros::Publisher mIkPub; + ros::Subscriber mStatusSub; actionlib::SimpleActionServer server = actionlib::SimpleActionServer(mNh, "do_click_ik", false); IK message; @@ -30,6 +31,7 @@ namespace mrover { void onInit() override; auto pointCloudCallback(sensor_msgs::PointCloud2ConstPtr const& msg) -> void; + void statusCallback(ArmStatus const&); void startClickIk(); void cancelClickIk(); diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index 9cfaffd59..eb1839b55 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -12,6 +12,7 @@ namespace mrover { nh.param("/frequency", frequency, 100); mIkSubscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); mPositionPublisher = nh.advertise("arm_position_cmd", 1); + mStatusPublisher = nh.advertise("arm_status", 1); } auto yawSo3(double r) -> SO3d { @@ -22,54 +23,26 @@ namespace mrover { auto ArmController::ik_callback(IK const& ik_target) -> void { Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; - positions.positions.resize(positions.names.size(), 0.f); - SE3d target_frame_to_arm_b_static; - while (ros::ok()) { - try { - // ROS_INFO("%d", mTfBuffer._frameExists("joint_b_static")); - // target_frame_to_arm_a_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "joint_a_static"); - target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); - // ROS_INFO("Found tf"); - break; - } catch (...) { - ROS_INFO("Failed to grab tf"); - continue; - } + positions.positions.resize(positions.names.size()); + SE3d target_frame_to_arm_base_link; + try { + target_frame_to_arm_base_link = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); + } catch (...) { + ROS_WARN_THROTTLE(1, "Failed to resolve information about target frame"); + return; } Eigen::Vector4d target{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z, 1}; - // double x = ik_target.target.pose.position.x; - // double y = ik_target.target.pose.position.y; - // double z = ik_target.target.pose.position.z; - // maybe just add offset instead of doing multiplication (we don't do any rotation?) - Eigen::Vector4d target_in_arm_b_static = target_frame_to_arm_b_static.transform() * target; - double x = target_in_arm_b_static.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector - double z = target_in_arm_b_static.z(); - double y = target_in_arm_b_static.y(); - // ROS_INFO("x: %f, y: %f, z: %f", x, y, z); - // SE3d pos{{target_in_arm_a_static.x(), target_in_arm_a_static.y(), target_in_arm_a_static.z()}, SO3d::Identity()}; - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", target_frame_to_arm_b_static); - - // SE3d link_ab = SE3Conversions::fromTfTree(buffer, "arm_a_link", "arm_b_link"); - // SE3d link_bc = SE3Conversions::fromTfTree(buffer, "arm_b_link", "arm_c_link"); - // SE3d link_cd = SE3Conversions::fromTfTree(buffer, "arm_c_link", "arm_d_link"); - // SE3d link_de = SE3Conversions::fromTfTree(buffer, "arm_d_link", "arm_e_link"); - - // SE3d offset = SE3Conversions::fromTfTree(buffer, "chassis_link", "arm_e_link").inverse() * pos; - // ROS_INFO("x: %f, y: %f, z: %f", offset.translation().x(), offset.translation().y(), offset.translation().z()); - // SE3d thing = SE3Conversions::fromTfTree(buffer, "arm_c_link", "arm_d_link"); - - // double C = x * x + z * z - LINK_BC * LINK_BC - LINK_CD * LINK_CD; - // double q2 = std::acos(C / (2 * LINK_BC * LINK_CD)); - // double q1 = std::atan2(z, x) - std::atan2(LINK_BC * std::sin(q2), LINK_BC + LINK_CD * std::cos(q2)); - // // q1 = std::clamp(q1, -TAU / 8, 0.0); - // double q3 = -(q1 + q2); - - // ROS_INFO("x: %f, y: %f, z: %f, q1: %f, q2: %f, q3: %f", x, y, z, q1, q2, q3); + Eigen::Vector4d target_in_arm_base_link = target_frame_to_arm_base_link.transform() * target; + double x = target_in_arm_base_link.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector + double z = target_in_arm_base_link.z(); + double y = target_in_arm_base_link.y(); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", target_frame_to_arm_base_link); + // final angle of end effector double gamma = 0; + double x3 = x - LINK_DE * std::cos(gamma); double z3 = z - LINK_DE * std::sin(gamma); - // SE3d joint_e_pos{{x3, 0, z3}, SO3d::Identity()}; double C = std::sqrt(x3 * x3 + z3 * z3); double alpha = std::acos((LINK_BC * LINK_BC + LINK_CD * LINK_CD - C * C) / (2 * LINK_BC * LINK_CD)); @@ -78,31 +51,43 @@ namespace mrover { double thetaB = -1 * (std::numbers::pi - alpha); double thetaC = gamma - thetaA - thetaB; - SE3d joint_b_pos{{0.034346, 0, 0.049024}, SO3d::Identity()}; - SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, yawSo3(-thetaA)}; - SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, yawSo3(-thetaB)}; - SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, yawSo3(-thetaC)}; - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_e_target", "arm_d_target", joint_e_pos); - double q1 = -thetaA; double q2 = -thetaB + 0.1608485915; double q3 = -thetaC - 0.1608485915; - if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3) && - y >= JOINT_A_MIN && y <= JOINT_A_MAX && - q1 >= JOINT_B_MIN && q1 <= JOINT_B_MAX && - q2 >= JOINT_C_MIN && q2 <= JOINT_C_MAX && - q3 >= JOINT_DE_PITCH_MIN && q3 <= JOINT_DE_PITCH_MAX) { - positions.positions[0] = static_cast(y); - positions.positions[1] = static_cast(q1); - positions.positions[2] = static_cast(q2); - positions.positions[3] = static_cast(q3); - mPositionPublisher.publish(positions); + if (std::isfinite(q1) && std::isfinite(q2) && std::isfinite(q3)) { + SE3d joint_b_pos{{0.034346, 0, 0.049024}, SO3d::Identity()}; + SE3d joint_c_pos{{LINK_BC * cos(thetaA), 0, LINK_BC * sin(thetaA)}, yawSo3(-thetaA)}; + SE3d joint_d_pos{{LINK_CD * cos(thetaB), 0, LINK_CD * sin(thetaB)}, yawSo3(-thetaB)}; + SE3d joint_e_pos{{LINK_DE * cos(thetaC), 0, LINK_DE * sin(thetaC)}, yawSo3(-thetaC)}; + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_b_target", "arm_a_link", joint_b_pos); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_c_target", "arm_b_target", joint_c_pos); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_d_target", "arm_c_target", joint_d_pos); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_e_target", "arm_d_target", joint_e_pos); + + if (y >= JOINT_A_MIN && y <= JOINT_A_MAX && + q1 >= JOINT_B_MIN && q1 <= JOINT_B_MAX && + q2 >= JOINT_C_MIN && q2 <= JOINT_C_MAX && + q3 >= JOINT_DE_PITCH_MIN && q3 <= JOINT_DE_PITCH_MAX) { + positions.positions[0] = static_cast(y); + positions.positions[1] = static_cast(q1); + positions.positions[2] = static_cast(q2); + positions.positions[3] = static_cast(q3); + mPositionPublisher.publish(positions); + ArmStatus status; + status.status = true; + mStatusPublisher.publish(status); + } else { + ROS_WARN_THROTTLE(1, "Can not reach target within arm limits!"); + ArmStatus status; + status.status = false; + mStatusPublisher.publish(status); + } } else { - // ROS_INFO("Can't reach arm target!"); + ROS_WARN_THROTTLE(1, "Can not solve for arm target!"); + ArmStatus status; + status.status = false; + mStatusPublisher.publish(status); } } diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index ed115506d..00c2b9df7 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -8,6 +8,7 @@ namespace mrover { [[maybe_unused]] ros::Subscriber mIkSubscriber; ros::Publisher mPositionPublisher; + ros::Publisher mStatusPublisher; tf2_ros::TransformBroadcaster mTfBroadcaster; tf2_ros::Buffer mTfBuffer{}; tf2_ros::TransformListener mTfListener{mTfBuffer}; diff --git a/src/teleoperation/arm_controller/pch.hpp b/src/teleoperation/arm_controller/pch.hpp index b827f9f71..fb01e862a 100644 --- a/src/teleoperation/arm_controller/pch.hpp +++ b/src/teleoperation/arm_controller/pch.hpp @@ -13,3 +13,4 @@ #include #include #include +#include From fc60c93851e162710089fb0c9b212be1cd0864e6 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 2 Apr 2024 19:11:22 -0400 Subject: [PATCH 48/54] fix arm controller limits --- src/teleoperation/arm_controller/arm_controller.cpp | 2 +- src/teleoperation/arm_controller/arm_controller.hpp | 9 ++++----- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index eb1839b55..1357c4899 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -9,7 +9,7 @@ namespace mrover { ros::NodeHandle nh; // sleep(2); double frequency{}; - nh.param("/frequency", frequency, 100); + mNh.param("/frequency", frequency, 100); mIkSubscriber = nh.subscribe("arm_ik", 1, &ArmController::ik_callback, this); mPositionPublisher = nh.advertise("arm_position_cmd", 1); mStatusPublisher = nh.advertise("arm_status", 1); diff --git a/src/teleoperation/arm_controller/arm_controller.hpp b/src/teleoperation/arm_controller/arm_controller.hpp index bc8a6bc05..2d42868d0 100644 --- a/src/teleoperation/arm_controller/arm_controller.hpp +++ b/src/teleoperation/arm_controller/arm_controller.hpp @@ -7,6 +7,7 @@ namespace mrover { class ArmController { [[maybe_unused]] ros::Subscriber mIkSubscriber; + ros::NodeHandle mNh; ros::Publisher mPositionPublisher; ros::Publisher mStatusPublisher; tf2_ros::TransformBroadcaster mTfBroadcaster; @@ -19,9 +20,8 @@ namespace mrover { static constexpr double LINK_BC = 0.5344417294; static constexpr double LINK_CD = 0.5531735368; static constexpr double LINK_DE = 0.044886000454425812; - // double const OFFSET = std::atan2(0.09, LINK_CD); - static constexpr double JOINT_A_MIN = -0.45; - static constexpr double JOINT_A_MAX = 0; + static constexpr double JOINT_A_MIN = 0; + static constexpr double JOINT_A_MAX = 0.45; static constexpr double JOINT_B_MIN = -0.25 * std::numbers::pi; static constexpr double JOINT_B_MAX = 0; static constexpr double JOINT_C_MIN = -0.959931; @@ -29,11 +29,10 @@ namespace mrover { static constexpr double JOINT_DE_PITCH_MIN = -0.75 * std::numbers::pi; static constexpr double JOINT_DE_PITCH_MAX = 0.75 * std::numbers::pi; static constexpr double END_EFFECTOR_LENGTH = 0.13; // measured from blender - // constexpr double JOINT_DE_ROLL_MIN = -0.75 * std::numbers::pi; - // constexpr double JOINT_DE_ROLL_MAX = 0.75 * std::numbers::pi; ArmController(); void ik_callback(IK const& new_ik_target); }; + } // namespace mrover \ No newline at end of file From c7a277cfe5c1ed80eea6f2b30b9a550de2886852 Mon Sep 17 00:00:00 2001 From: Alison Ryckman Date: Tue, 2 Apr 2024 19:31:27 -0400 Subject: [PATCH 49/54] womp womp --- src/teleoperation/frontend/src/components/CameraFeed.vue | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index 621dcad60..49cd1a1ef 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -23,7 +23,7 @@ export default defineComponent({ }, data() { return { - IKCam: false + IKCam: false, ws: null as WebSocket | null, isUnmounted: false, } From 2a6126b66ccb563ab42683d5552e46e4af456e02 Mon Sep 17 00:00:00 2001 From: Sarah Shapin Date: Tue, 2 Apr 2024 20:16:10 -0400 Subject: [PATCH 50/54] Auton cameras working, CameraInfo now fully unnecessary --- scripts/debug_service.py | 8 +-- src/teleoperation/backend/consumers.py | 8 --- src/teleoperation/db.sqlite3 | Bin 139264 -> 139264 bytes .../frontend/src/components/AutonTask.vue | 7 ++- .../frontend/src/components/CameraDisplay.vue | 16 +++--- .../frontend/src/components/CameraFeed.vue | 52 +++++++++--------- .../frontend/src/components/Cameras.vue | 14 ----- 7 files changed, 44 insertions(+), 61 deletions(-) diff --git a/scripts/debug_service.py b/scripts/debug_service.py index ce2b7d761..775f688d4 100755 --- a/scripts/debug_service.py +++ b/scripts/debug_service.py @@ -6,16 +6,16 @@ from typing import Any import rospy -from std_srvs.srv import SetBool, SetBoolResponse +from mrover.srv import ChangeCameras, ChangeCamerasResponse # Change these values for the service name and type definition to test different values -SERVICE_NAME = "science_enable_heater_b1" -SERVICE_TYPE = SetBool +SERVICE_NAME = "change_cameras" +SERVICE_TYPE = ChangeCameras def print_service_request(service_request: Any): rospy.loginfo(service_request) - return SetBoolResponse(success=True) + return ChangeCamerasResponse(success=True) def main(): diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 9347dda15..8a03b2129 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -698,14 +698,6 @@ def capture_panorama(self) -> None: except rospy.ServiceException as e: print(f"Service call failed: {e}") - # def capture_photo(self): - # try: - # response = self.capture_photo_srv() - # image = response.photo - # self.image_callback(image) - # except rospy.ServiceException as e: - # print(f"Service call failed: {e}") - # def image_callback(self, msg): # bridge = CvBridge() # try: diff --git a/src/teleoperation/db.sqlite3 b/src/teleoperation/db.sqlite3 index b8faa90637c5ec8f9ac8e6e70a1b1965e05498d0..a8ca65870f23ca866a85247fca9f12f89a2af8e5 100644 GIT binary patch delta 25 gcmZoTz|nAkV}dke@Joystick Values

+
+ +
@@ -44,7 +47,7 @@
- +
@@ -59,6 +62,7 @@ import DriveMoteusStateTable from './DriveMoteusStateTable.vue' import AutonRoverMap from './AutonRoverMap.vue' import AutonWaypointEditor from './AutonWaypointEditor.vue' import Cameras from './Cameras.vue' +import CameraFeed from './CameraFeed.vue' import MotorsStatusTable from './MotorsStatusTable.vue' import OdometryReading from './OdometryReading.vue' import JoystickValues from './JoystickValues.vue' @@ -75,6 +79,7 @@ export default defineComponent({ AutonRoverMap, AutonWaypointEditor, Cameras, + CameraFeed, MotorsStatusTable, OdometryReading, JoystickValues, diff --git a/src/teleoperation/frontend/src/components/CameraDisplay.vue b/src/teleoperation/frontend/src/components/CameraDisplay.vue index 5cfdc4d78..532d7b1d5 100644 --- a/src/teleoperation/frontend/src/components/CameraDisplay.vue +++ b/src/teleoperation/frontend/src/components/CameraDisplay.vue @@ -3,7 +3,7 @@
- +
@@ -19,11 +19,7 @@ export default defineComponent({ CameraFeed }, props: { - name: { - type: Array, - required: true - }, - id: { + names: { type: Array, required: true }, @@ -33,7 +29,7 @@ export default defineComponent({ required: true }, mission: { - type: String, // {'sa', 'ik', 'other'} + type: String, // {'sa', 'ik', 'auton'} required: true } }, @@ -44,9 +40,13 @@ export default defineComponent({ }, computed: { + + }, + + methods: { getID(i: number) { return this.streamOrder[i - 1] - } + }, }, watch: { diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index c7dc898df..e5304206d 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -1,23 +1,23 @@ @@ -37,7 +37,7 @@ export default defineComponent({ required: true }, mission: { - type: String, // {'sa', 'ik', 'other'} + type: String, // {'sa', 'ik', 'auton'} required: true } }, @@ -45,12 +45,6 @@ export default defineComponent({ Checkbox }, - computed: { - isPrimary() { - return (this.mission === 'sa') - } - }, - data() { return { ws: null as WebSocket | null, @@ -115,9 +109,9 @@ export default defineComponent({ changeQuality: function () { this.sendMessage({ type: 'sendCameras', - primary: this.isPrimary(), + primary: this.mission === 'sa', device: this.id, - resolution: this.quality + resolution: parseInt(this.quality) }) }, toggleIKMode: function () { @@ -282,4 +276,10 @@ export default defineComponent({ flex-direction: column; gap: 5px; } + +canvas { + width:640px; + height:480px; +} + diff --git a/src/teleoperation/frontend/src/components/Cameras.vue b/src/teleoperation/frontend/src/components/Cameras.vue index 0a7dc0096..4aa5e5021 100644 --- a/src/teleoperation/frontend/src/components/Cameras.vue +++ b/src/teleoperation/frontend/src/components/Cameras.vue @@ -39,18 +39,6 @@

All Cameras

-
- -