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:
$ 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

103
ik_client.cpp
View File

@ -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;
}