urdf model and ros code for planning trajectories of the icub arms, head and torso