dorian.veloso
/
LAB3_SRONG_PHERWANI_VELOSO
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

no time to finish...

This commit is contained in:
Dorian VELOSO 2024-12-11 17:18:58 +01:00
parent 08abb0d06f
commit d8ee7a748d
2 changed files with 38 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
INSTALL_dynamixelSDK.txt
View File

@ -1,7 +1,7 @@
Clone the Dynamixel SDK git repository on the laptop using the following command: Clone the Dynamixel SDK git repository on the laptop using the following command:
$ git clone https://github.com/ROBOTIS-GIT/DynamixelSDK.git $ 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++ $ cd DynamixelSDK/c++
Go into the build directory using: Go into the build directory using:
$ cd build $ cd build

79
ik_client.cpp
View File

@ -19,12 +19,6 @@ double rad2deg(double rad)
return rad * 180.0 / M_PI; 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) int main(int argc, char** argv)
{ {
ros::init(argc, argv, "ik_client"); 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 jointCmdPublisher = nh.advertise<advrobotics_lab3::joints>("joint_cmd", 1);
ros::Publisher gripperCmdPublisher = nh.advertise<advrobotics_lab3::gripper>("gripper_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; 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("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)); rad2deg(srv.response.q1), rad2deg(srv.response.q2), rad2deg(srv.response.q3));
advrobotics_lab3::joints jointCmdMsg; advrobotics_lab3::joints jointCmdMsg;
@ -68,44 +67,16 @@ int main(int argc, char** argv)
jointCmdMsg.q2 = rad2deg(srv.response.q2); jointCmdMsg.q2 = rad2deg(srv.response.q2);
jointCmdMsg.q3 = rad2deg(srv.response.q3); 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); 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::spinOnce();
ros::Rate loopRate(_fps);
loopRate.sleep(); loopRate.sleep();
} }
else
{
ROS_ERROR("Failed to call the service ik!");
return 1;
}
ROS_INFO("Robot has reached the target position.");     return 0;
return 0;
} }