3d - Working of CCD algorithm for Inverse Kinematics

Question

Lets say I've a robotic arm with joints at points A,B,C,D in a 3D space. Let D be the end effector(bottommost child) and A be the topmost parent. Let T be the target point anywhere in the space. The aim is to make the end effector reach the target with minimum rotations in top levels(parents).

What I initially thought:

1) Rotate the arm C by angle TCD. 2) Then rotate the arm B by new angle TBD. 3) Then rotate the arm A by new angle TAD.

But the end effector seems to point away from the target after step 2. What am I doing wrong and how can I fix it?

Answer 1

Before I started use some more advanced approaches I did it like this:

pe=desired_new_position;

for (i=0;i<number_of_actuators;i++)
 {
 // choose better direction
                   p=direct_kinematics(); d =|p-pe|; // actual step
 actuator(i)--;  p=direct_kinematics(); d0=|p-pe|; // previous step
 actuator(i)+=2; p=direct_kinematics(); d1=|p-pe|; // next step
 actuator(i)--;  dir=0; d0=d;
      if ((d0<d)&&(d0<d1)) dir=-1;
 else if ((d1<d)&&(d1<d0)) dir=+1;
 else continue;

 for (;;)
  {
  actuator(i)+=dir; p=direct_kinematics(); d =|p-pe|;
  if (d>d0) { actuator(i)-=dir; break; }
  if (actuator(i) on the edge limit) break;
  }

 }

[notes]

1. you can modify it to inc/dec actuator position by some step instead of 1

   stop if difference crossed zero then start again with smaller step until step == 1 This will improve performance but for most application is step=1 enough because new position is usually near the last one.

2. beware that this can get stuck in local min/max

   if the output get stuck (effector position is unchanged) then randomize the actuators and try again. Occurrence of this depend on the kinematics complexity and on the kind of path you want to use

3. if the arms are driven more on the top then on the bottom

   then try reverse the i-for loop

4. if you have to control the effector normal

   then you have to exclude its rotation axises from CCD and set it before CCD