Add add_cost method in the inverse kinematics
#130
Merged
add_cost method in the inverse kinematics
#130