def __init__(self, init_path, bfs=10):
assert isinstance(path, Path)
self.init_path = init_path
self._dmp = DMPs_discrete(dmps=7, bfs=bfs)
self._dmp.imitate_path(
self._path_to_y_des(init_path, self.NUMBER_STABLE_STATES))
if self.init_path.header.frame_id == '':
self.init_path.header.frame_id = self.init_path.poses[
0].header.frame_id
def __init__(self, init_path, bfs=10):
assert isinstance(path, Path)
self.init_path = init_path
self._dmp = DMPs_discrete(dmps=7, bfs=bfs)
self._dmp.imitate_path(self._path_to_y_des(init_path, self.NUMBER_STABLE_STATES))
if self.init_path.header.frame_id == '':
self.init_path.header.frame_id = self.init_path.poses[0].header.frame_id
class DiscreteTaskSpaceTrajectory(object):
NUMBER_STABLE_STATES = 50
def __init__(self, init_path, bfs=10):
assert isinstance(path, Path)
self.init_path = init_path
self._dmp = DMPs_discrete(dmps=7, bfs=bfs)
self._dmp.imitate_path(
self._path_to_y_des(init_path, self.NUMBER_STABLE_STATES))
if self.init_path.header.frame_id == '':
self.init_path.header.frame_id = self.init_path.poses[
0].header.frame_id
def _path_to_y_des(self, path, nss):
y_des = []
for pose_s in path.poses:
assert pose_s.header.frame_id == self.init_path.header.frame_id
pose = [
val for sublist in pose_to_list(pose_s) for val in sublist
] # flatten to [x, y, z, x, y, z, w]
y_des.append(pose)
# Repeat the last point (stable state) n times to avoid brutal cuts due to asymptotic approach
for n in range(nss):
y_des.append(y_des[-1])
return np.array(y_des).transpose()
def rollout(self, goal):
assert isinstance(goal, PoseStamped)
self._dmp.goal = [
val for sublist in pose_to_list(goal) for val in sublist
]
y_track, dy_track, ddy_track = self._dmp.rollout()
path = Path()
for y in y_track:
path.poses.append(
list_to_pose([[y[0], y[1], y[2]], [y[3], y[4], y[5], y[6]]],
frame_id=self.init_path.header.frame_id))
path.header.stamp = rospy.Time.now()
path.header.frame_id = self.init_path.header.frame_id
return path
class DiscreteTaskSpaceTrajectory(object):
NUMBER_STABLE_STATES = 50
def __init__(self, init_path, bfs=10):
assert isinstance(path, Path)
self.init_path = init_path
self._dmp = DMPs_discrete(dmps=7, bfs=bfs)
self._dmp.imitate_path(self._path_to_y_des(init_path, self.NUMBER_STABLE_STATES))
if self.init_path.header.frame_id == '':
self.init_path.header.frame_id = self.init_path.poses[0].header.frame_id
def _path_to_y_des(self, path, nss):
y_des = []
for pose_s in path.poses:
assert pose_s.header.frame_id == self.init_path.header.frame_id
pose = [val for sublist in pose_to_list(pose_s) for val in sublist] # flatten to [x, y, z, x, y, z, w]
y_des.append(pose)
# Repeat the last point (stable state) n times to avoid brutal cuts due to asymptotic approach
for n in range(nss):
y_des.append(y_des[-1])
return np.array(y_des).transpose()
def rollout(self, goal):
assert isinstance(goal, PoseStamped)
self._dmp.goal = [val for sublist in pose_to_list(goal) for val in sublist]
y_track, dy_track, ddy_track = self._dmp.rollout()
path = Path()
for y in y_track:
path.poses.append(list_to_pose([[y[0], y[1], y[2]], [y[3], y[4], y[5], y[6]]],
frame_id=self.init_path.header.frame_id))
path.header.stamp = rospy.Time.now()
path.header.frame_id = self.init_path.header.frame_id
return path
