no time to finish...

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

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,26 +35,31 @@ 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);
-
+ // Add gripper publisher
-    // Parse the target position
-    double targetX = atof(argv[1]);
-    double targetY = atof(argv[2]);
-    double targetZ = atof(argv[3]);
 
     advrobotics_lab3::ikpoppy srv;
-    srv.request.x = targetX;
+    srv.request.x = atof(argv[1]);
-    srv.request.y = targetY;
+    srv.request.y = atof(argv[2]);
-    srv.request.z = targetZ;
+    srv.request.z = atof(argv[3]);
 
-    // Call the IK service to get the target joint positions
+    // Open the gripper before moving
-    if (!client.call(srv))
+    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
     {
-        ROS_ERROR("Failed to call the service ik!");
+    	gripperCmdPublisher.publish(gripperCmd);
-        return 1;
+    	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)", targetX, targetY, targetZ,
+        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;
@@ -68,44 +67,16 @@ int main(int argc, char** argv)
         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();
+        ros::Rate loopRate(_fps);
         loopRate.sleep();
     }
-
+    else
-    ROS_INFO("Robot has reached the target position.");
+    {
-    return 0;
+        ROS_ERROR("Failed to call the service ik!");
+        return 1;
     }
 
+    return 0;
+}