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
diff --git a/pantograph_bringup/launch/pantograph.launch.py b/pantograph_bringup/launch/pantograph.launch.py
index 00439e5..4fd489e 100644
--- a/pantograph_bringup/launch/pantograph.launch.py
+++ b/pantograph_bringup/launch/pantograph.launch.py
@@ -90,17 +90,24 @@ 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'],
- )
+ # 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,
@@ -108,7 +115,8 @@ def generate_launch_description():
robot_state_pub_node,
joint_state_broadcaster_spawner,
pantograph_mimick_controller_spawner,
- effort_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 89e1f27..0344bca 100644
--- a/pantograph_description/config/ros2_controllers.yaml
+++ b/pantograph_description/config/ros2_controllers.yaml
@@ -11,6 +11,10 @@ controller_manager:
type: pantograph_mimick_controller/PantographMimickController
forward_effort_controller:
type: effort_controllers/JointGroupEffortController
+ tool_orientation_controller:
+ type: position_controllers/JointGroupPositionController
+ panto_position_controller:
+ type: position_controllers/JointGroupPositionController
haptic_trajectory_controller:
@@ -32,3 +36,15 @@ forward_effort_controller:
joints:
- panto_a1
- panto_a5
+
+tool_orientation_controller:
+ ros__parameters:
+ joints:
+ - 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 05ae079..e5d8005 100644
--- a/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro
+++ b/pantograph_description/ros2_control/pantograph.r2c_hardware.xacro
@@ -142,6 +142,37 @@
+
+
+
+
+ ${0}
+ -3
+ 3
+
+
+
+
+
+
+ ${0}
+ -3
+ 3
+
+
+
+
+
+
+
+ ${0}
+ -0.5
+ 0.5
+
+
+
+
+
diff --git a/pantograph_description/urdf/pantograph.urdf.xacro b/pantograph_description/urdf/pantograph.urdf.xacro
index d12c897..082caff 100644
--- a/pantograph_description/urdf/pantograph.urdf.xacro
+++ b/pantograph_description/urdf/pantograph.urdf.xacro
@@ -185,6 +185,44 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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/include/pantograph_library/pantograph_model.hpp b/pantograph_library/include/pantograph_library/pantograph_model.hpp
index 68cfa3b..ec785be 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,24 @@ 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);
+
+ /// 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);
+
+ /// 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 +82,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/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
diff --git a/pantograph_library/src/pantograph_model.cpp b/pantograph_library/src/pantograph_model.cpp
index 7ebdec7..787f0ff 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
@@ -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]);
@@ -118,11 +123,295 @@ 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!");
- return Eigen::Matrix::Zero();
+ // 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 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_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;
+
+ // 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 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
+ 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;
+ double delta3 = alpha3 + alpha3bis + beta3;
+
+ // Calculate angle between P3 frame and tool frame
+ // in order to keep tool frame parallel to base frame
+
+ // 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;
+}
+
+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; // = 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_panto_TC(q);
+
+ // Convert P3 in the plane to coords in 3D
+ P3 << P3_2D, 0;
+
+ // Convert coords of PI in base frame to P3 frame
+ P3I = PI - P3;
+
+ // 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));
+
+ // 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;
+ // PU[2] = PU[2] - 2 * PI_z;
+
+ return PU;
}
+Eigen::Vector
+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;
+
+ // Coords of insertion point PI in base frame according to CAD
+ 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 = PI - PU; // check minus sign
+ PIU = PU - PI;
+
+ // 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));
+ //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;
+
+ // 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] = 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;
+ 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_panto_TC(P3_2D);
+
+ // Return pantograph joints angles + needle orientation angles
+
+ 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
+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 (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
+ 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
+
+ // 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; // As a first approximation, L_out is constant
+
+ /*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 / 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
diff --git a/pantograph_library/tests/test_panto_model.cc b/pantograph_library/tests/test_panto_model.cc
new file mode 100644
index 0000000..3c2d810
--- /dev/null
+++ b/pantograph_library/tests/test_panto_model.cc
@@ -0,0 +1,132 @@
+// 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
+by the C++ model and the same coords calculate by the MATLAB model*/
+
+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] = 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[1] = 0.149223285959824;
+
+ // Coords of P3 according to C++ model
+ 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 */
+
+ 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 < double, 2 > q_cpp, q_matlab, P3;
+ Eigen::Vector < double, 5 > 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
+ // 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_panto_TC(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 < double, 2 > q;
+ Eigen::Vector < double, 3 > PU_cpp, PU_matlab;
+
+ double error = 1e-10;
+
+ // q values according to MATLAB model
+ 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.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);
+
+ /*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, TestSystemIKM) {
+ pantograph_library::PantographModel panto_model_;
+ Eigen::Vector < double, 2 > q_cpp, q_matlab;
+ Eigen::Vector < double, 8 > joints;
+ Eigen::Vector3d PU_matlab;
+
+ double error = 1e-10;
+
+ // PU coords according to MATLAB model
+ PU_matlab[0] = 0.0425000000000000;
+ PU_matlab[1] = 0.174218467450767;
+ PU_matlab[2] = 0.198431955345491;
+
+ // q values according to MATLAB model
+ // 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);
+ 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);
+}
diff --git a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp
index 199df7e..8e8889a 100644
--- a/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp
+++ b/pantograph_mimick_controller/src/pantograph_mimick_controller.cpp
@@ -66,7 +66,10 @@ 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_theta_joint",
+ params_.prefix + "tool_phi_joint",
+ params_.prefix + "needle_interaction_joint"};
RCLCPP_INFO(get_node()->get_logger(), "configure successful");
return controller_interface::CallbackReturn::SUCCESS;
@@ -84,6 +87,12 @@ 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);
+ command_interfaces_config.names.push_back(
+ pantograph_joint_names_[7] + "/" + HW_IF_POSITION);
return command_interfaces_config;
}
@@ -162,18 +171,33 @@ 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;
+ 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 = -pos_a2 - pos_a1;
+ double pos_a4 = panto_joint_state[3];
+
+ double pos_theta = full_joint_state[5];
+ 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 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;
}