Better convergance, casadi and new collision avoidance

[ichumuh]

• casadi support has been added, which is an alternative to symengine
  • compiling is not necessary with casadi and function evaluation is as fast as symengine with llvm backend.
  • casadi uses automatic differentiation, which is better for our usecase. It has derivatives for functions like min, max or if. This means the hacked differentiable functions like diffable_min, that were needed with symengine, are not required anymore. You can still use them to support both backends.
  • with casadi giskard can handle a few hundred constraints at the same time, after which the qp solver becomes the new bottleneck
  • instead of importing symengine wrapper, do from giskardpy import symbolic_wrapper, you can look up the functions in casadi_wrapper.py or symengine_wrapper.py
  • use symbolic_wrapper.dot for matrix multiplication instead of *
• trajectory generation was changed
  • the old method multiplied the velocity command with the sample period. This created trajectories that converge very slowly
  • the new method adds the velocity command unmodified to the current joint state. This means we have full control over the convergence behavior in the constraint definition.
  • If you just want to fix old constraints, do it like this: if the old limit is l and the max velocity is m (you should always use one) and the sample period is t, than the limit should now be min(l, m*t). This creates the same trajectory at max velocity, but removes the slow convergence when close to the goal. This shortens short trajectories in particular.
• collision avoidance
  • self and external collision avoidance use different constraints now.
  • external collision avoidance can now push away from the n closest objects, set through a parameter. This creates n constraints for each movable joint.
  • self collision avoidance is one constraint for each pair of links in the self collision matrix.
• qp solver
  • removes entries from the matrix with weight = 0, this greatly speeds up planning with the new collision avoidance, because there are rarely n close objects. So if a constraint is inactive to some part of a trajectory, make sure its weight gets set to 0, to speed up planning.
• a lot of runtime optimization, such that planning with a few hundred constraints is now about as fast as planning with an order of magnitude fewer constraints before.