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

It's hard to directly code this, but you may solve this problem in a view of optimization (and many state-of-art method can be implied to solve this, which can be easily found on the Internet). Let x' and y' be the fixed topmost A, and x'' and y'' be the target. Then this problem can be formalized as the follows,

where gamma_i is the weight of rotation of each joint (you may simply let gamma_i=1), and alpha_i is the rotation value. Your optimization goal is to get the minimum rotation of the parents, subject to some constraints. x', y', x'', y'' are constants, the other are variables. Other inequality can be added to limit the angle of rotation.

Equation f(x',y',x_1,y_1,alpha_1)=0 means, with the rotation of A by alpha_1, point B can be relocated to x_1, y_1. If you can specify the detail of rotation (for example is f is linear, then simplex method can be implemented, but it seems that f is quadratic).

Sorry for my poor English, and wish this can help :p

Answer 2

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