From d8ee7a748dc5bb9e7288cc90f1f2275a724aaf6a Mon Sep 17 00:00:00 2001
From: "dorian.veloso" <dorian.veloso@ecam.fr>
Date: Wed, 11 Dec 2024 17:18:58 +0100
Subject: [PATCH] no time to finish...

---
 INSTALL_dynamixelSDK.txt |   2 +-
 ik_client.cpp            | 103 ++++++++++++++-------------------------
 2 files changed, 38 insertions(+), 67 deletions(-)

diff --git a/INSTALL_dynamixelSDK.txt b/INSTALL_dynamixelSDK.txt
index c2ac97f..0c6446d 100644
--- a/INSTALL_dynamixelSDK.txt
+++ b/INSTALL_dynamixelSDK.txt
@@ -1,7 +1,7 @@
 Clone the Dynamixel SDK git repository on the laptop using the following command:
 $ git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git
 
-Go into the DynamixelSDK/c++/ directory using:
+Go into the DynamixelSDK/c++/ directory using:x²
 $ cd DynamixelSDK/c++
 Go into the build directory using:
 $ cd build
diff --git a/ik_client.cpp b/ik_client.cpp
index 37fcc95..c7c9fcf 100644
--- a/ik_client.cpp
+++ b/ik_client.cpp
@@ -19,12 +19,6 @@ double rad2deg(double rad)
     return rad * 180.0 / M_PI;
 }
 
-// Function to calculate the Euclidean distance
-double calculateDistance(double x1, double y1, double z1, double x2, double y2, double z2)
-{
-    return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2) + pow(z2 - z1, 2));
-}
-
 int main(int argc, char** argv)
 {
     ros::init(argc, argv, "ik_client");
@@ -41,71 +35,48 @@ int main(int argc, char** argv)
 
     ros::Publisher jointCmdPublisher = nh.advertise<advrobotics_lab3::joints>("joint_cmd", 1);
     ros::Publisher gripperCmdPublisher = nh.advertise<advrobotics_lab3::gripper>("gripper_cmd", 1);
-
-    // Parse the target position
-    double targetX = atof(argv[1]);
-    double targetY = atof(argv[2]);
-    double targetZ = atof(argv[3]);
+ // Add gripper publisher
 
     advrobotics_lab3::ikpoppy srv;
-    srv.request.x = targetX;
-    srv.request.y = targetY;
-    srv.request.z = targetZ;
+    srv.request.x = atof(argv[1]);
+    srv.request.y = atof(argv[2]);
+    srv.request.z = atof(argv[3]);
 
-    // Call the IK service to get the target joint positions
-    if (!client.call(srv))
+    // Open the gripper before moving
+    advrobotics_lab3::gripper gripperCmd;
+    gripperCmd.gripper = -40.0; // Assuming -40.0 is the "open" position
+    ROS_INFO("Publishing gripper command: %f", gripperCmd.gripper);
+    ros::Rate rate(10); // Publish at 10 Hz
+    for (int i = 0; i < 10; ++i) // Publish for 1 second
+    {
+    	gripperCmdPublisher.publish(gripperCmd);
+    	ros::spinOnce();
+    	rate.sleep(); // Allow time for the gripper to open
+    }
+    ros::Duration(3.0).sleep(); // Allow time for the gripper to open
+
+    // Send the target position
+    if (client.call(srv))
+    {
+        ROS_INFO("IK: (x, y, z) --> (J1, J2, J3)");
+        ROS_INFO("(%f, %f, %f) --> (%f, %f, %f)", atof(argv[1]), atof(argv[2]), atof(argv[3]),
+                 rad2deg(srv.response.q1), rad2deg(srv.response.q2), rad2deg(srv.response.q3));
+
+        advrobotics_lab3::joints jointCmdMsg;
+        jointCmdMsg.q1 = rad2deg(srv.response.q1);
+        jointCmdMsg.q2 = rad2deg(srv.response.q2);
+        jointCmdMsg.q3 = rad2deg(srv.response.q3);
+
+        jointCmdPublisher.publish(jointCmdMsg);
+        ros::spinOnce();
+        ros::Rate loopRate(_fps);
+        loopRate.sleep();
+    }
+    else
     {
         ROS_ERROR("Failed to call the service ik!");
         return 1;
     }
 
-    ROS_INFO("IK: (x, y, z) --> (J1, J2, J3)");
-    ROS_INFO("(%f, %f, %f) --> (%f, %f, %f)", targetX, targetY, targetZ,
-             rad2deg(srv.response.q1), rad2deg(srv.response.q2), rad2deg(srv.response.q3));
-
-    advrobotics_lab3::joints jointCmdMsg;
-    jointCmdMsg.q1 = rad2deg(srv.response.q1);
-    jointCmdMsg.q2 = rad2deg(srv.response.q2);
-    jointCmdMsg.q3 = rad2deg(srv.response.q3);
-
-    // Gripper command
-    advrobotics_lab3::gripper gripperCmd;
-
-    // Initialize robot's current position
-    double currentX = 0.0, currentY = 0.0, currentZ = 0.0; // Assuming starting at (0, 0, 0)
-
-    ros::Rate loopRate(10); // Loop at 10 Hz
-    double distanceThreshold = 1.0; // Threshold for "near target" in units
-
-    while (ros::ok())
-    {
-        // Calculate the distance to the target
-        double distanceToTarget = calculateDistance(currentX, currentY, currentZ, targetX, targetY, targetZ);
-
-        // Publish joint commands
-        jointCmdPublisher.publish(jointCmdMsg);
-
-        // If the robot is near the target, open the gripper
-        if (distanceToTarget <= distanceThreshold)
-        {
-            gripperCmd.gripper = -40.0; // Open position
-            gripperCmdPublisher.publish(gripperCmd);
-            ROS_INFO("Gripper opened as robot reached near the target position.");
-            break;
-        }
-
-        // Simulate robot progress (for demonstration purposes, adjust this for real-world feedback)
-        currentX += (targetX - currentX) * 0.1; // Simulated step toward target
-        currentY += (targetY - currentY) * 0.1;
-        currentZ += (targetZ - currentZ) * 0.1;
-
-        ROS_INFO("Current Position: (%f, %f, %f), Distance to Target: %f", currentX, currentY, currentZ, distanceToTarget);
-
-        ros::spinOnce();
-        loopRate.sleep();
-    }
-
-    ROS_INFO("Robot has reached the target position.");
-    return 0;
-}
-
+    return 0;
+}
\ No newline at end of file