cleaning
added jacobian and samples
added joint constraints