Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add needle description #6

Merged
merged 28 commits into from
Apr 10, 2024
Merged
Changes from 1 commit
Commits
Show all changes
28 commits
Select commit Hold shift + click to select a range
1a02242
Update README.md
BlackSnow-333 Mar 18, 2024
5b6a469
Merge branch 'ICube-Robotics:main' into main
BlackSnow-333 Mar 25, 2024
724c02b
add needle to description pkg
BlackSnow-333 Mar 27, 2024
f45b1f3
added C++ model of the complete system for visualization
BlackSnow-333 Apr 4, 2024
1abb608
updated C++ model of the pantograph (added needle description and for…
BlackSnow-333 Apr 5, 2024
f9fab7a
added "tests" folder and updated CMake config
BlackSnow-333 Apr 5, 2024
88ff028
Updated test description
BlackSnow-333 Apr 5, 2024
b340288
updated test description
BlackSnow-333 Apr 5, 2024
97aaf94
added python model of the complete system (pantograph + needle) and f…
BlackSnow-333 Apr 5, 2024
a62f50f
modified to include universal joint at end effector
BlackSnow-333 Apr 8, 2024
e83bb30
added tool_orientation_controller for testing of universal joint at e…
BlackSnow-333 Apr 8, 2024
1f173cf
Added jacobian definition according to T.CESARE report
BlackSnow-333 Apr 8, 2024
6a24cc8
modified controller to include universal joint at end effector for si…
BlackSnow-333 Apr 8, 2024
5aac159
changed joint names for clarification
BlackSnow-333 Apr 9, 2024
5ad33ea
changed joint names for clarification
BlackSnow-333 Apr 9, 2024
477cfd6
added panto_position_controller for testing
BlackSnow-333 Apr 9, 2024
2e8e5bf
corrected syntaxis errors
BlackSnow-333 Apr 9, 2024
81b421a
changed type of tool joint from "revolute" to "continuous"
BlackSnow-333 Apr 9, 2024
73ad104
added prismatic joint for modelling of needle insertion
BlackSnow-333 Apr 9, 2024
96718ae
added prismatic joint for needle insertion modelling
BlackSnow-333 Apr 9, 2024
65b7ac7
error correction on pantograph model
BlackSnow-333 Apr 10, 2024
a3af869
added mimick joints for render of the needle
BlackSnow-333 Apr 10, 2024
66c2e78
error correction of pantograph model
BlackSnow-333 Apr 10, 2024
0f1026e
updated launch file
BlackSnow-333 Apr 10, 2024
83c1fa0
error correction of pantograph model
BlackSnow-333 Apr 10, 2024
b00f985
updated with new pantograph model
BlackSnow-333 Apr 10, 2024
42b0a39
updated to test new panto model
BlackSnow-333 Apr 10, 2024
3928d78
updated system model
BlackSnow-333 Apr 10, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
error correction on pantograph model
BlackSnow-333 committed Apr 10, 2024
commit 65b7ac7082d552d9cd32dcc7d3b1b8651d6e942c
Original file line number Diff line number Diff line change
@@ -55,8 +55,11 @@ class PantographModel
/// Inverse kinematics of the complete system p = fk_needle(P3) = fk_needle(fk([q1,q2]))
Eigen::Vector<double, 8> ik_system(Eigen::Vector<double, 3> p);

/// Inverse kinematics of the pantograph according to equations of T.CESARE report
Eigen::Vector<double, 5> ik_panto_optimized(Eigen::Vector<double, 2> p);
/// Forward kinematics of the pantograph according to the equations of T.CESARE report
Eigen::Vector<double, 2> fk_panto_TC(Eigen::Vector<double, 2> q);

/// Inverse kinematics of the pantograph according to the equations of T.CESARE report
Eigen::Vector<double, 5> ik_panto_TC(Eigen::Vector<double, 2> p);

/// Populate all joints position of the full system (pantograph + needle)
Eigen::Vector<double, 8> populate_all_joint_positions_full_system(Eigen::Vector<double, 2> q);
134 changes: 83 additions & 51 deletions pantograph_library/src/pantograph_model.cpp
Original file line number Diff line number Diff line change
@@ -51,8 +51,8 @@ PantographModel::set_link_lenghts(
Eigen::Vector<double, 5>
PantographModel::populate_all_joint_positions(Eigen::Vector<double, 2> q)
{
auto p = fk(q);
return ik(p);
auto p = fk_panto_TC(q);
return ik_panto_TC(p);
}

Eigen::Vector<double, 2>
@@ -152,73 +152,106 @@ PantographModel::jacobian(Eigen::Vector<double, 2> q, Eigen::Vector<double, 2> q
return J;
}

Eigen::Vector<double, 2>
PantographModel::fk_panto_TC(Eigen::Vector<double, 2> 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<double, 5>
PantographModel::ik_panto_optimized(Eigen::Vector<double, 2> P3)
PantographModel::ik_panto_TC(Eigen::Vector<double, 2> 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<double, 5> 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();

// 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<double, 2> 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<double, 3> PU)

// Get angles of the pantograph joints using IKM
Eigen::Vector<double, 5> 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<double, 8> jnt_ext_pos;
20 changes: 11 additions & 9 deletions pantograph_library/tests/test_panto_model.cc
Original file line number Diff line number Diff line change
@@ -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;