Adding Potential fields as coupling term

Ferdinand50 commented 9 months ago

Hello,

I am currently trying to add the potential field method as a coupling term to the dmp. So far I could achieve decent results with a python implementation by writing my own analytical_solution_external_force() method. I simply added the potential field force to the forcing term.

        #calculate external force
        phi_x_v, point_robot, external_force = self.potential_field.calculate_external_forces(xs[tt, dim], obstacle.position)
        # sum forcing term force with external force 
        force = np.add(forcing_terms[tt, :], phi_x_v)
        # Add forcing term to the acceleration of the spring state
        xds[tt, SPRING_Z] = xds[tt, SPRING_Z] + force / self._tau #add external force

When It comes to the cpp implementation things did not turn out right. For me it looks like that executing the dmp via dmp->analyticalSolution(ts, xs_ana, xds_ana, forcing_terms, fa_output) in cpp results in different output compared to the dmp.analytical_solution(ts) in python. It seems that the dmp is not reaching its attactor state in cpp with the analytical solution. Therefore implementing the potential field method in cpp is not possible.

I also tried to implement the potential field method via the real-time integration methods but since there is no forcing term I think it is not possible. Do you have any suggestions to me?

stulp commented 9 months ago

Thanks for using dmpbbo!

My assumption is that you would like to implement a potential field for obstacle avoidance. I.e. some obstacle should add a further forcing term to the DMP accelerations, on top of the forcing terming computed by the function approximators. This "obstacle forcing term" you are trying to add to the analytical solution.

Recommendation: approach it differently

This raises interesting questions. External obstacles (or other factors) and analyticalSolution inherintly do not blend together well I think, because

The analyticalSolution integrates the system for the entire duration in one go without considering external factors (such as obstacles) along the way. It has an "eyes shut" approach.
The distance between the obstacle and the end-effector should be bases on actual observed distances, not the desired trajectory provided to the controller by the DMP. I.e. using xsas an input to the potential field is not such a good idea. For instance, the desired endeff trajectory may be 10cm away from the obstacle, but the actual endeff only 5cm. It is the latter you will want to be taking into account.

Some ideas how to get started

So I guess this functionality must be included in the numerical integration loop, either inside the DMP, or also outside (so as not to have to touch the dmpbbo code itself). Here is the code snippet that I would use as a basis:

https://github.com/stulp/dmpbbo/blob/5f90d3c5d0a666c2294b0d71e41e8799f06416c5/demos/robot/robotExecuteDmp.cpp#L88-L95

You'd then apply the potential field as follows in the code snippet below. I hope this helps!

  MyOwnObstacleSimulator simulator;
  dmp->integrateStart(x, xd);
  for (int ii = 0; ii < n_time_steps; ii++) {
    dmp->stateAsPosVelAcc(x, xd, y_des, yd_des, ydd_des);
    simulator.integrateStep(dt, y_des, yd_des, ydd_des);

   // On a real robot this would be measured by joint encoders and forward kinematics, for instance
    y_endeff_observed = simulator.getEndEff() 
    // On a real robot, this would be extracted from a laser range finder, for instance
    y_obstacle_observed = simulator.getObstacle(); 
    // Compute the potential field and add it as an external forcing term to the integration
    ydd_external = my_potential_field(y_endeff_observed, y_obstacle_observed);

    // You'd have to implement a version of `integrateStep` that takes external accelerations into account
    // That should be straightforward because you can add it to the forcing term. 
    // Note that some care must be taken to distinguish when converting `ydd_external`
    //  (accelerations in the real world) to `xd` (in the DMP state).
    dmp->integrateStep(dt, x, x, xd, ydd_external);
    // The advantage of the above is that the DMP code is now agnostic where the acceleration came from,
    // i.e. obstacle avoidance, force fields, it can be anything. 
  }

Thus:

Implement dmp::integrateStep(dt, x, x, xd, ydd_external) in Dmp.cpp
- Would be great if you make a pull request, then everyone profits!
Implement the obstacle potential field part in your own code.

Side remark

It is more of a bug than a feature that the C++ implementation includes the analytical solution. The only reason to use the C++ implementation is if you want to run dmp integration in a real-time loop on a robot. But the analytical solution will never be in the real-time loop of the robot, because it computes an entire trajectory off-line, before sending any pos/vel/acc commands to the robot.

The "bug" arises from the fact that I implemented the library in C++ first, and ported it to Python later. Since the C++ code still works, I might as well leave it in.

Ferdinand50 commented 7 months ago

Hello again and thank you for the detailed answer! Sorry for my late reply. I implemented a python version which uses the analytical solution for prototyping. It achieves okay results but as you suggest an "integrateStep in C++ with potential fields" will achieve better results. Unfortunately, my thesis is finished. However, I suggest my successor to continue with your suggestion. This should then lead to a contribution to your work. I will close this issue for now. Thank you very much!

stulp / dmpbbo