fix a bug in SingleBodyDynamicsFullKinematicsPlanner for gradient computation in comMatch function
Fix all gradient issue