[Python] set{Start,End}State correctly project from the new state and not the original one

src/hpp/corbaserver/rbprm/rbprmfullbody.py

@@ -325,7 +325,7 @@ class FullBody(Robot):
         num_max_sample = 0
         for (limbName, normal) in zip(contacts, normals):
             p = cl.getEffectorPosition(limbName, configuration)[0]
-            cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
+            sId = cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
         return cl.setStartStateId(sId)
 
     # # Initialize the goal configuration of the path interpolation
@@ -342,7 +342,7 @@ class FullBody(Robot):
         num_max_sample = 0
         for (limbName, normal) in zip(contacts, normals):
             p = cl.getEffectorPosition(limbName, configuration)[0]
-            cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
+            sId = cl.addNewContact(sId, limbName, p, normal, num_max_sample, False, [0, 0, 0, 0])
         return cl.setEndStateId(sId)
 
     # # Initialize the first state of the path interpolation