def ObjFunc(x, qstart, qend, ndof, nwaypoints, nsamples, weights, vmax, amax,
trajref, robot):
traj = MakeTraj(x, qstart, qend, ndof, nwaypoints, vmax, amax)
dpos, dvel, dacc = Trajectory.Diff3(traj, trajref, nsamples, robot)
cpos, cvel, cacc, cdur = weights
cost = cpos * dpos * dpos + cvel * dvel * dvel + cacc * dacc * dacc + cdur * (
traj.duration - trajref.duration)
print cost, cpos * dpos * dpos, cvel * dvel * dvel, cacc * dacc * dacc, cdur * (
traj.duration - trajref.duration)
return cost
