modified so you do not need to retime twice

Author: Shervin Javdani

src/prpy/base/wam.py

@@ -304,18 +304,8 @@ def MoveUntilTouch(manipulator, direction, distance, max_distance=float('+inf'),
                 delta_t = 0.01
 
                 traj_config_spec = traj.GetConfigurationSpecification()
-                traj_angle_group = traj_config_spec.GetGroupFromName('joint_values')
-                path_config_spec = openravepy.ConfigurationSpecification()
-#                path_config_spec.AddDeltaTimeGroup()
-                path_config_spec.AddGroup(traj_angle_group.name, traj_angle_group.dof, 'linear')
-                #path_config_spec.AddGroup(traj_angle_group)
-#                path_config_spec.AddDerivativeGroups(1, False);
-#                path_config_spec.AddDerivativeGroups(2, False);
-#                path_config_spec.AddGroup('owd_blend_radius', 1, 'next')
-                path_config_spec.ResetGroupOffsets()
-
                 new_traj = openravepy.RaveCreateTrajectory(manipulator.GetRobot().GetEnv(), '')
-                new_traj.Init(path_config_spec)
+                new_traj.Init(traj_config_spec)
 
                 for (ignore_col_with, oldstate) in zip(ignore_collisions, ignore_col_obj_oldstate):
                     ignore_col_with.Enable(oldstate)
@@ -325,24 +315,29 @@ def MoveUntilTouch(manipulator, direction, distance, max_distance=float('+inf'),
                     waypoint_ind = 0
                     for t in numpy.arange(0, traj_duration, delta_t):
                         traj_sample = traj.Sample(t)
-                        print traj_sample
 
                         waypoint = traj_config_spec.ExtractJointValues(traj_sample, manipulator.GetRobot(), manipulator.GetArmIndices())
                         manipulator.SetDOFValues(waypoint)
-                        #if manipulator.GetRobot().GetEnv().CheckCollision(manipulator.GetRobot()):
+
+                        # Check collision with each body on the robot
                         for body in manipulator.GetRobot().GetEnv().GetBodies():
                             if manipulator.GetRobot().GetEnv().CheckCollision(manipulator.GetRobot(), body):
                                 collided_with_obj = True
                                 break
                         if collided_with_obj:
                             break
                         else:
-                            #waypoint = numpy.append(waypoint,t)
-                            new_traj.Insert(int(waypoint_ind), waypoint, path_config_spec)
+                            #set timing on new sampled waypoint
+                            if waypoint_ind == 0:
+                                traj_config_spec.InsertDeltaTime(traj_sample, 0.)
+                            else:
+                                traj_config_spec.InsertDeltaTime(traj_sample, delta_t)
+                            
+                            new_traj.Insert(int(waypoint_ind), traj_sample)
                             waypoint_ind += 1
 
-                #manipulator.GetRobot().ExecuteTrajectory(new_traj, execute = True, retime=True, blend=True)
-                manipulator.GetRobot().ExecutePath(new_traj)
+
+                manipulator.GetRobot().ExecuteTrajectory(new_traj)
 
             return collided_with_obj
         # Trajectory is aborted by OWD because we felt a force.