From 1a0224271170550293e0efa7570d92bf2aed431e Mon Sep 17 00:00:00 2001 From: Luis Maldonado <128458400+BlackSnow-333@users.noreply.github.com> Date: Mon, 18 Mar 2024 09:56:29 +0100 Subject: [PATCH 01/27] Update README.md --- README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index 401deaf..c796ed8 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,8 @@ WS_PANTOGRAPH=~/dev/ws_pantograph_ros2/ mkdir -p $WS_PANTOGRAPH/src cd $WS_PANTOGRAPH/src -git clone https://github.com/ICube-Robotics/needle_pantograph_ros2.git +# git clone https://github.com/ICube-Robotics/needle_pantograph_ros2.git +git clone https://github.com/BlackSnow-333/needle_pantograph_ros2.git vcs import . < needle_pantograph_ros2/needle_pantograph_ros2.repos rosdep install --ignore-src --from-paths . -y -r From 724c02b65c96fb9a4ad063614f3f3427be912553 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 27 Mar 2024 10:40:47 +0100 Subject: [PATCH 02/27] add needle to description pkg --- .../config/ros2_controllers.yaml | 9 ++++++ .../pantograph.r2c_hardware.xacro | 19 ++++++++++++ .../urdf/pantograph.urdf.xacro | 30 +++++++++++++++++++ 3 files changed, 58 insertions(+) diff --git a/pantograph_description/config/ros2_controllers.yaml b/pantograph_description/config/ros2_controllers.yaml index 89e1f27..b4672e6 100644 --- a/pantograph_description/config/ros2_controllers.yaml +++ b/pantograph_description/config/ros2_controllers.yaml @@ -11,6 +11,9 @@ controller_manager: type: pantograph_mimick_controller/PantographMimickController forward_effort_controller: type: effort_controllers/JointGroupEffortController + # tool_orientation_controller: + # type: position_controllers/JointGroupPositionController + haptic_trajectory_controller: @@ -32,3 +35,9 @@ forward_effort_controller: joints: - panto_a1 - panto_a5 + +# tool_orientation_controller: +# ros_parameters: +# joints: +# - tool_roll +# - tool_pitch diff --git a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro index 05ae079..d91e26b 100644 --- a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro +++ b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro @@ -142,6 +142,25 @@ + + + + + ${0} + -3 + 3 + + + + + + + ${0} + -3 + 3 + + + diff --git a/pantograph_description/urdf/pantograph.urdf.xacro b/pantograph_description/urdf/pantograph.urdf.xacro index d12c897..c804730 100644 --- a/pantograph_description/urdf/pantograph.urdf.xacro +++ b/pantograph_description/urdf/pantograph.urdf.xacro @@ -185,6 +185,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From f45b1f3250e0e7ce1b49dc98f732b1ee5ed7f8d2 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Thu, 4 Apr 2024 08:40:29 +0200 Subject: [PATCH 03/27] added C++ model of the complete system for visualization --- .../pantograph_library/pantograph_model.hpp | 22 ++- pantograph_library/src/pantograph_model.cpp | 168 +++++++++++++++++- 2 files changed, 186 insertions(+), 4 deletions(-) diff --git a/pantograph_library/include/pantograph_library/pantograph_model.hpp b/pantograph_library/include/pantograph_library/pantograph_model.hpp index 68cfa3b..411b689 100644 --- a/pantograph_library/include/pantograph_library/pantograph_model.hpp +++ b/pantograph_library/include/pantograph_library/pantograph_model.hpp @@ -34,7 +34,7 @@ class PantographModel double l_a2 = 0.165, double l_a3 = 0.165, double l_a4 = 0.1, - double l_a5 = 0.085); + double l_a5 = 0.085); // length in m /// Populate all joints position /// -> [jnt_a1, jnt_a2, ..., jnt_a5] = populate_all_joint_positions(q = [jnt_a1, jnt_a5]) @@ -49,6 +49,18 @@ class PantographModel /// Jacobian J(q, q_dot), where q = [joint_a1, joint_a5].T Eigen::Matrix jacobian(Eigen::Vector q, Eigen::Vector q_dot); + /// Forward kinematics of the complete system p = fk_needle(P3) = fk_needle(fk([q1,q2])) + Eigen::Vector fk_system(Eigen::Vector q); + + /// Inverse kinematics of the complete system p = fk_needle(P3) = fk_needle(fk([q1,q2])) + Eigen::Vector ik_system(Eigen::Vector p); + + /// Populate all joints position of the full system (pantograph + needle) + Eigen::Vector populate_all_joint_positions_full_system(Eigen::Vector q); + + /// Calculate the force that the pantograph needs to apply to create a force felt by the user + double get_panto_force(Eigen::Vector p, double f_guide, double alpha); + protected: /// First left link length in m (i.e., A1->A2) double l_a1_; @@ -64,6 +76,14 @@ class PantographModel /// Dist between A1 and A5 in m (i.e., A4->A5) double l_a5_; + + /// Length of the needle in m + double l_needle_ = 0.2; // in m + + /// Coords of insertion point according to CAD + double PI_x = 0.0425; + double PI_y = 0.16056; + double PI_z = 0.09; }; } // namespace pantograph_library diff --git a/pantograph_library/src/pantograph_model.cpp b/pantograph_library/src/pantograph_model.cpp index 7ebdec7..497cf83 100644 --- a/pantograph_library/src/pantograph_model.cpp +++ b/pantograph_library/src/pantograph_model.cpp @@ -118,11 +118,173 @@ PantographModel::ik(Eigen::Vector p) Eigen::Matrix PantographModel::jacobian(Eigen::Vector q, Eigen::Vector q_dot) { - (void) q; - (void) q_dot; + // (void) q; + // (void) q_dot; // TODO(tpoignonec): add jacobian computation - throw std::logic_error("Function not yet implemented!"); + // throw std::logic_error("Function not yet implemented!"); + + // Get the angles of all the pantograph joints + Eigen::Vector jnt_pos; + jnt_pos = populate_all_joint_positions(q); + + double theta2 = jnt_pos[1]; // Check if IKM is correct + double theta4 = jnt_pos[3]; + + // Jacobian coefficients according to T.CESARE report + double S14S42 = sin(q(0) - theta4) / sin(theta4 - theta2); + double S24S42 = sin(q(1) - theta4) / sin(theta4 - theta2); + + double J11 = -l_a1_ * (sin(q(1)) + sin(theta2) * S14S42); + double J12 = l_a4_ * sin(theta2) * S24S42; + double J21 = l_a1_ * (cos(q(1)) + cos(theta2) * S14S42); + double J22 = -l_a4_ * cos(theta2) * S24S42; + + Eigen::Matrix2d J; + + J << J11, J21, + J21, J22; + + return Eigen::Matrix::Zero(); } +Eigen::Vector +PantographModel::fk_system(Eigen::Vector q) +{ + // FKM of the complete system : calculates cartesian position of interaction point PU + // according to joint space coordinates q + Eigen::Vector3d PI, P3, P3I, PIU, PU; + + // Coordinates of insertion point PI in base frame according to CAD + PI[0] = PI_x; + PI[0] = PI_y; + PI[0] = PI_z; + + // PI[0] = 0.0425; + // PI[1] = 0.16056; + // PI[2] = 0.09; + + // Get coords of P3 in base frame according to FKM + Eigen::Vector2d P3_2D; + P3_2D = fk(q); + + // Convert P3 in the plane to coords in 3D + // P3[0] = P3_2D[0]; + // P3[1] = P3_2D[1]; + // P3[2] = 0; + P3 << P3_2D, 0; + + // Convert coords of PI in base frame to P3 frame + P3I = PI - P3; + + // Get azimuth (theta) and elevation (phi) angles (in radians) of vector P3PI + double theta = std::atan2(P3I[1], P3I[0]); + double phi = std::atan2(P3I[2], std::sqrt(std::pow(P3I[0], 2) + std::pow(P3I[1], 2))); + double Lin = std::sqrt(std::pow(P3I[0], 2) + std::pow(P3I[1], 2) + std::pow(P3I[2], 2)); + + // Calculate translation from PI to PU + double Lout = l_needle_ - Lin; + PIU[0] = Lout * std::cos(phi) * std::cos(theta); // Coords of PU in PI frame + PIU[1] = Lout * std::cos(phi) * std::sin(theta); + PIU[2] = Lout * std::sin(phi); + + // Convert coords of PU in PI frame to coords in base frame + PU = PI + PIU; + + return PU; +} +Eigen::Vector +PantographModel::ik_system(Eigen::Vector p) +{ + /* Inverse kinematics model of the complete system: + computes the angles of all the joints according + to the cartesian position of interaction point PU*/ + + Eigen::Vector3d PI, P3, PI3, PIU, PU; + + // Coords of insertion point PI in base frame according to CAD + + PI[0] = PI_x; + PI[0] = PI_y; + PI[0] = PI_z; + + // PI[0] = 0.0425; + // PI[1] = 0.16056; + // PI[2] = 0.09; + + // Transformation of PU coords in base frame to coords in PI frame + PIU = PU - PI; + + // Get azimuth (theta) and elevation (phi) angles (in radians) of vector PIPU + double theta = std::atan2(PIU[1], PIU[0]); + double phi = std::atan2(PIU[2], std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2))); + double Lout = std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2) + std::pow(PIU[2], 2)); + + // Get end effector position P3 + double Lin = l_needle_ - Lout; + + // Transformation from I to P3 in I frame + PI3[0] = Lin * std::cos(phi) * std::cos(theta); + PI3[1] = Lin * std::cos(phi) * std::sin(theta); + PI3[2] = Lin * std::sin(phi); + + // Transformation of P3 coords on I frame to coords in base frame + P3 = PI - PI3; + + Eigen::Vector P3_2D; + P3_2D[0] = P3[0]; + P3_2D[1] = P3[1]; + + // Get angles of the pantograph joints using IKM + Eigen::Vector jnt_pos; + jnt_pos = ik(P3_2D); + + // Return pantograph joints angles + needle orientation angles + Eigen::Vector jnt_ext_pos; + + jnt_ext_pos << jnt_pos, theta, phi; + + return jnt_ext_pos; +} + +// Calculate joint positions of the complete system for visualization +Eigen::Vector +PantographModel::populate_all_joint_positions_full_system(Eigen::Vector q) +{ + auto p = fk_system(q); + return ik_system(p); +} + +// Calculate force applied by the pantograph to create the force felt by the user +double +PantographModel::get_panto_force(Eigen::Vector p, double f_guide, double alpha) +{ + // Get all the robot joint angles IKM + Eigen::Vector jnt_ext_pos; + jnt_ext_pos = ik_system(p); + double theta = jnt_ext_pos[5]; // Azimuth angle + double phi = jnt_ext_pos[6]; // Elevation angle + + // get insertion point height + double H = PI_z; // = 0.09 based on insertion point coords + + // Calculate length of the needle segment between P3 and PI + double l_in = H/std::sin(phi); + + // Calculate length of the needle segment between PI and PU + double l_out = l_needle_ - l_in; + + /*Calculate the perpendicular component of the force that the system + needs to apply to make the user feel a force at the point PU */ + double f_perp = f_guide*(l_out/l_in); + + /* Calculate the force that the pantograph needs to generate + beta is the angle between the force vector Fmech and the needle vector */ + double cos_beta = std::cos(phi)*std::cos(theta-alpha); + double f_mech = f_perp*(l_in/l_out); + + return f_mech; +} + + } // namespace pantograph_library From 1abb6080ea4d03b61503ed33a03cca3f437eb5eb Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Fri, 5 Apr 2024 11:30:01 +0200 Subject: [PATCH 04/27] updated C++ model of the pantograph (added needle description and force estimation) --- .../pantograph_library/pantograph_model.hpp | 3 + pantograph_library/src/pantograph_model.cpp | 166 +++++++++++++----- 2 files changed, 123 insertions(+), 46 deletions(-) diff --git a/pantograph_library/include/pantograph_library/pantograph_model.hpp b/pantograph_library/include/pantograph_library/pantograph_model.hpp index 411b689..670e808 100644 --- a/pantograph_library/include/pantograph_library/pantograph_model.hpp +++ b/pantograph_library/include/pantograph_library/pantograph_model.hpp @@ -55,6 +55,9 @@ class PantographModel /// Inverse kinematics of the complete system p = fk_needle(P3) = fk_needle(fk([q1,q2])) Eigen::Vector ik_system(Eigen::Vector p); + /// Inverse kinematics of the pantograph according to equations of T.CESARE report + Eigen::Vector ik_panto_optimized(Eigen::Vector p); + /// Populate all joints position of the full system (pantograph + needle) Eigen::Vector populate_all_joint_positions_full_system(Eigen::Vector q); diff --git a/pantograph_library/src/pantograph_model.cpp b/pantograph_library/src/pantograph_model.cpp index 497cf83..ef13935 100644 --- a/pantograph_library/src/pantograph_model.cpp +++ b/pantograph_library/src/pantograph_model.cpp @@ -67,12 +67,17 @@ PantographModel::fk(Eigen::Vector q) // Coordinates of P3 with respect to P1 double dist_A2_to_A4 = (p_at_A2 - p_at_A4).norm(); - double P2Ph = - (std::pow( - l_a2_, - 2) - std::pow(l_a3_, 2) + std::pow(dist_A2_to_A4, 2)) / (2 * dist_A2_to_A4); - Eigen::Vector2d Ph = p_at_A2 + (P2Ph / dist_A2_to_A4) * (p_at_A4 - p_at_A2); - double P3Ph = std::sqrt(l_a2_ * 2 - P2Ph * 2); + + double P2Ph = (std::pow(l_a2_, 2) - std::pow(l_a3_, 2) + + std::pow(dist_A2_to_A4, 2)) / + (2 * dist_A2_to_A4); + + // Eigen::Vector2d Ph = p_at_A2 + (P2Ph / dist_A2_to_A4) * (p_at_A4 - p_at_A2); + Eigen::Vector2d Ph; + Ph[0] = p_at_A2[0] + (P2Ph / dist_A2_to_A4) * (p_at_A4[0] - p_at_A2[0]); + Ph[1] = p_at_A2[1] + (P2Ph / dist_A2_to_A4) * (p_at_A4[1] - p_at_A2[1]); + + double P3Ph = std::sqrt(std::pow(l_a2_, 2) - std::pow(P2Ph, 2)); Eigen::Vector2d p_at_A3; p_at_A3[0] = Ph[0] + (P3Ph / dist_A2_to_A4) * (p_at_A4[1] - p_at_A2[1]); p_at_A3[1] = Ph[1] - (P3Ph / dist_A2_to_A4) * (p_at_A4[0] - p_at_A2[0]); @@ -148,6 +153,77 @@ PantographModel::jacobian(Eigen::Vector q, Eigen::Vector q return Eigen::Matrix::Zero(); } +Eigen::Vector +PantographModel::ik_panto_optimized(Eigen::Vector P3) +{ + Eigen::Vector jnt_pos; + Eigen::Vector2d P2, P4, P5; + // Coords of P5 in base frame + P5 << -l_a5_, 0; + + // Calculate the length of the segment P1P3 + double a13 = P3.norm(); + + // Calculate the length of the segment P5P3 + double a53 = (P3 - P5).norm(); + + // Calculate the angle of the active joints + double alpha1 = + std::acos( + (std::pow(l_a1_, 2) - std::pow(l_a2_, 2) + std::pow(a13, 2)) / + (2 * l_a1_ * a13)); + + double beta1 = std::atan2(P3[1], -P3[0]); + + // theta1 + jnt_pos[0] = PI_CST - alpha1 - beta1; + + double alpha5 = std::acos( + (std::pow(a53, 2) + std::pow(l_a4_, 2) - std::pow(l_a3_, 2)) / + (2 * a53 * l_a4_)); + double beta5 = std::atan2(P3[1], P3[0] - l_a5_); + + // theta5 + jnt_pos[4] = alpha5 + beta5; + + // Calculate the other joint angles + // Coords of P2 in base frame + P2[0] = l_a1_ * std::cos(jnt_pos[0]); + P2[1] = l_a1_ * std::sin(jnt_pos[0]); + + // Coords of P4 in base frame + P4[0] = P5[0] + l_a4_ * std::cos(jnt_pos[1]); + P4[1] = P5[1] + l_a4_ * std::sin(jnt_pos[1]); + + // Calculate the length of the segment P2P4 + double a24 = std::sqrt(std::pow(P4[0] - P2[0], 2) + std::pow(P4[1] - P2[1], 2)); + + // Angles of the left arm + double gamma2 = + std::acos( + (std::pow(a24, 2) + std::pow(l_a2_, 2) - std::pow(l_a3_, 2)) / + (2 * l_a2_ * a24)); + + double delta = std::atan2(P4[1] - P2[1], P4[0] - P2[0]); + + // theta2 + jnt_pos[1] = gamma2 + delta; + + // Angles of the right arm + double gamma4 = + std::acos( + (std::pow(a24, 2) + std::pow(l_a3_, 2) - std::pow(l_a2_, 2)) / + (2 * a24 * l_a3_)); + + // theta4 + jnt_pos[3] = PI_CST - gamma4 - delta; + + // theta3 + jnt_pos[2] = -(PI_CST / 2) + gamma4 + 2 * gamma2; + + return jnt_pos; +} + Eigen::Vector PantographModel::fk_system(Eigen::Vector q) { @@ -156,22 +232,15 @@ PantographModel::fk_system(Eigen::Vector q) Eigen::Vector3d PI, P3, P3I, PIU, PU; // Coordinates of insertion point PI in base frame according to CAD - PI[0] = PI_x; - PI[0] = PI_y; - PI[0] = PI_z; - - // PI[0] = 0.0425; - // PI[1] = 0.16056; - // PI[2] = 0.09; + PI[0] = PI_x; // = 0.0425; + PI[1] = PI_y; // = 0.16056; + PI[2] = PI_z; // = 0.09; // Get coords of P3 in base frame according to FKM Eigen::Vector2d P3_2D; P3_2D = fk(q); // Convert P3 in the plane to coords in 3D - // P3[0] = P3_2D[0]; - // P3[1] = P3_2D[1]; - // P3[2] = 0; P3 << P3_2D, 0; // Convert coords of PI in base frame to P3 frame @@ -194,33 +263,28 @@ PantographModel::fk_system(Eigen::Vector q) return PU; } Eigen::Vector -PantographModel::ik_system(Eigen::Vector p) +PantographModel::ik_system(Eigen::Vector PU) { /* Inverse kinematics model of the complete system: computes the angles of all the joints according to the cartesian position of interaction point PU*/ - Eigen::Vector3d PI, P3, PI3, PIU, PU; + Eigen::Vector3d PI, P3, PI3, PIU; // Coords of insertion point PI in base frame according to CAD - - PI[0] = PI_x; - PI[0] = PI_y; - PI[0] = PI_z; - - // PI[0] = 0.0425; - // PI[1] = 0.16056; - // PI[2] = 0.09; + PI[0] = PI_x; // = 0.0425; + PI[1] = PI_y; // = 0.16056; + PI[2] = PI_z; // = 0.09; // Transformation of PU coords in base frame to coords in PI frame - PIU = PU - PI; + PIU = PI - PU; // Get azimuth (theta) and elevation (phi) angles (in radians) of vector PIPU double theta = std::atan2(PIU[1], PIU[0]); double phi = std::atan2(PIU[2], std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2))); double Lout = std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2) + std::pow(PIU[2], 2)); - // Get end effector position P3 + // Get length of the needle segment between PI and P3 double Lin = l_needle_ - Lout; // Transformation from I to P3 in I frame @@ -229,19 +293,22 @@ PantographModel::ik_system(Eigen::Vector p) PI3[2] = Lin * std::sin(phi); // Transformation of P3 coords on I frame to coords in base frame - P3 = PI - PI3; + // P3 = PI - PI3 + P3 = PI + PI3; + // Convert P3 coords in 3D to coords in plane Eigen::Vector P3_2D; P3_2D[0] = P3[0]; P3_2D[1] = P3[1]; // Get angles of the pantograph joints using IKM Eigen::Vector jnt_pos; - jnt_pos = ik(P3_2D); + jnt_pos = ik(P3_2D); // check if ik is correct + + // jnt_pos = ik_panto_optimized(P3_2D); // Return pantograph joints angles + needle orientation angles Eigen::Vector jnt_ext_pos; - jnt_ext_pos << jnt_pos, theta, phi; return jnt_ext_pos; @@ -255,36 +322,43 @@ PantographModel::populate_all_joint_positions_full_system(Eigen::Vector p, double f_guide, double alpha) +/* Calculate force applied by the pantograph (f_mech)to create the force +felt by the user with: +- alpha : the angle determined by the direction + of the force applied by the user +- f_guide : the desired guiding force to be felt by the user*/ +double +PantographModel::get_panto_force(Eigen::Vector PU, double f_guide, double alpha) { - // Get all the robot joint angles IKM + // Get all the robot joint angles IKM Eigen::Vector jnt_ext_pos; - jnt_ext_pos = ik_system(p); + jnt_ext_pos = ik_system(PU); double theta = jnt_ext_pos[5]; // Azimuth angle - double phi = jnt_ext_pos[6]; // Elevation angle + double phi = jnt_ext_pos[6]; // Elevation angle - // get insertion point height - double H = PI_z; // = 0.09 based on insertion point coords + // get insertion point height + double H = PI_z; // = 0.09 based on insertion point coords // Calculate length of the needle segment between P3 and PI - double l_in = H/std::sin(phi); + double l_in = H / std::sin(phi); // Calculate length of the needle segment between PI and PU - double l_out = l_needle_ - l_in; + double l_out = l_needle_ - l_in; // As a first approximation, L_out is constant - /*Calculate the perpendicular component of the force that the system + /*Calculate the perpendicular component of the force that the system needs to apply to make the user feel a force at the point PU */ - double f_perp = f_guide*(l_out/l_in); + double f_perp = f_guide * (l_out / l_in); /* Calculate the force that the pantograph needs to generate beta is the angle between the force vector Fmech and the needle vector */ - double cos_beta = std::cos(phi)*std::cos(theta-alpha); - double f_mech = f_perp*(l_in/l_out); + double cos_beta = std::cos(phi) * std::cos(theta - alpha); + double f_mech = f_perp / std::sqrt(1 - std::pow(cos_beta, 2)); + + // Calculate the folt felt by the user at point PU + double f_user = f_perp * (l_in / l_out); return f_mech; -} +} } // namespace pantograph_library From f9fab7a1316ae0a06985aa9c409d0a19e1331c37 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Fri, 5 Apr 2024 11:32:57 +0200 Subject: [PATCH 05/27] added "tests" folder and updated CMake config --- pantograph_library/CMakeLists.txt | 37 ++++++ pantograph_library/tests/test_panto_model.cc | 129 +++++++++++++++++++ 2 files changed, 166 insertions(+) create mode 100644 pantograph_library/tests/test_panto_model.cc diff --git a/pantograph_library/CMakeLists.txt b/pantograph_library/CMakeLists.txt index a9389d4..e9b2ced 100644 --- a/pantograph_library/CMakeLists.txt +++ b/pantograph_library/CMakeLists.txt @@ -71,3 +71,40 @@ ament_export_targets( ) ament_export_dependencies(Eigen3) ament_package() + +#----------------------------------------------------- +# Config for using GoogleTest +#----------------------------------------------------- + +include(FetchContent) +fetchcontent_declare( + googletest + URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip +) + +# For Windows: Prevent overriding the parent project's compiler/linker settings +set(gtest_force_shared_crt ON CACHE BOOL "" FORCE) +fetchcontent_makeavailable(googletest) + +enable_testing() + +add_executable( + test_panto_model + tests/test_panto_model.cc +) +ament_target_dependencies(test_panto_model PUBLIC Eigen3) +target_link_libraries( + test_panto_model + GTest::gtest_main + pantograph_library +) + +include(GoogleTest) +gtest_discover_tests(test_panto_model) + +# Add include folder +# Where Google Test's .h files can be found. +set(gtest_build_include_dirs + "${PROJECT_SOURCE_DIR}/include" + "${PROJECT_SOURCE_DIR}") +include_directories(${gtest_build_include_dirs}) diff --git a/pantograph_library/tests/test_panto_model.cc b/pantograph_library/tests/test_panto_model.cc new file mode 100644 index 0000000..3d7fee4 --- /dev/null +++ b/pantograph_library/tests/test_panto_model.cc @@ -0,0 +1,129 @@ +/* To run tests go to pantograph_library dir then run: + cmake -S . -B build + cmake --build build + cd build && ctest */ + +#include +#include "pantograph_library/pantograph_model.hpp" + +/* Test of the FKM of the pantograph by comparing the coords of P3 calculated +by the C++ model and the same coords calculate by the MATLAB model*/ + +TEST(TestModel, TestPantoFKM) { + pantograph_library::PantographModel panto_model_{} ; + Eigen::Vector q; + Eigen::Vector P3_cpp, P3_matlab; + + double error = 1e-10; + + // q values according to MATLAB model + q[0] = 0.5; + q[1] = pantograph_library::PI_CST - 0.5; // 2.64159265358979 + + // Coords of P3 according to MATLAB model + P3_matlab[0] = -0.0425000000000000; + P3_matlab[1] = 0.149223285959824; + + // Coords of P3 according to C++ model + P3_cpp = panto_model_.fk(q); + + /*Expect values to be equal (up to a 1e-10 error) between + the C++ and the MATLAB model */ + + EXPECT_NEAR(P3_cpp[0],P3_matlab[0],error); + EXPECT_NEAR(P3_cpp[1],P3_matlab[1],error); +} + +/* Test of the IKM of the pantograph by comparing the coords of the joints calculated +by the C++ model and the same coords calculate by the MATLAB model*/ + +TEST(TestModel, TestPantoIKM) { + pantograph_library::PantographModel panto_model_ ; + Eigen::Vector q_cpp, q_matlab, P3; + Eigen::Vector joints; + + double error = 1e-10; + + // P3 coords according to MATLAB model + P3[0] = -0.0425000000000000; + P3[1] = 0.149223285959824; + + // q values according to MATLAB model + q_matlab[0] = 0.500000000000000; + q_matlab[1] = 2.64159265358979; + + // q values according to C++ model + joints = panto_model_.ik(P3); + q_cpp[0] = joints[0]; + q_cpp[1] = joints[4]; + + /* Expect values to be equal (up to a 1e-10 error) between + the C++ and the MATLAB model */ + // Actuated joints + EXPECT_NEAR(q_cpp[0],q_matlab[0], error); + EXPECT_NEAR(q_cpp[1],q_matlab[1], error); + + // Other joints for visual render +} + + + +/* Test of the FKM of the complete system by comparing the coords of PU calculated +by the C++ model and the same coords calculate by the MATLAB model*/ + +TEST(TestModel, TestSystemFKM) { + pantograph_library::PantographModel panto_model_{} ; + Eigen::Vector q; + Eigen::Vector PU_cpp, PU_matlab; + + double error = 1e-10; + + // q values according to MATLAB model + q[0] = 0.5; + q[1] = 2.64159265358979; pantograph_library::PI_CST - 0.5; + + // Coords of PU according to MATLAB model + PU_matlab[0] = 0.0942524755499338; + PU_matlab[1] = 0.167462388425657; + PU_matlab[2] = 0.144796738817577; + + // Coords of PU according to C++ model + PU_cpp = panto_model_.fk_system(q); + + /*Expect values to be equal (up to a 1e-10 error) between + the C++ and the MATLAB model */ + EXPECT_NEAR(PU_cpp[0],PU_matlab[0],error); + EXPECT_NEAR(PU_cpp[1],PU_matlab[1],error); + EXPECT_NEAR(PU_cpp[2],PU_matlab[2],error); +} + +/* Test of the IKM of the system by comparing the values of q calculated +by the C++ model and the same coords calculate by the MATLAB model*/ + +TEST(TestModel, TestIKM) { + pantograph_library::PantographModel panto_model_ ; + Eigen::Vector q_cpp, q_matlab; + Eigen::Vector joints; + Eigen::Vector3d PU_matlab; + + double error = 1e-10; + + // PU coords according to MATLAB model + PU_matlab[0] = 0.0942524755499338; + PU_matlab[1] = 0.167462388425657; + PU_matlab[2] = 0.144796738817577; + + // q values according to MATLAB model + q_matlab[0] = 0.500000000000000; + q_matlab[1] = 2.64159265358979; // PI_CST - 0.5 + + // q values according to C++ model + joints = panto_model_.ik_system(PU_matlab); + q_cpp[0] = joints[0]; + q_cpp[1] = joints[4]; + + /* Expect values to be equal (up to a 1e-10 error) between + the C++ and the MATLAB model */ + EXPECT_NEAR(q_cpp[0],q_matlab[0], error); + EXPECT_NEAR(q_cpp[1],q_matlab[1], error); +} From 88ff0282304413967547bdd2aa1485ba094bf5f9 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Fri, 5 Apr 2024 11:50:54 +0200 Subject: [PATCH 06/27] Updated test description --- pantograph_library/tests/test_panto_model.cc | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/pantograph_library/tests/test_panto_model.cc b/pantograph_library/tests/test_panto_model.cc index 3d7fee4..fd6ee5b 100644 --- a/pantograph_library/tests/test_panto_model.cc +++ b/pantograph_library/tests/test_panto_model.cc @@ -1,7 +1,6 @@ -/* To run tests go to pantograph_library dir then run: - cmake -S . -B build - cmake --build build - cd build && ctest */ +/* To run tests go to ws_pantograph dir then + colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install + cd build/pantograph_library/ && ctest*/ #include #include "pantograph_library/pantograph_model.hpp" From b34028850be330a2e2856cf40c2bb84d7186574a Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Fri, 5 Apr 2024 14:36:47 +0200 Subject: [PATCH 07/27] updated test description --- pantograph_library/tests/test_panto_model.cc | 92 ++++++++++---------- 1 file changed, 46 insertions(+), 46 deletions(-) diff --git a/pantograph_library/tests/test_panto_model.cc b/pantograph_library/tests/test_panto_model.cc index fd6ee5b..278c338 100644 --- a/pantograph_library/tests/test_panto_model.cc +++ b/pantograph_library/tests/test_panto_model.cc @@ -1,24 +1,25 @@ -/* To run tests go to ws_pantograph dir then - colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install - cd build/pantograph_library/ && ctest*/ +// Copyright 2024, ICube Laboratory, University of Strasbourg +// To run tests go to ws_pantograph dir then : +// colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install +// cd build/pantograph_library/ && ctest #include #include "pantograph_library/pantograph_model.hpp" -/* Test of the FKM of the pantograph by comparing the coords of P3 calculated +/* Test of the FKM of the pantograph by comparing the coords of P3 calculated by the C++ model and the same coords calculate by the MATLAB model*/ -TEST(TestModel, TestPantoFKM) { - pantograph_library::PantographModel panto_model_{} ; - Eigen::Vector q; - Eigen::Vector P3_cpp, P3_matlab; +TEST(TestModel, TestPantoFKM) { + pantograph_library::PantographModel panto_model_ {}; + Eigen::Vector < double, 2 > q; + Eigen::Vector < double, 2 > P3_cpp, P3_matlab; double error = 1e-10; // q values according to MATLAB model - q[0] = 0.5; - q[1] = pantograph_library::PI_CST - 0.5; // 2.64159265358979 - + q[0] = 0.5; + q[1] = pantograph_library::PI_CST - 0.5; // 2.64159265358979 + // Coords of P3 according to MATLAB model P3_matlab[0] = -0.0425000000000000; P3_matlab[1] = 0.149223285959824; @@ -26,20 +27,20 @@ TEST(TestModel, TestPantoFKM) { // Coords of P3 according to C++ model P3_cpp = panto_model_.fk(q); - /*Expect values to be equal (up to a 1e-10 error) between + /*Expect values to be equal (up to a 1e-10 error) between the C++ and the MATLAB model */ - EXPECT_NEAR(P3_cpp[0],P3_matlab[0],error); - EXPECT_NEAR(P3_cpp[1],P3_matlab[1],error); + EXPECT_NEAR(P3_cpp[0], P3_matlab[0], error); + EXPECT_NEAR(P3_cpp[1], P3_matlab[1], error); } -/* Test of the IKM of the pantograph by comparing the coords of the joints calculated +/* Test of the IKM of the pantograph by comparing the coords of the joints calculated by the C++ model and the same coords calculate by the MATLAB model*/ -TEST(TestModel, TestPantoIKM) { - pantograph_library::PantographModel panto_model_ ; - Eigen::Vector q_cpp, q_matlab, P3; - Eigen::Vector joints; +TEST(TestModel, TestPantoIKM) { + pantograph_library::PantographModel panto_model_; + Eigen::Vector < double, 2 > q_cpp, q_matlab, P3; + Eigen::Vector < double, 5 > joints; double error = 1e-10; @@ -50,36 +51,35 @@ TEST(TestModel, TestPantoIKM) { // q values according to MATLAB model q_matlab[0] = 0.500000000000000; q_matlab[1] = 2.64159265358979; - + // q values according to C++ model joints = panto_model_.ik(P3); q_cpp[0] = joints[0]; q_cpp[1] = joints[4]; - /* Expect values to be equal (up to a 1e-10 error) between + /* Expect values to be equal (up to a 1e-10 error) between the C++ and the MATLAB model */ // Actuated joints - EXPECT_NEAR(q_cpp[0],q_matlab[0], error); - EXPECT_NEAR(q_cpp[1],q_matlab[1], error); + EXPECT_NEAR(q_cpp[0], q_matlab[0], error); + EXPECT_NEAR(q_cpp[1], q_matlab[1], error); // Other joints for visual render } - -/* Test of the FKM of the complete system by comparing the coords of PU calculated +/* Test of the FKM of the complete system by comparing the coords of PU calculated by the C++ model and the same coords calculate by the MATLAB model*/ -TEST(TestModel, TestSystemFKM) { - pantograph_library::PantographModel panto_model_{} ; - Eigen::Vector q; - Eigen::Vector PU_cpp, PU_matlab; +TEST(TestModel, TestSystemFKM) { + pantograph_library::PantographModel panto_model_ {}; + Eigen::Vector < double, 2 > q; + Eigen::Vector < double, 3 > PU_cpp, PU_matlab; double error = 1e-10; // q values according to MATLAB model - q[0] = 0.5; - q[1] = 2.64159265358979; pantograph_library::PI_CST - 0.5; + q[0] = 0.5; + q[1] = 2.64159265358979; pantograph_library::PI_CST - 0.5; // Coords of PU according to MATLAB model PU_matlab[0] = 0.0942524755499338; @@ -87,22 +87,22 @@ TEST(TestModel, TestSystemFKM) { PU_matlab[2] = 0.144796738817577; // Coords of PU according to C++ model - PU_cpp = panto_model_.fk_system(q); - - /*Expect values to be equal (up to a 1e-10 error) between + PU_cpp = panto_model_.fk_system(q); + + /*Expect values to be equal (up to a 1e-10 error) between the C++ and the MATLAB model */ - EXPECT_NEAR(PU_cpp[0],PU_matlab[0],error); - EXPECT_NEAR(PU_cpp[1],PU_matlab[1],error); - EXPECT_NEAR(PU_cpp[2],PU_matlab[2],error); + EXPECT_NEAR(PU_cpp[0], PU_matlab[0], error); + EXPECT_NEAR(PU_cpp[1], PU_matlab[1], error); + EXPECT_NEAR(PU_cpp[2], PU_matlab[2], error); } -/* Test of the IKM of the system by comparing the values of q calculated +/* Test of the IKM of the system by comparing the values of q calculated by the C++ model and the same coords calculate by the MATLAB model*/ TEST(TestModel, TestIKM) { - pantograph_library::PantographModel panto_model_ ; - Eigen::Vector q_cpp, q_matlab; - Eigen::Vector joints; + pantograph_library::PantographModel panto_model_; + Eigen::Vector < double, 2 > q_cpp, q_matlab; + Eigen::Vector < double, 7 > joints; Eigen::Vector3d PU_matlab; double error = 1e-10; @@ -114,15 +114,15 @@ TEST(TestModel, TestIKM) { // q values according to MATLAB model q_matlab[0] = 0.500000000000000; - q_matlab[1] = 2.64159265358979; // PI_CST - 0.5 - + q_matlab[1] = 2.64159265358979; // PI_CST - 0.5 + // q values according to C++ model joints = panto_model_.ik_system(PU_matlab); q_cpp[0] = joints[0]; q_cpp[1] = joints[4]; - /* Expect values to be equal (up to a 1e-10 error) between + /* Expect values to be equal (up to a 1e-10 error) between the C++ and the MATLAB model */ - EXPECT_NEAR(q_cpp[0],q_matlab[0], error); - EXPECT_NEAR(q_cpp[1],q_matlab[1], error); + EXPECT_NEAR(q_cpp[0], q_matlab[0], error); + EXPECT_NEAR(q_cpp[1], q_matlab[1], error); } From 97aaf94b18f179c1923298fe4dac583e6fe12ecc Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Fri, 5 Apr 2024 14:38:05 +0200 Subject: [PATCH 08/27] added python model of the complete system (pantograph + needle) and funtion for force estimation --- .../pantograph_library/_model.py | 100 ++++++++++++++++++ 1 file changed, 100 insertions(+) diff --git a/pantograph_library/pantograph_library/_model.py b/pantograph_library/pantograph_library/_model.py index 5a8277b..b137834 100644 --- a/pantograph_library/pantograph_library/_model.py +++ b/pantograph_library/pantograph_library/_model.py @@ -22,6 +22,12 @@ class PantographModel: __a1 = 0.100 __a2 = 0.165 __a5 = 0.085 + __l_needle = 0.2 + + # Insertion point coords according to CAD + PI_x = 0.0425 + PI_y = 0.16056 + PI_z = 0.09 def __init__(self): self.__a3 = self.__a2 @@ -117,7 +123,97 @@ def jacobian(self, q): return J + # Models of the complete system (pantograph + needle) + @staticmethod + def fk_system(self, q): + # FKM of the complete system : calculates cartesian position + # of interaction point PU according to joint space coordinates q + PI = np.array([self.PI_x, self.PI_y, self.PI_z]) + + # Get coords of P3 in base frame according to panto FKM + P3_2D = self.fk(q) + + # Convert P3 coords in plane to coords in 3D + P3 = np.append(P3_2D, 0) + + # Convert coords of PI in base frame to P3 frame + P3I = PI - P3 + + # Get azimuth (theta) and elevation (phi) angles (in radians) of vector P3PI + theta = np.arctan2(P3I[1], P3I[0]) + phi = np.arctan2(P3I[2], np.sqrt(P3I[0]**2 + P3I[1]**2)) + Lin = np.sqrt(P3I[0]**2 + P3I[1]**2 + P3I[2]**2) + + # Calculate translation from PI to PU + Lout = self.l_needle - Lin + PIU = Lout * np.array([np.cos(phi)*np.cos(theta), np.cos(phi)*np.sin(theta), np.sin(phi)]) + + # Convert coords of PU in PI frame to base frame + PU = PI + PIU + + return PU + + @staticmethod + def ik_system(self, PU): + # Inverse kinematics model of the complete system: + # computes the angles of all the joints according + # to the cartesian position of interaction point PU + PI = np.array([self.PI_x, self.PI_y, self.PI_z]) + + # Transformation of PU coords in base frame to coords in PI frame + PIU = PI - PU + + # Get azimuth (theta) and elevation (phi) angles (in radians) of vector P3PI + theta = np.arctan2(PIU[1], PIU[0]) + phi = np.arctan2(PIU[2], np.sqrt(PIU[0]**2 + PIU[1]**2)) + Lout = np.sqrt(PIU[0]**2 + PIU[1]**2 + PIU[2]**2) + + # Get length of the needle segment betzeen PI and P3 + Lin = self.l_needle - Lout + + PI3 = Lin * np.array([np.cos(phi)*np.cos(theta), np.cos(phi)*np.sin(theta), np.sin(phi)]) + + # Convert coords of PU in PI frame to base frame + P3 = PI + PI3 + + # Convert P3 coords in 3D to coords in plane + P3_2D = np.array([P3[0], P3[1]]) + + # Get angle of the pantograph joints using IKM + jnt_pos = self.ik(self, P3_2D) + + # Return pantograph joint angles + needle orientation angles + jnt_ext_pos = np.append(jnt_pos, [theta, phi]) + return jnt_ext_pos + + @staticmethod + def get_panto_force(self, PU, f_guide, alpha): + # Get all the robot joint angles + jnt_ext_pos = self.ik_system(PU) + theta = jnt_ext_pos[5] + phi = jnt_ext_pos[6] + + # Get insertion point height + H = self.PI_z + + # Calculate length of the needle segment between P3 and PI + Lin = H / np.sin(phi) + + # Calculate the length of the needle segment between PI and PU + Lout = self.l_needle - Lin + + # Calculate the perpendicular component of the force that the system + # needs to apply to create the force desired at point PU + f_perp = f_guide * (Lout/Lin) + + # Calculate the force that the pantograph needs to generate + # beta is the angle between the force vector f_mech and the needle + cos_beta = np.cos(phi) * np.cos(theta - alpha) + f_mech = f_perp / np.sqrt(1 - cos_beta ** 2) + + return f_mech # Read-only properties + @property def a1(self): return self.__a1 @@ -137,3 +233,7 @@ def a4(self): @property def a5(self): return self.__a5 + + @property + def l_needle(self): + return self.__l_needle From a62f50f1d39db5f33634fa34fe19d0b95e453cc6 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Mon, 8 Apr 2024 10:59:54 +0200 Subject: [PATCH 09/27] modified to include universal joint at end effector --- .../src/pantograph_mimick_controller.cpp | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp index 199df7e..df41121 100644 --- a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp +++ b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp @@ -66,7 +66,9 @@ controller_interface::CallbackReturn PantographMimickController::on_configure( params_.prefix + "panto_a2", params_.prefix + "panto_a3", params_.prefix + "panto_a4", - params_.prefix + "panto_a5"}; + params_.prefix + "panto_a5", + params_.prefix + "tool_pitch_joint", + params_.prefix + "tool_roll_joint"}; RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -84,6 +86,10 @@ PantographMimickController::command_interface_configuration() const pantograph_joint_names_[2] + "/" + HW_IF_POSITION); command_interfaces_config.names.push_back( pantograph_joint_names_[3] + "/" + HW_IF_POSITION); + command_interfaces_config.names.push_back( + pantograph_joint_names_[5] + "/" + HW_IF_POSITION); + command_interfaces_config.names.push_back( + pantograph_joint_names_[6] + "/" + HW_IF_POSITION); return command_interfaces_config; } @@ -162,18 +168,30 @@ controller_interface::return_type PantographMimickController::update( q << pos_a1, pos_a5; // Calculate the pantograph (passive) joint positions - Eigen::Vector full_joint_state; - full_joint_state = pantograph_model_.populate_all_joint_positions(q); + // Eigen::Vector full_joint_state; + // full_joint_state = pantograph_model_.populate_all_joint_positions(q); + + // Calculate the passive joints of the complete system + Eigen::Vector full_joint_state; + full_joint_state = pantograph_model_.populate_all_joint_positions_full_system(q); double pos_a2 = full_joint_state[1]; double pos_a3 = full_joint_state[2]; double pos_a4 = full_joint_state[3]; + double pos_pitch = full_joint_state[5]; + double pos_roll = full_joint_state[6]; + // write mimics to HW command_interfaces_[0].set_value(pos_a2); command_interfaces_[1].set_value(pos_a3); command_interfaces_[2].set_value(pos_a4); + // Write pitch and orientation angles to end effector universal joint + command_interfaces_[5].set_value(pos_pitch); + command_interfaces_[6].set_value(pos_roll); + + return controller_interface::return_type::OK; } From e83bb3064660b71e4d19d2fb31b2cacdb2822f74 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Mon, 8 Apr 2024 11:00:42 +0200 Subject: [PATCH 10/27] added tool_orientation_controller for testing of universal joint at end effector --- .../launch/pantograph.launch.py | 19 +++++++++----- .../config/ros2_controllers.yaml | 14 +++++----- .../pantograph.r2c_hardware.xacro | 26 +++++++++---------- 3 files changed, 33 insertions(+), 26 deletions(-) diff --git a/pantograph_bringup/launch/pantograph.launch.py b/pantograph_bringup/launch/pantograph.launch.py index 00439e5..6252f13 100644 --- a/pantograph_bringup/launch/pantograph.launch.py +++ b/pantograph_bringup/launch/pantograph.launch.py @@ -90,11 +90,11 @@ def generate_launch_description(): executable='spawner', arguments=['joint_state_broadcaster'], ) - pantograph_mimick_controller_spawner = Node( - package='controller_manager', - executable='spawner', - arguments=['pantograph_mimick_controller'], - ) + # pantograph_mimick_controller_spawner = Node( + # package='controller_manager', + # executable='spawner', + # arguments=['pantograph_mimick_controller'], + # ) effort_controller_spawner = Node( package='controller_manager', @@ -102,13 +102,20 @@ def generate_launch_description(): arguments=['forward_effort_controller'], ) + tool_orientation_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['tool_orientation_controller'], + ) + nodes = [ control_node, rviz_node, robot_state_pub_node, joint_state_broadcaster_spawner, - pantograph_mimick_controller_spawner, + # pantograph_mimick_controller_spawner, effort_controller_spawner, + tool_orientation_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) diff --git a/pantograph_description/config/ros2_controllers.yaml b/pantograph_description/config/ros2_controllers.yaml index b4672e6..8586ade 100644 --- a/pantograph_description/config/ros2_controllers.yaml +++ b/pantograph_description/config/ros2_controllers.yaml @@ -11,8 +11,8 @@ controller_manager: type: pantograph_mimick_controller/PantographMimickController forward_effort_controller: type: effort_controllers/JointGroupEffortController - # tool_orientation_controller: - # type: position_controllers/JointGroupPositionController + tool_orientation_controller: + type: position_controllers/JointGroupPositionController @@ -36,8 +36,8 @@ forward_effort_controller: - panto_a1 - panto_a5 -# tool_orientation_controller: -# ros_parameters: -# joints: -# - tool_roll -# - tool_pitch +tool_orientation_controller: + ros__parameters: + joints: + - tool_roll_joint + - tool_pitch_joint diff --git a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro index d91e26b..ec840dd 100644 --- a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro +++ b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro @@ -142,25 +142,25 @@ - + - + + ${0} + -1.57 + 1.57 + + + + + + ${0} - -3 - 3 + -1.57 + 1.57 - - - - ${0} - -3 - 3 - - - From 1f173cf7d135b6ec1b31928e86d80b8feadbbd11 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Mon, 8 Apr 2024 15:48:14 +0200 Subject: [PATCH 11/27] Added jacobian definition according to T.CESARE report --- pantograph_library/src/pantograph_model.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/pantograph_library/src/pantograph_model.cpp b/pantograph_library/src/pantograph_model.cpp index ef13935..83d4c08 100644 --- a/pantograph_library/src/pantograph_model.cpp +++ b/pantograph_library/src/pantograph_model.cpp @@ -149,8 +149,7 @@ PantographModel::jacobian(Eigen::Vector q, Eigen::Vector q J << J11, J21, J21, J22; - - return Eigen::Matrix::Zero(); + return J; } Eigen::Vector From 6a24cc82ead4c2c3f4430c475d38c0c61643f265 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Mon, 8 Apr 2024 16:12:49 +0200 Subject: [PATCH 12/27] modified controller to include universal joint at end effector for simulation --- .../src/pantograph_mimick_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp index df41121..734fd5e 100644 --- a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp +++ b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp @@ -188,8 +188,8 @@ controller_interface::return_type PantographMimickController::update( command_interfaces_[2].set_value(pos_a4); // Write pitch and orientation angles to end effector universal joint - command_interfaces_[5].set_value(pos_pitch); - command_interfaces_[6].set_value(pos_roll); + command_interfaces_[3].set_value(pos_pitch); + command_interfaces_[4].set_value(pos_roll); return controller_interface::return_type::OK; From 5aac15996e11175c07657d172dd99d28a5216cb5 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Tue, 9 Apr 2024 10:01:06 +0200 Subject: [PATCH 13/27] changed joint names for clarification --- .../config/ros2_controllers.yaml | 13 ++++-- .../pantograph.r2c_hardware.xacro | 35 +++++++++++---- .../urdf/pantograph.urdf.xacro | 44 ++++++++++--------- 3 files changed, 60 insertions(+), 32 deletions(-) diff --git a/pantograph_description/config/ros2_controllers.yaml b/pantograph_description/config/ros2_controllers.yaml index 8586ade..1eb48bd 100644 --- a/pantograph_description/config/ros2_controllers.yaml +++ b/pantograph_description/config/ros2_controllers.yaml @@ -13,7 +13,8 @@ controller_manager: type: effort_controllers/JointGroupEffortController tool_orientation_controller: type: position_controllers/JointGroupPositionController - + panto_position_controller: + type: position_controllers/JointGroupPositionController haptic_trajectory_controller: @@ -39,5 +40,11 @@ forward_effort_controller: tool_orientation_controller: ros__parameters: joints: - - tool_roll_joint - - tool_pitch_joint + - tool_theta_joint + - tool_phi_joint + +panto_position_controller: + ros__parameters: + joints: + - panto_a1 + - panto_a5 diff --git a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro index ec840dd..b8b303b 100644 --- a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro +++ b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro @@ -143,24 +143,43 @@ - + ${0} - -1.57 - 1.57 + -3 + 3 - + - ${0} - -1.57 - 1.57 - + ${0} + -3 + 3 + + + + diff --git a/pantograph_description/urdf/pantograph.urdf.xacro b/pantograph_description/urdf/pantograph.urdf.xacro index c804730..611b106 100644 --- a/pantograph_description/urdf/pantograph.urdf.xacro +++ b/pantograph_description/urdf/pantograph.urdf.xacro @@ -187,34 +187,36 @@ - + - + - + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - + From 5ad33ea87ce3d1825d56782783c92e9beaf78ada Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Tue, 9 Apr 2024 10:23:54 +0200 Subject: [PATCH 14/27] changed joint names for clarification --- .../src/pantograph_mimick_controller.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp index 734fd5e..1106534 100644 --- a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp +++ b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp @@ -67,8 +67,8 @@ controller_interface::CallbackReturn PantographMimickController::on_configure( params_.prefix + "panto_a3", params_.prefix + "panto_a4", params_.prefix + "panto_a5", - params_.prefix + "tool_pitch_joint", - params_.prefix + "tool_roll_joint"}; + params_.prefix + "tool_theta_joint", + params_.prefix + "tool_phi_joint"}; RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -179,8 +179,8 @@ controller_interface::return_type PantographMimickController::update( double pos_a3 = full_joint_state[2]; double pos_a4 = full_joint_state[3]; - double pos_pitch = full_joint_state[5]; - double pos_roll = full_joint_state[6]; + double pos_theta = full_joint_state[5]; + double pos_phi = full_joint_state[6]; // check for correct angle orientation // write mimics to HW command_interfaces_[0].set_value(pos_a2); @@ -188,8 +188,8 @@ controller_interface::return_type PantographMimickController::update( command_interfaces_[2].set_value(pos_a4); // Write pitch and orientation angles to end effector universal joint - command_interfaces_[3].set_value(pos_pitch); - command_interfaces_[4].set_value(pos_roll); + command_interfaces_[3].set_value(pos_theta); + command_interfaces_[4].set_value(pos_phi); return controller_interface::return_type::OK; From 477cfd61a7efbb6076d8b2a6c89574bd988f40f7 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Tue, 9 Apr 2024 10:24:42 +0200 Subject: [PATCH 15/27] added panto_position_controller for testing --- pantograph_description/config/ros2_controllers.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pantograph_description/config/ros2_controllers.yaml b/pantograph_description/config/ros2_controllers.yaml index 1eb48bd..0344bca 100644 --- a/pantograph_description/config/ros2_controllers.yaml +++ b/pantograph_description/config/ros2_controllers.yaml @@ -14,7 +14,7 @@ controller_manager: tool_orientation_controller: type: position_controllers/JointGroupPositionController panto_position_controller: - type: position_controllers/JointGroupPositionController + type: position_controllers/JointGroupPositionController haptic_trajectory_controller: From 2e8e5bf7cc540b6784fd3e04fe150f35653b003f Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Tue, 9 Apr 2024 11:40:06 +0200 Subject: [PATCH 16/27] corrected syntaxis errors --- pantograph_library/tests/test_panto_model.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pantograph_library/tests/test_panto_model.cc b/pantograph_library/tests/test_panto_model.cc index 278c338..241e022 100644 --- a/pantograph_library/tests/test_panto_model.cc +++ b/pantograph_library/tests/test_panto_model.cc @@ -79,7 +79,7 @@ TEST(TestModel, TestSystemFKM) { // q values according to MATLAB model q[0] = 0.5; - q[1] = 2.64159265358979; pantograph_library::PI_CST - 0.5; + q[1] = 2.64159265358979; // pantograph_library::PI_CST - 0.5; // Coords of PU according to MATLAB model PU_matlab[0] = 0.0942524755499338; From 81b421aa5955124b682d801febfc5ab530b35537 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Tue, 9 Apr 2024 11:40:56 +0200 Subject: [PATCH 17/27] changed type of tool joint from "revolute" to "continuous" --- pantograph_description/urdf/pantograph.urdf.xacro | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/pantograph_description/urdf/pantograph.urdf.xacro b/pantograph_description/urdf/pantograph.urdf.xacro index 611b106..4bbffa2 100644 --- a/pantograph_description/urdf/pantograph.urdf.xacro +++ b/pantograph_description/urdf/pantograph.urdf.xacro @@ -189,11 +189,10 @@ - + - @@ -208,11 +207,10 @@ - + - From 73ad1048fdf9266518741d8decc7e2042e2b6b7b Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Tue, 9 Apr 2024 13:23:38 +0200 Subject: [PATCH 18/27] added prismatic joint for modelling of needle insertion --- .../pantograph.r2c_hardware.xacro | 21 +++++++------------ .../urdf/pantograph.urdf.xacro | 10 ++++++++- 2 files changed, 16 insertions(+), 15 deletions(-) diff --git a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro index b8b303b..e5d8005 100644 --- a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro +++ b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro @@ -161,24 +161,17 @@ - - + ${0} - -1.57 - 1.57 + -0.5 + 0.5 - --> + + + diff --git a/pantograph_description/urdf/pantograph.urdf.xacro b/pantograph_description/urdf/pantograph.urdf.xacro index 4bbffa2..082caff 100644 --- a/pantograph_description/urdf/pantograph.urdf.xacro +++ b/pantograph_description/urdf/pantograph.urdf.xacro @@ -214,7 +214,15 @@ - + + + + + + + + + From 96718ae4cb89b0c1e80cc6dde26bbbed341aff18 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Tue, 9 Apr 2024 13:24:09 +0200 Subject: [PATCH 19/27] added prismatic joint for needle insertion modelling --- .../pantograph_library/pantograph_model.hpp | 4 ++-- pantograph_library/src/pantograph_model.cpp | 16 +++++++++++----- pantograph_library/tests/test_panto_model.cc | 2 +- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/pantograph_library/include/pantograph_library/pantograph_model.hpp b/pantograph_library/include/pantograph_library/pantograph_model.hpp index 670e808..329eded 100644 --- a/pantograph_library/include/pantograph_library/pantograph_model.hpp +++ b/pantograph_library/include/pantograph_library/pantograph_model.hpp @@ -53,13 +53,13 @@ class PantographModel Eigen::Vector fk_system(Eigen::Vector q); /// Inverse kinematics of the complete system p = fk_needle(P3) = fk_needle(fk([q1,q2])) - Eigen::Vector ik_system(Eigen::Vector p); + Eigen::Vector ik_system(Eigen::Vector p); /// Inverse kinematics of the pantograph according to equations of T.CESARE report Eigen::Vector ik_panto_optimized(Eigen::Vector p); /// Populate all joints position of the full system (pantograph + needle) - Eigen::Vector populate_all_joint_positions_full_system(Eigen::Vector q); + Eigen::Vector populate_all_joint_positions_full_system(Eigen::Vector q); /// Calculate the force that the pantograph needs to apply to create a force felt by the user double get_panto_force(Eigen::Vector p, double f_guide, double alpha); diff --git a/pantograph_library/src/pantograph_model.cpp b/pantograph_library/src/pantograph_model.cpp index 83d4c08..9a20a54 100644 --- a/pantograph_library/src/pantograph_model.cpp +++ b/pantograph_library/src/pantograph_model.cpp @@ -250,6 +250,9 @@ PantographModel::fk_system(Eigen::Vector q) double phi = std::atan2(P3I[2], std::sqrt(std::pow(P3I[0], 2) + std::pow(P3I[1], 2))); double Lin = std::sqrt(std::pow(P3I[0], 2) + std::pow(P3I[1], 2) + std::pow(P3I[2], 2)); + // Check for correct definition of phi angle + phi = (PI_CST / 2) - phi; + // Calculate translation from PI to PU double Lout = l_needle_ - Lin; PIU[0] = Lout * std::cos(phi) * std::cos(theta); // Coords of PU in PI frame @@ -261,7 +264,7 @@ PantographModel::fk_system(Eigen::Vector q) return PU; } -Eigen::Vector +Eigen::Vector PantographModel::ik_system(Eigen::Vector PU) { /* Inverse kinematics model of the complete system: @@ -283,6 +286,9 @@ PantographModel::ik_system(Eigen::Vector PU) double phi = std::atan2(PIU[2], std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2))); double Lout = std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2) + std::pow(PIU[2], 2)); + // Check for correct definition of phi angle + phi = (PI_CST / 2) - phi; + // Get length of the needle segment between PI and P3 double Lin = l_needle_ - Lout; @@ -307,14 +313,14 @@ PantographModel::ik_system(Eigen::Vector PU) // jnt_pos = ik_panto_optimized(P3_2D); // Return pantograph joints angles + needle orientation angles - Eigen::Vector jnt_ext_pos; - jnt_ext_pos << jnt_pos, theta, phi; + Eigen::Vector jnt_ext_pos; + jnt_ext_pos << jnt_pos, theta, phi, Lout; return jnt_ext_pos; } // Calculate joint positions of the complete system for visualization -Eigen::Vector +Eigen::Vector PantographModel::populate_all_joint_positions_full_system(Eigen::Vector q) { auto p = fk_system(q); @@ -330,7 +336,7 @@ double PantographModel::get_panto_force(Eigen::Vector PU, double f_guide, double alpha) { // Get all the robot joint angles IKM - Eigen::Vector jnt_ext_pos; + Eigen::Vector jnt_ext_pos; jnt_ext_pos = ik_system(PU); double theta = jnt_ext_pos[5]; // Azimuth angle double phi = jnt_ext_pos[6]; // Elevation angle diff --git a/pantograph_library/tests/test_panto_model.cc b/pantograph_library/tests/test_panto_model.cc index 241e022..9be253d 100644 --- a/pantograph_library/tests/test_panto_model.cc +++ b/pantograph_library/tests/test_panto_model.cc @@ -102,7 +102,7 @@ by the C++ model and the same coords calculate by the MATLAB model*/ TEST(TestModel, TestIKM) { pantograph_library::PantographModel panto_model_; Eigen::Vector < double, 2 > q_cpp, q_matlab; - Eigen::Vector < double, 7 > joints; + Eigen::Vector < double, 8 > joints; Eigen::Vector3d PU_matlab; double error = 1e-10; From 65b7ac7082d552d9cd32dcc7d3b1b8651d6e942c Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 10 Apr 2024 10:47:48 +0200 Subject: [PATCH 20/27] error correction on pantograph model --- .../pantograph_library/pantograph_model.hpp | 7 +- pantograph_library/src/pantograph_model.cpp | 134 +++++++++++------- pantograph_library/tests/test_panto_model.cc | 20 +-- 3 files changed, 99 insertions(+), 62 deletions(-) diff --git a/pantograph_library/include/pantograph_library/pantograph_model.hpp b/pantograph_library/include/pantograph_library/pantograph_model.hpp index 329eded..ec785be 100644 --- a/pantograph_library/include/pantograph_library/pantograph_model.hpp +++ b/pantograph_library/include/pantograph_library/pantograph_model.hpp @@ -55,8 +55,11 @@ class PantographModel /// Inverse kinematics of the complete system p = fk_needle(P3) = fk_needle(fk([q1,q2])) Eigen::Vector ik_system(Eigen::Vector p); - /// Inverse kinematics of the pantograph according to equations of T.CESARE report - Eigen::Vector ik_panto_optimized(Eigen::Vector p); + /// Forward kinematics of the pantograph according to the equations of T.CESARE report + Eigen::Vector fk_panto_TC(Eigen::Vector q); + + /// Inverse kinematics of the pantograph according to the equations of T.CESARE report + Eigen::Vector ik_panto_TC(Eigen::Vector p); /// Populate all joints position of the full system (pantograph + needle) Eigen::Vector populate_all_joint_positions_full_system(Eigen::Vector q); diff --git a/pantograph_library/src/pantograph_model.cpp b/pantograph_library/src/pantograph_model.cpp index 9a20a54..1219bc0 100644 --- a/pantograph_library/src/pantograph_model.cpp +++ b/pantograph_library/src/pantograph_model.cpp @@ -51,8 +51,8 @@ PantographModel::set_link_lenghts( Eigen::Vector PantographModel::populate_all_joint_positions(Eigen::Vector q) { - auto p = fk(q); - return ik(p); + auto p = fk_panto_TC(q); + return ik_panto_TC(p); } Eigen::Vector @@ -152,13 +152,59 @@ PantographModel::jacobian(Eigen::Vector q, Eigen::Vector q return J; } +Eigen::Vector +PantographModel::fk_panto_TC(Eigen::Vector q) +{ + Eigen::Vector2d P2, P3, P4, P5; + + P5 << l_a5_, 0; + + // Coords of P2 in base frame + P2[0] = l_a1_ * std::cos(q[0]); + P2[1] = l_a1_ * std::sin(q[0]); + + // Coords of P4 in base frame + P4[0] = P5[0] + l_a4_ * std::cos(q[1]); + P4[1] = P5[1] + l_a4_ * std::sin(q[1]); + + // Calculate the angles of the different joints (relative to base frame) + double a24 = (P2 - P4).norm(); + + // Left arm + double gamma2 = + std::acos( + (std::pow(a24, 2) + std::pow(l_a2_, 2) - std::pow(l_a3_, 2)) / + (2 * a24 * l_a2_)); + + double delta = std::atan2(P4[1] - P2[1], P4[0] - P2[0]); + + double theta2 = gamma2 + delta; + + // Right arm + double gamma4 = + std::acos( + (std::pow(a24, 2) + std::pow(l_a3_, 2) - std::pow(l_a2_, 2)) / + (2 * a24 * l_a3_)); + + double theta4 = PI_CST - gamma4 - delta; + + // Coordinates of P3 according to left kinematic chain + P3[0] = P2[0] + l_a2_ * std::cos(theta2); + P3[1] = P2[1] + l_a2_ * std::sin(theta2); + + return P3; +} + Eigen::Vector -PantographModel::ik_panto_optimized(Eigen::Vector P3) +PantographModel::ik_panto_TC(Eigen::Vector P3) { + /* Inverse kinematics model of the pantograph, as calculated in + T.CESARE report. the angles thetai are used to simulate the position + of the different robot segments in RViz */ Eigen::Vector jnt_pos; Eigen::Vector2d P2, P4, P5; // Coords of P5 in base frame - P5 << -l_a5_, 0; + P5 << l_a5_, 0; // Calculate the length of the segment P1P3 double a13 = P3.norm(); @@ -166,59 +212,46 @@ PantographModel::ik_panto_optimized(Eigen::Vector P3) // Calculate the length of the segment P5P3 double a53 = (P3 - P5).norm(); - // Calculate the angle of the active joints + // Calculate the angles of the active joints double alpha1 = std::acos( (std::pow(l_a1_, 2) - std::pow(l_a2_, 2) + std::pow(a13, 2)) / (2 * l_a1_ * a13)); - - double beta1 = std::atan2(P3[1], -P3[0]); - - // theta1 - jnt_pos[0] = PI_CST - alpha1 - beta1; + double beta1 = std::atan2(P3[1], P3[0]); double alpha5 = std::acos( (std::pow(a53, 2) + std::pow(l_a4_, 2) - std::pow(l_a3_, 2)) / (2 * a53 * l_a4_)); - double beta5 = std::atan2(P3[1], P3[0] - l_a5_); - - // theta5 - jnt_pos[4] = alpha5 + beta5; + double beta5 = std::atan2(P3[1], l_a5_ - P3[0]); // Calculate the other joint angles - // Coords of P2 in base frame - P2[0] = l_a1_ * std::cos(jnt_pos[0]); - P2[1] = l_a1_ * std::sin(jnt_pos[0]); - - // Coords of P4 in base frame - P4[0] = P5[0] + l_a4_ * std::cos(jnt_pos[1]); - P4[1] = P5[1] + l_a4_ * std::sin(jnt_pos[1]); - - // Calculate the length of the segment P2P4 - double a24 = std::sqrt(std::pow(P4[0] - P2[0], 2) + std::pow(P4[1] - P2[1], 2)); - - // Angles of the left arm - double gamma2 = - std::acos( - (std::pow(a24, 2) + std::pow(l_a2_, 2) - std::pow(l_a3_, 2)) / - (2 * l_a2_ * a24)); - - double delta = std::atan2(P4[1] - P2[1], P4[0] - P2[0]); - - // theta2 - jnt_pos[1] = gamma2 + delta; - - // Angles of the right arm - double gamma4 = - std::acos( - (std::pow(a24, 2) + std::pow(l_a3_, 2) - std::pow(l_a2_, 2)) / - (2 * a24 * l_a3_)); - - // theta4 - jnt_pos[3] = PI_CST - gamma4 - delta; - - // theta3 - jnt_pos[2] = -(PI_CST / 2) + gamma4 + 2 * gamma2; + // left arm + double alpha2 = std::acos( + (std::pow(l_a1_, 2) + std::pow(l_a2_, 2) - std::pow(a13, 2)) / + (2 * l_a1_ * l_a2_)); + + // Right arm + double alpha4 = std::acos( + (std::pow(l_a4_, 2) + std::pow(l_a3_, 2) - std::pow(a53, 2)) / + (2 * l_a3_ * l_a4_)); + + // angle between the 2 arms + double alpha3 = PI_CST - alpha2 - alpha1; + double alpha3bis = PI_CST - alpha4 - alpha5; + double beta3 = PI_CST - beta1 - beta5; + + /*Return all the angles for Rviz render + angles are defined according to parent frame + and are positive in trigonometric direction */ + jnt_pos[0] = alpha1 + beta1; + jnt_pos[1] = - (PI_CST - alpha2); + jnt_pos[2] = PI_CST - alpha3 - beta3 - alpha3bis; + jnt_pos[3] = (PI_CST - alpha4); + jnt_pos[4] = - (PI_CST - alpha5 - beta5); + + // Calculate angle between P3 frame and tool frame + // in order to keep tool frame parallel to base frame + double theta_tool = std::asin((P3[1] - P2[1])/l_a2_ ); return jnt_pos; } @@ -237,7 +270,8 @@ PantographModel::fk_system(Eigen::Vector q) // Get coords of P3 in base frame according to FKM Eigen::Vector2d P3_2D; - P3_2D = fk(q); + // P3_2D = fk(q); // check if fk is correct + P3_2D = fk_panto_TC(q); // Convert P3 in the plane to coords in 3D P3 << P3_2D, 0; @@ -308,9 +342,7 @@ PantographModel::ik_system(Eigen::Vector PU) // Get angles of the pantograph joints using IKM Eigen::Vector jnt_pos; - jnt_pos = ik(P3_2D); // check if ik is correct - - // jnt_pos = ik_panto_optimized(P3_2D); + jnt_pos = ik_panto_TC(P3_2D); // Return pantograph joints angles + needle orientation angles Eigen::Vector jnt_ext_pos; diff --git a/pantograph_library/tests/test_panto_model.cc b/pantograph_library/tests/test_panto_model.cc index 9be253d..92dfd21 100644 --- a/pantograph_library/tests/test_panto_model.cc +++ b/pantograph_library/tests/test_panto_model.cc @@ -17,15 +17,15 @@ TEST(TestModel, TestPantoFKM) { double error = 1e-10; // q values according to MATLAB model - q[0] = 0.5; - q[1] = pantograph_library::PI_CST - 0.5; // 2.64159265358979 + q[0] = pantograph_library::PI_CST - 0.5; // 2.64159265358979 + q[1] = 0.5; // Coords of P3 according to MATLAB model - P3_matlab[0] = -0.0425000000000000; + P3_matlab[0] = 0.0425000000000000; P3_matlab[1] = 0.149223285959824; // Coords of P3 according to C++ model - P3_cpp = panto_model_.fk(q); + P3_cpp = panto_model_.fk_panto_TC(q); /*Expect values to be equal (up to a 1e-10 error) between the C++ and the MATLAB model */ @@ -45,15 +45,17 @@ TEST(TestModel, TestPantoIKM) { double error = 1e-10; // P3 coords according to MATLAB model - P3[0] = -0.0425000000000000; + P3[0] = 0.0425000000000000; P3[1] = 0.149223285959824; // q values according to MATLAB model - q_matlab[0] = 0.500000000000000; - q_matlab[1] = 2.64159265358979; + // Note: In RViz angles are defined according to parent frame + // and are positive in trigonometric direction + q_matlab[0] = 2.64159265358979; + q_matlab[1] = - 0.500000000000000; // q values according to C++ model - joints = panto_model_.ik(P3); + joints = panto_model_.ik_panto_TC(P3); q_cpp[0] = joints[0]; q_cpp[1] = joints[4]; @@ -99,7 +101,7 @@ TEST(TestModel, TestSystemFKM) { /* Test of the IKM of the system by comparing the values of q calculated by the C++ model and the same coords calculate by the MATLAB model*/ -TEST(TestModel, TestIKM) { +TEST(TestModel, TestSystemIKM) { pantograph_library::PantographModel panto_model_; Eigen::Vector < double, 2 > q_cpp, q_matlab; Eigen::Vector < double, 8 > joints; From a3af869bb47449223b13c785b8a45657e36e3c1f Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 10 Apr 2024 10:48:54 +0200 Subject: [PATCH 21/27] added mimick joints for render of the needle --- .../src/pantograph_mimick_controller.cpp | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp index 1106534..27188fc 100644 --- a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp +++ b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp @@ -68,7 +68,8 @@ controller_interface::CallbackReturn PantographMimickController::on_configure( params_.prefix + "panto_a4", params_.prefix + "panto_a5", params_.prefix + "tool_theta_joint", - params_.prefix + "tool_phi_joint"}; + params_.prefix + "tool_phi_joint", + params_.prefix + "needle_interaction_joint"}; RCLCPP_INFO(get_node()->get_logger(), "configure successful"); return controller_interface::CallbackReturn::SUCCESS; @@ -90,6 +91,8 @@ PantographMimickController::command_interface_configuration() const pantograph_joint_names_[5] + "/" + HW_IF_POSITION); command_interfaces_config.names.push_back( pantograph_joint_names_[6] + "/" + HW_IF_POSITION); + command_interfaces_config.names.push_back( + pantograph_joint_names_[7] + "/" + HW_IF_POSITION); return command_interfaces_config; } @@ -168,29 +171,32 @@ controller_interface::return_type PantographMimickController::update( q << pos_a1, pos_a5; // Calculate the pantograph (passive) joint positions - // Eigen::Vector full_joint_state; - // full_joint_state = pantograph_model_.populate_all_joint_positions(q); + Eigen::Vector panto_joint_state; + panto_joint_state = pantograph_model_.populate_all_joint_positions(q); // Calculate the passive joints of the complete system - Eigen::Vector full_joint_state; + Eigen::Vector full_joint_state; full_joint_state = pantograph_model_.populate_all_joint_positions_full_system(q); - double pos_a2 = full_joint_state[1]; - double pos_a3 = full_joint_state[2]; - double pos_a4 = full_joint_state[3]; + double pos_a2 = panto_joint_state[1]; + double pos_a3 = panto_joint_state[2]; + double pos_a4 = panto_joint_state[3]; double pos_theta = full_joint_state[5]; - double pos_phi = full_joint_state[6]; // check for correct angle orientation + double pos_phi = full_joint_state[6]; + double pos_needle = full_joint_state[7]; // write mimics to HW command_interfaces_[0].set_value(pos_a2); command_interfaces_[1].set_value(pos_a3); command_interfaces_[2].set_value(pos_a4); - // Write pitch and orientation angles to end effector universal joint + // Write angles of the end effector universal joint command_interfaces_[3].set_value(pos_theta); command_interfaces_[4].set_value(pos_phi); + // Write needle position along insertion axis + command_interfaces_[5].set_value(pos_needle); return controller_interface::return_type::OK; } From 66c2e78fb74c2b9c53334f7372352bb6b2561122 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 10 Apr 2024 13:42:21 +0200 Subject: [PATCH 22/27] error correction of pantograph model --- .../src/pantograph_mimick_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp index 27188fc..ab51ae5 100644 --- a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp +++ b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp @@ -179,7 +179,7 @@ controller_interface::return_type PantographMimickController::update( full_joint_state = pantograph_model_.populate_all_joint_positions_full_system(q); double pos_a2 = panto_joint_state[1]; - double pos_a3 = panto_joint_state[2]; + double pos_a3 = - pos_a2 - pos_a1; double pos_a4 = panto_joint_state[3]; double pos_theta = full_joint_state[5]; From 0f1026efc905c67e00add7ffcfe6290dea86fe7b Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 10 Apr 2024 15:17:08 +0200 Subject: [PATCH 23/27] updated launch file --- .../launch/pantograph.launch.py | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/pantograph_bringup/launch/pantograph.launch.py b/pantograph_bringup/launch/pantograph.launch.py index 6252f13..4fd489e 100644 --- a/pantograph_bringup/launch/pantograph.launch.py +++ b/pantograph_bringup/launch/pantograph.launch.py @@ -90,32 +90,33 @@ def generate_launch_description(): executable='spawner', arguments=['joint_state_broadcaster'], ) - # pantograph_mimick_controller_spawner = Node( - # package='controller_manager', - # executable='spawner', - # arguments=['pantograph_mimick_controller'], - # ) - - effort_controller_spawner = Node( - package='controller_manager', - executable='spawner', - arguments=['forward_effort_controller'], - ) - tool_orientation_controller_spawner = Node( + pantograph_mimick_controller_spawner = Node( package='controller_manager', executable='spawner', - arguments=['tool_orientation_controller'], + arguments=['pantograph_mimick_controller'], ) + # effort_controller_spawner = Node( + # package='controller_manager', + # executable='spawner', + # arguments=['forward_effort_controller'], + # ) + + # tool_orientation_controller_spawner = Node( + # package='controller_manager', + # executable='spawner', + # arguments=['tool_orientation_controller'], + # ) + nodes = [ control_node, rviz_node, robot_state_pub_node, joint_state_broadcaster_spawner, - # pantograph_mimick_controller_spawner, - effort_controller_spawner, - tool_orientation_controller_spawner, + pantograph_mimick_controller_spawner, + # effort_controller_spawner, + # tool_orientation_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) From 83c1fa062275caaf2a896594bf3f6faa04326945 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 10 Apr 2024 15:17:43 +0200 Subject: [PATCH 24/27] error correction of pantograph model --- pantograph_library/src/pantograph_model.cpp | 55 ++++++++++++--------- 1 file changed, 33 insertions(+), 22 deletions(-) diff --git a/pantograph_library/src/pantograph_model.cpp b/pantograph_library/src/pantograph_model.cpp index 1219bc0..5e7d643 100644 --- a/pantograph_library/src/pantograph_model.cpp +++ b/pantograph_library/src/pantograph_model.cpp @@ -213,16 +213,20 @@ PantographModel::ik_panto_TC(Eigen::Vector P3) double a53 = (P3 - P5).norm(); // Calculate the angles of the active joints + // theta 1 = q[0] double alpha1 = std::acos( (std::pow(l_a1_, 2) - std::pow(l_a2_, 2) + std::pow(a13, 2)) / (2 * l_a1_ * a13)); double beta1 = std::atan2(P3[1], P3[0]); + jnt_pos[0] = alpha1 + beta1; + // theta5 = q[1] double alpha5 = std::acos( (std::pow(a53, 2) + std::pow(l_a4_, 2) - std::pow(l_a3_, 2)) / (2 * a53 * l_a4_)); double beta5 = std::atan2(P3[1], l_a5_ - P3[0]); + jnt_pos[4] = -(PI_CST - alpha5 - beta5); // Calculate the other joint angles // left arm @@ -239,19 +243,25 @@ PantographModel::ik_panto_TC(Eigen::Vector P3) double alpha3 = PI_CST - alpha2 - alpha1; double alpha3bis = PI_CST - alpha4 - alpha5; double beta3 = PI_CST - beta1 - beta5; + double delta3 = alpha3 + alpha3bis + beta3; - /*Return all the angles for Rviz render - angles are defined according to parent frame - and are positive in trigonometric direction */ - jnt_pos[0] = alpha1 + beta1; - jnt_pos[1] = - (PI_CST - alpha2); - jnt_pos[2] = PI_CST - alpha3 - beta3 - alpha3bis; - jnt_pos[3] = (PI_CST - alpha4); - jnt_pos[4] = - (PI_CST - alpha5 - beta5); - // Calculate angle between P3 frame and tool frame // in order to keep tool frame parallel to base frame - double theta_tool = std::asin((P3[1] - P2[1])/l_a2_ ); + + // Coords of P2 in base frame + P2[0] = l_a1_ * std::cos(jnt_pos[0]); + P2[1] = l_a1_ * std::sin(jnt_pos[0]); + + // Coords of P4 in base frame + P4[0] = P5[0] + l_a4_ * std::cos(jnt_pos[4]); + P4[1] = P5[1] + l_a4_ * std::sin(jnt_pos[4]); + + /*Return the angles of all the passive joints for Rviz render + angles are defined according to parent frame + and are positive in trigonometric direction */ + jnt_pos[1] = -(PI_CST - alpha2); // = theta2 + jnt_pos[2] = -(PI_CST - delta3); // = theta3 + jnt_pos[3] = (PI_CST - alpha4); // = theta4 return jnt_pos; } @@ -270,7 +280,6 @@ PantographModel::fk_system(Eigen::Vector q) // Get coords of P3 in base frame according to FKM Eigen::Vector2d P3_2D; - // P3_2D = fk(q); // check if fk is correct P3_2D = fk_panto_TC(q); // Convert P3 in the plane to coords in 3D @@ -279,14 +288,13 @@ PantographModel::fk_system(Eigen::Vector q) // Convert coords of PI in base frame to P3 frame P3I = PI - P3; - // Get azimuth (theta) and elevation (phi) angles (in radians) of vector P3PI + // Get angles (in radians) of vector P3PI according to P3 frame: + // theta is the angle of rotation around z + // phi is the angle of rotation around y double theta = std::atan2(P3I[1], P3I[0]); double phi = std::atan2(P3I[2], std::sqrt(std::pow(P3I[0], 2) + std::pow(P3I[1], 2))); double Lin = std::sqrt(std::pow(P3I[0], 2) + std::pow(P3I[1], 2) + std::pow(P3I[2], 2)); - // Check for correct definition of phi angle - phi = (PI_CST / 2) - phi; - // Calculate translation from PI to PU double Lout = l_needle_ - Lin; PIU[0] = Lout * std::cos(phi) * std::cos(theta); // Coords of PU in PI frame @@ -313,15 +321,17 @@ PantographModel::ik_system(Eigen::Vector PU) PI[2] = PI_z; // = 0.09; // Transformation of PU coords in base frame to coords in PI frame - PIU = PI - PU; + // PIU = PI - PU; // check minus sign + PIU = PU - PI; - // Get azimuth (theta) and elevation (phi) angles (in radians) of vector PIPU + // Get angles (in radians) of vector PIPU in PI frame + // theta is the angle of rotation around z + // phi is the angle of rotation around y double theta = std::atan2(PIU[1], PIU[0]); double phi = std::atan2(PIU[2], std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2))); double Lout = std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2) + std::pow(PIU[2], 2)); - - // Check for correct definition of phi angle - phi = (PI_CST / 2) - phi; + //convert phi for the correct definition of the angle in RViz + phi = phi - (PI_CST / 2); // Get length of the needle segment between PI and P3 double Lin = l_needle_ - Lout; @@ -329,10 +339,10 @@ PantographModel::ik_system(Eigen::Vector PU) // Transformation from I to P3 in I frame PI3[0] = Lin * std::cos(phi) * std::cos(theta); PI3[1] = Lin * std::cos(phi) * std::sin(theta); - PI3[2] = Lin * std::sin(phi); + PI3[2] = -Lin * std::sin(phi); // Transformation of P3 coords on I frame to coords in base frame - // P3 = PI - PI3 + //P3 = PI - PI3; P3 = PI + PI3; // Convert P3 coords in 3D to coords in plane @@ -345,6 +355,7 @@ PantographModel::ik_system(Eigen::Vector PU) jnt_pos = ik_panto_TC(P3_2D); // Return pantograph joints angles + needle orientation angles + Eigen::Vector jnt_ext_pos; jnt_ext_pos << jnt_pos, theta, phi, Lout; From b00f9856cf45c4cb7ce94ea121ac7e5671a54f92 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 10 Apr 2024 15:18:30 +0200 Subject: [PATCH 25/27] updated with new pantograph model --- .../src/pantograph_mimick_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp index ab51ae5..8e8889a 100644 --- a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp +++ b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp @@ -179,7 +179,7 @@ controller_interface::return_type PantographMimickController::update( full_joint_state = pantograph_model_.populate_all_joint_positions_full_system(q); double pos_a2 = panto_joint_state[1]; - double pos_a3 = - pos_a2 - pos_a1; + double pos_a3 = -pos_a2 - pos_a1; double pos_a4 = panto_joint_state[3]; double pos_theta = full_joint_state[5]; From 42b0a39a7b5c5a83cf0d3b410bc8a457f646c581 Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 10 Apr 2024 15:19:02 +0200 Subject: [PATCH 26/27] updated to test new panto model --- pantograph_library/tests/test_panto_model.cc | 26 +++++++++++--------- 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/pantograph_library/tests/test_panto_model.cc b/pantograph_library/tests/test_panto_model.cc index 92dfd21..3c2d810 100644 --- a/pantograph_library/tests/test_panto_model.cc +++ b/pantograph_library/tests/test_panto_model.cc @@ -49,10 +49,10 @@ TEST(TestModel, TestPantoIKM) { P3[1] = 0.149223285959824; // q values according to MATLAB model - // Note: In RViz angles are defined according to parent frame + // Note: In RViz angles are defined according to parent frame // and are positive in trigonometric direction q_matlab[0] = 2.64159265358979; - q_matlab[1] = - 0.500000000000000; + q_matlab[1] = -0.500000000000000; // q values according to C++ model joints = panto_model_.ik_panto_TC(P3); @@ -80,13 +80,13 @@ TEST(TestModel, TestSystemFKM) { double error = 1e-10; // q values according to MATLAB model - q[0] = 0.5; - q[1] = 2.64159265358979; // pantograph_library::PI_CST - 0.5; + q[0] = 2.64159265358979; // pantograph_library::PI_CST - 0.5; + q[1] = 0.5; // Coords of PU according to MATLAB model - PU_matlab[0] = 0.0942524755499338; - PU_matlab[1] = 0.167462388425657; - PU_matlab[2] = 0.144796738817577; + PU_matlab[0] = 0.0425000000000000; + PU_matlab[1] = 0.174218467450767; + PU_matlab[2] = 0.198431955345491; // Coords of PU according to C++ model PU_cpp = panto_model_.fk_system(q); @@ -110,13 +110,15 @@ TEST(TestModel, TestSystemIKM) { double error = 1e-10; // PU coords according to MATLAB model - PU_matlab[0] = 0.0942524755499338; - PU_matlab[1] = 0.167462388425657; - PU_matlab[2] = 0.144796738817577; + PU_matlab[0] = 0.0425000000000000; + PU_matlab[1] = 0.174218467450767; + PU_matlab[2] = 0.198431955345491; // q values according to MATLAB model - q_matlab[0] = 0.500000000000000; - q_matlab[1] = 2.64159265358979; // PI_CST - 0.5 + // Note: In RViz angles are defined according to parent frame + // and are positive in trigonometric direction + q_matlab[0] = 2.64159265358979; + q_matlab[1] = -0.500000000000000; // q values according to C++ model joints = panto_model_.ik_system(PU_matlab); From 3928d78e967b1ae04d828f532d34a7b9ebe2f95d Mon Sep 17 00:00:00 2001 From: Luis Maldonado Date: Wed, 10 Apr 2024 15:51:52 +0200 Subject: [PATCH 27/27] updated system model --- pantograph_library/src/pantograph_model.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/pantograph_library/src/pantograph_model.cpp b/pantograph_library/src/pantograph_model.cpp index 5e7d643..787f0ff 100644 --- a/pantograph_library/src/pantograph_model.cpp +++ b/pantograph_library/src/pantograph_model.cpp @@ -299,10 +299,11 @@ PantographModel::fk_system(Eigen::Vector q) double Lout = l_needle_ - Lin; PIU[0] = Lout * std::cos(phi) * std::cos(theta); // Coords of PU in PI frame PIU[1] = Lout * std::cos(phi) * std::sin(theta); - PIU[2] = Lout * std::sin(phi); + PIU[2] = - Lout * std::sin(phi); // Convert coords of PU in PI frame to coords in base frame PU = PI + PIU; + // PU[2] = PU[2] - 2 * PI_z; return PU; } @@ -331,15 +332,19 @@ PantographModel::ik_system(Eigen::Vector PU) double phi = std::atan2(PIU[2], std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2))); double Lout = std::sqrt(std::pow(PIU[0], 2) + std::pow(PIU[1], 2) + std::pow(PIU[2], 2)); //convert phi for the correct definition of the angle in RViz - phi = phi - (PI_CST / 2); + phi = phi + (PI_CST / 2); // Get length of the needle segment between PI and P3 double Lin = l_needle_ - Lout; // Transformation from I to P3 in I frame - PI3[0] = Lin * std::cos(phi) * std::cos(theta); - PI3[1] = Lin * std::cos(phi) * std::sin(theta); - PI3[2] = -Lin * std::sin(phi); + // PI3[0] = Lin * std::cos(phi) * std::cos(theta); + // PI3[1] = Lin * std::cos(phi) * std::sin(theta); + // PI3[2] = -Lin * std::sin(phi); + + PI3[0] = PI_x + Lin * std::sin(phi) * std::cos(theta); + PI3[1] = PI_y + Lin * std::sin(phi) * std::sin(theta); + PI3[2] = PI_z - Lin * std::sin(phi); // Transformation of P3 coords on I frame to coords in base frame //P3 = PI - PI3;