From f490eff867bfe7d532aac2fd03c783a389d4a5ca Mon Sep 17 00:00:00 2001 From: Rachel Date: Thu, 11 Aug 2016 17:03:46 -0400 Subject: [PATCH 1/5] First pass at util function for concatenating trajectories --- src/prpy/util.py | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/src/prpy/util.py b/src/prpy/util.py index 93ac3a5..be6f53f 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -2129,3 +2129,38 @@ def GetPointFrom(focus): raise ValueError('Focus of the point is an unknown object') return coord + +def concatenateTrajectories(robot, traj_list): + """ + Given a list of trajectories for a single manipulator, + concatenate them into a single trajectory + + @param robot + @param traj_list List of consective trajectories for a given manipulator + @return base_traj Combined trajectory + """ + + base_traj = traj_list[0] + base_cspec = base_traj.GetConfigurationSpecification() + traj_indices = base_cspec.ExtractUsedIndices(robot) + + offset = base_traj.GetNumWaypoints() + for i in xrange(1, len(traj_list)): + traj = traj_list[i] + cspec = traj.GetConfigurationSpecification() + sub = numpy.subtract(cspec.ExtractUsedIndices(robot), traj_indices) + if sub.sum() != 0: + raise ValueError('Trajectories are not on the same manipulator.') + + epsilon = 0.01 + previous_point = base_traj.GetWaypoint(base_traj.GetNumWaypoints()-1) + next_point = traj.GetWaypoint(0) + #TODO is there a better way to check for this? + if numpy.subtract(previous_point, next_point).sum() < epsilon: + raise ValueError('Trajectories are not consecutive.') + + for j in xrange(traj.GetNumWaypoints()): + waypoint = traj.GetWaypoint(j) + base_traj.Insert(offset, waypoint) + offset += 1 + return base_traj From 8c71662cdde05c3da0c7ddf0926aa214e7d3c4ac Mon Sep 17 00:00:00 2001 From: Rachel Date: Mon, 29 Aug 2016 18:02:52 -0400 Subject: [PATCH 2/5] Concatenating trajectories, check for robot and tags --- src/prpy/util.py | 40 ++++++++++++++++++++++++++-------------- 1 file changed, 26 insertions(+), 14 deletions(-) diff --git a/src/prpy/util.py b/src/prpy/util.py index be6f53f..3aae7a5 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -2130,37 +2130,49 @@ def GetPointFrom(focus): return coord -def concatenateTrajectories(robot, traj_list): +def concatenateTrajectories(traj_list): """ Given a list of trajectories for a single manipulator, concatenate them into a single trajectory - @param robot @param traj_list List of consective trajectories for a given manipulator @return base_traj Combined trajectory """ + from planning.base import Tags + + if len(traj_list) == 0: + raise ValueError('Trajectory list is empty') base_traj = traj_list[0] base_cspec = base_traj.GetConfigurationSpecification() - traj_indices = base_cspec.ExtractUsedIndices(robot) + base_robot = base_cspec.ExtractUsedBodies(base_traj.GetEnv())[0] + traj_indices = GetTrajectoryIndices(base_traj) offset = base_traj.GetNumWaypoints() for i in xrange(1, len(traj_list)): - traj = traj_list[i] - cspec = traj.GetConfigurationSpecification() - sub = numpy.subtract(cspec.ExtractUsedIndices(robot), traj_indices) + traj_i = traj_list[i] + cspec_i = traj_i.GetConfigurationSpecification() + robot_i = cspec_i.ExtractUsedBodies(traj_i.GetEnv())[0] + if robot_i != base_robot: + raise ValueError('Trajectories are not on the same robot') + + sub = numpy.subtract(GetTrajectoryIndices(traj_i), traj_indices) if sub.sum() != 0: raise ValueError('Trajectories are not on the same manipulator.') - epsilon = 0.01 - previous_point = base_traj.GetWaypoint(base_traj.GetNumWaypoints()-1) - next_point = traj.GetWaypoint(0) - #TODO is there a better way to check for this? - if numpy.subtract(previous_point, next_point).sum() < epsilon: - raise ValueError('Trajectories are not consecutive.') + tags_i = GetTrajectoryTags(traj_i) + # If one segment is not smooth, the entire trajectory is not (?) + if 'smooth' in tags_i and not tags_i['smooth']: + SetTrajectoryTags(base_traj, {TAGS.SMOOTH: True}, append=True) + + # The entire trajectory gets tagged as constrained if one of + # the segments is constrained. + if 'constrained' in tags_i and tags_i['constrained']: + SetTrajectoryTags(base_traj, {TAGS.CONSTRAINED: True}, append=True) - for j in xrange(traj.GetNumWaypoints()): - waypoint = traj.GetWaypoint(j) + # Add trajectory in by inserting each waypoint + for j in xrange(traj_i.GetNumWaypoints()): + waypoint = traj_i.GetWaypoint(j) base_traj.Insert(offset, waypoint) offset += 1 return base_traj From 2bc1b2adcd8246a92670d3fb6ee2f16256a82b0c Mon Sep 17 00:00:00 2001 From: Rachel Date: Wed, 14 Sep 2016 14:13:33 -0400 Subject: [PATCH 3/5] Concatenating trajectory, check all flags and match cspecs --- src/prpy/util.py | 41 ++++++++++++++++++++++++++--------------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/src/prpy/util.py b/src/prpy/util.py index 3aae7a5..36a8167 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -2130,7 +2130,7 @@ def GetPointFrom(focus): return coord -def concatenateTrajectories(traj_list): +def ConcatenateTrajectories(traj_list): """ Given a list of trajectories for a single manipulator, concatenate them into a single trajectory @@ -2145,34 +2145,45 @@ def concatenateTrajectories(traj_list): base_traj = traj_list[0] base_cspec = base_traj.GetConfigurationSpecification() - base_robot = base_cspec.ExtractUsedBodies(base_traj.GetEnv())[0] - traj_indices = GetTrajectoryIndices(base_traj) + + smoothFlag = True + constrainedFlag = False + deterministicTrajectoryFlag = True + deterministicEndPointFlag = False offset = base_traj.GetNumWaypoints() for i in xrange(1, len(traj_list)): traj_i = traj_list[i] - cspec_i = traj_i.GetConfigurationSpecification() - robot_i = cspec_i.ExtractUsedBodies(traj_i.GetEnv())[0] - if robot_i != base_robot: - raise ValueError('Trajectories are not on the same robot') - sub = numpy.subtract(GetTrajectoryIndices(traj_i), traj_indices) - if sub.sum() != 0: - raise ValueError('Trajectories are not on the same manipulator.') + cspec_i = traj_i.GetConfigurationSpecification() + if base_cspec != cspec_i: + raise ValueError('The configuration specifications of the trajectory do not match') tags_i = GetTrajectoryTags(traj_i) - # If one segment is not smooth, the entire trajectory is not (?) + # Only True if all trajectories have this set if 'smooth' in tags_i and not tags_i['smooth']: - SetTrajectoryTags(base_traj, {TAGS.SMOOTH: True}, append=True) + smoothFlag = False - # The entire trajectory gets tagged as constrained if one of - # the segments is constrained. + # True if any trajectory has this set if 'constrained' in tags_i and tags_i['constrained']: - SetTrajectoryTags(base_traj, {TAGS.CONSTRAINED: True}, append=True) + constrainedFlag = True + + # Only True if all trajectories have this set + if 'deterministic' in tags_i and not tags_i['deterministic']: + deterministcTrajectoryFlag = False + + # True if the last trajectory has this set + if i == len(traj_list)-1 and 'deterministic_endpoint' in tags_i and tags_i['determinstic_endpoint']: + deterministicEndPointFlag = True # Add trajectory in by inserting each waypoint for j in xrange(traj_i.GetNumWaypoints()): waypoint = traj_i.GetWaypoint(j) base_traj.Insert(offset, waypoint) offset += 1 + + flagSettings = {TAGS.SMOOTH: smoothFlag, TAGS.CONSTRAINED: constrainedFlag, + TAGS.DETERMINISTIC_TRAJECTORY: deterministicTrajectoryFlag, + TAGS.DETERMINISTIC_ENDPOINT: deterministicEndPointFlag} + SetTrajectoryTags(base_traj, flagSettings, append=True) return base_traj From 1bd969a5d4f154b57a236865e57e85ba9542490b Mon Sep 17 00:00:00 2001 From: Rachel Date: Thu, 26 Jan 2017 10:55:56 -0500 Subject: [PATCH 4/5] Concatenate trajectories by starting with a blank one, add unit test --- src/prpy/util.py | 54 +++++++++++++++++++++++++++++----------------- tests/test_util.py | 21 ++++++++++++++++++ 2 files changed, 55 insertions(+), 20 deletions(-) diff --git a/src/prpy/util.py b/src/prpy/util.py index 36a8167..50b5389 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -2130,7 +2130,7 @@ def GetPointFrom(focus): return coord -def ConcatenateTrajectories(traj_list): +def ConcatenateTrajectories(robot, traj_list): """ Given a list of trajectories for a single manipulator, concatenate them into a single trajectory @@ -2143,47 +2143,61 @@ def ConcatenateTrajectories(traj_list): if len(traj_list) == 0: raise ValueError('Trajectory list is empty') - base_traj = traj_list[0] - base_cspec = base_traj.GetConfigurationSpecification() + env = robot.GetEnv() + + # Create an empty trajectory to populate + base_traj = openravepy.RaveCreateTrajectory(env, '') + base_cspec = robot.GetActiveConfigurationSpecification('linear') + base_traj.Init(base_cspec) + #base_cspec = base_traj.GetConfigurationSpecification() smoothFlag = True constrainedFlag = False deterministicTrajectoryFlag = True deterministicEndPointFlag = False - offset = base_traj.GetNumWaypoints() - for i in xrange(1, len(traj_list)): - traj_i = traj_list[i] - - cspec_i = traj_i.GetConfigurationSpecification() + offset = 0 + for traj in traj_list: + cspec_i = traj.GetConfigurationSpecification() if base_cspec != cspec_i: raise ValueError('The configuration specifications of the trajectory do not match') - tags_i = GetTrajectoryTags(traj_i) + tags = GetTrajectoryTags(traj) # Only True if all trajectories have this set - if 'smooth' in tags_i and not tags_i['smooth']: + if 'smooth' in tags and not tags['smooth']: smoothFlag = False # True if any trajectory has this set - if 'constrained' in tags_i and tags_i['constrained']: + if 'constrained' in tags and tags['constrained']: constrainedFlag = True # Only True if all trajectories have this set - if 'deterministic' in tags_i and not tags_i['deterministic']: + if 'deterministic' in tags and not tags['deterministic']: deterministcTrajectoryFlag = False - # True if the last trajectory has this set - if i == len(traj_list)-1 and 'deterministic_endpoint' in tags_i and tags_i['determinstic_endpoint']: - deterministicEndPointFlag = True + # Check that trajectory leds from previous, + # or if the first, start the trajectory + epsilon = 0.001 + if offset == 0: + base_traj.Insert(offset, traj.GetWaypoint(0)) + offset += 1 + else: + if sum(numpy.subtract(traj.GetWaypoint(0), base_traj.GetWaypoint(offset-1))) > epsilon: + raise ValueError('The trajectories are not consecutive.') # Add trajectory in by inserting each waypoint - for j in xrange(traj_i.GetNumWaypoints()): - waypoint = traj_i.GetWaypoint(j) + for j in xrange(1, traj.GetNumWaypoints()): + waypoint = traj.GetWaypoint(j) base_traj.Insert(offset, waypoint) offset += 1 - flagSettings = {TAGS.SMOOTH: smoothFlag, TAGS.CONSTRAINED: constrainedFlag, - TAGS.DETERMINISTIC_TRAJECTORY: deterministicTrajectoryFlag, - TAGS.DETERMINISTIC_ENDPOINT: deterministicEndPointFlag} + # True if the last trajectory has this set + last_tags = GetTrajectoryTags(traj_list[-1]) + if 'deterministic_endpoint' in last_tags and last_tags['determinstic_endpoint']: + deterministicEndPointFlag = True + + flagSettings = {Tags.SMOOTH: smoothFlag, Tags.CONSTRAINED: constrainedFlag, + Tags.DETERMINISTIC_TRAJECTORY: deterministicTrajectoryFlag, + Tags.DETERMINISTIC_ENDPOINT: deterministicEndPointFlag} SetTrajectoryTags(base_traj, flagSettings, append=True) return base_traj diff --git a/tests/test_util.py b/tests/test_util.py index a29be18..6b385bd 100644 --- a/tests/test_util.py +++ b/tests/test_util.py @@ -1088,6 +1088,27 @@ def test_GetMinDistanceBetweenTransformAndWorkspaceTraj(self): numpy.testing.assert_array_almost_equal(T_loc, expected_T_loc, decimal=7, \ err_msg=error, verbose=True) + def test_ConcatenateTrajectories(self): + q0 = self.robot.GetDOFValues(self.active_dof_indices).tolist() + q1 = [0.02, 0.01, 0.02, 0.01, 0.01, 0.01, 0.0] + q2 = [0.50, 0.30, 0.02, 0.01, 0.01, 0.01, 0.0] + traj1 = self.CreateTrajectory(q0, q1) + traj2 = self.CreateTrajectory(q1, q2) + combined_traj = prpy.util.ConcatenateTrajectories(self.robot, [traj1, traj2]) + + combined_waypoints = numpy.zeros((combined_traj.GetNumWaypoints(), 7)) + for j in xrange(combined_traj.GetNumWaypoints()): + combined_waypoints[j] = combined_traj.GetWaypoint(j) + all_waypoints = [q0, q1, q2] + + error = 'The waypoints on the trajectory' + ' do not match the individual waypoints' + numpy.testing.assert_array_almost_equal(combined_waypoints, + all_waypoints, + decimal=4, + err_msg=error, + verbose=True) + class Test_GetPointFrom(unittest.TestCase): """ From b167c0a033f17d9eb7594b297731a649efaaffc6 Mon Sep 17 00:00:00 2001 From: Rachel Date: Fri, 27 Jan 2017 13:30:52 -0500 Subject: [PATCH 5/5] Correct error message bug --- tests/test_util.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/test_util.py b/tests/test_util.py index 6b385bd..d34290c 100644 --- a/tests/test_util.py +++ b/tests/test_util.py @@ -1101,8 +1101,7 @@ def test_ConcatenateTrajectories(self): combined_waypoints[j] = combined_traj.GetWaypoint(j) all_waypoints = [q0, q1, q2] - error = 'The waypoints on the trajectory' - ' do not match the individual waypoints' + error = 'The waypoints on the trajectory do not match the individual waypoints' numpy.testing.assert_array_almost_equal(combined_waypoints, all_waypoints, decimal=4,