fork download
copy 
  1. #include <stdio.h>
  2. #include <math.h>
  3. #include <stdlib.h>
  4.  
  5. #define NUM_POINTS 100
  6.  
  7. // Structure to hold joint angles
  8. typedef struct {
  9.     double theta1;
  10.     double theta2;
  11.     double theta3;
  12. } JointAngles;
  13.  
  14. // Function to calculate inverse kinematics
  15. JointAngles inverse_kinematics(double x, double y, double z) {
  16.     // Given parameters
  17.     double a1 = 1.5;   // m
  18.     double a2 = 1.2;   // m
  19.     double d2 = 0.3;   // m
  20.     double d3 = 0.3;   // m
  21.     double a3 = 0.75;  // m
  22.  
  23.     JointAngles angles;
  24.  
  25.     // Solve for theta1
  26.     angles.theta1 = atan2(y, x);
  27.  
  28.     // Solve for theta2 and theta3
  29.     double r = sqrt(x*x + y*y) - a1;
  30.     double h = z - d2 - d3;
  31.  
  32.     double D = (r*r + h*h - a2*a2 - a3*a3)/(2*a2*a3);
  33.     angles.theta3 = atan2(sqrt(1-D*D), D);
  34.  
  35.     double k1 = a2 + a3*cos(angles.theta3);
  36.     double k2 = a3*sin(angles.theta3);
  37.     angles.theta2 = atan2(h, r) - atan2(k2, k1);
  38.  
  39.     return angles;
  40. }
  41.  
  42. // Function to perform path planning and plot joint variables
  43. void path_planning() {
  44.     // Define start and end points (example values)
  45.     double p1[3] = {1.0, 1.5, 0.5};  // (x1, y1, z1)
  46.     double p2[3] = {1.5, 1.0, 0.7};  // (x2, y2, z2)
  47.  
  48.     // Time parameters
  49.     double t_total = 10.0;  // seconds
  50.     double dt = t_total / (NUM_POINTS - 1);
  51.  
  52.     // Arrays to store results
  53.     double time[NUM_POINTS];
  54.     JointAngles angles[NUM_POINTS];
  55.     int i;
  56.  
  57.     // Linear path planning
  58.     for (i = 0; i < NUM_POINTS; i++) {
  59.         time[i] = i * dt;
  60.  
  61.         // Interpolate position
  62.         double alpha = (double)i / (NUM_POINTS - 1);
  63.         double x = p1[0] + alpha * (p2[0] - p1[0]);
  64.         double y = p1[1] + alpha * (p2[1] - p1[1]);
  65.         double z = p1[2] + alpha * (p2[2] - p1[2]);
  66.  
  67.         // Calculate inverse kinematics
  68.         angles[i] = inverse_kinematics(x, y, z);
  69.     }
  70.  
  71.     // Write results to file for plotting (can use GNUplot or other tools)
  72.     FILE *fp = fopen("joint_variables.txt", "w");
  73.     if (fp == NULL) {
  74.         printf("Error opening file!\n");
  75.         return;
  76.     }
  77.  
  78.     fprintf(fp, "Time theta1 theta2 theta3\n");
  79.     for (i = 0; i < NUM_POINTS; i++) {
  80.         fprintf(fp, "%.2f %.4f %.4f %.4f\n", 
  81.                 time[i], angles[i].theta1, angles[i].theta2, angles[i].theta3);
  82.     }
  83.     fclose(fp);
  84.  
  85.     printf("Joint variables saved to 'joint_variables.txt'\n");
  86.     printf("You can plot this data using GNUplot or other tools.\n");
  87. }
  88.  
  89. int main() {
  90.     printf("PUMA Robot Inverse Kinematics and Path Planning\n");
  91.     path_planning();
  92.     return 0;
  93. }
Success #stdin #stdout 0s 5292KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
PUMA Robot Inverse Kinematics and Path Planning
Error opening file!
https://ideone.com/aICFEU
language:
C (gcc 8.3)
created:
7 days ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!