Feature/concatenate trajs #354
Conversation
| @@ -2129,3 +2129,50 @@ def GetPointFrom(focus): | |||
| raise ValueError('Focus of the point is an unknown object') | |||
|
|
|||
| return coord | |||
|
|
|||
| def concatenateTrajectories(traj_list): | |||
There was a problem hiding this comment.
We follow OpenRAVE's naming convention by using PascalCase instead of camelCase.
|
I posted a few comments. The implementation should actually become quite a bit simpler after applying those changes. @rachelholladay: Could you take a look? |
|
@mkoval I made the changes you requested. The problem I notice (which I didn't before) is that this doesn't check the flags of the first trajectory in the list. The two ways I thought to fix this were
Would one be preferred over the other? Or some other option I haven't considered? |
| 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']: |
There was a problem hiding this comment.
Wouldn't it be cleaner to do this outside of the loop?
|
@rachelholladay Thanks for this PR! This concatenation can be further postprocessed to smoothen and shortcut a sequence of trajectories, which would be useful addition. As for the question you asked, I think option 2 (initializing with an empty base trajectory) would be cleaner. |
|
@gilwoolee I switched it to using a blank trajectory and even added a unit test. I guess I'd like to have a function argument for whether it should be smoothed or not. What do you think? |
|
|
||
| # Create an empty trajectory to populate | ||
| base_traj = openravepy.RaveCreateTrajectory(env, '') | ||
| base_cspec = robot.GetActiveConfigurationSpecification('linear') |
There was a problem hiding this comment.
Why do we need this to be linear? Also, why are we requiring that the trajectories have to be on the current active dofs? Shouldn't it just be the spec of the first trajectory?
| base_traj = openravepy.RaveCreateTrajectory(env, '') | ||
| base_cspec = robot.GetActiveConfigurationSpecification('linear') | ||
| base_traj.Init(base_cspec) | ||
| #base_cspec = base_traj.GetConfigurationSpecification() |
| tags = GetTrajectoryTags(traj) | ||
| # Only True if all trajectories have this set | ||
| if 'smooth' in tags and not tags['smooth']: | ||
| smoothFlag = False |
There was a problem hiding this comment.
This would be skipped if smooth is not in tags. Is smooth by default true?
| constrainedFlag = True | ||
|
|
||
| # Only True if all trajectories have this set | ||
| if 'deterministic' in tags and not tags['deterministic']: |
| base_traj.Insert(offset, traj.GetWaypoint(0)) | ||
| offset += 1 | ||
| else: | ||
| if sum(numpy.subtract(traj.GetWaypoint(0), base_traj.GetWaypoint(offset-1))) > epsilon: |
There was a problem hiding this comment.
I think we should use numpy.linalg.norm here.
| decimal=4, | ||
| err_msg=error, | ||
| verbose=True) | ||
|
|
There was a problem hiding this comment.
Can we add tests for checking tags? E.g. when one of the tags along the trajectory is not smooth, etc.
Util functionality to concatenate a list of trajectories into one trajectory by appending them onto each other.
Requested by @gilwoolee