Probabilistic Movement Primitives Part 4: Conditioning
The modulation of via-points, final positions, and velocities is carried out using conditioning so that MP can adapt to new situations. In order to condition the MP to reach a certain state
where state vector
where
Let's code
via_points = [(0.2, .02),(0.6, .06)] # (time t, state y*)
new_mean_W = mean_W
new_sigma_W = sigma_W
traj_point_sigma=1e-10 # observation variance for the via points
for viapoint in via_points:
plt.scatter(viapoint[0], viapoint[1], marker='o', s=100)
# basis functions at observed time points
PhiT, _= generate_basis_function(phase_z=get_phase_from_time(viapoint[0])
, phase_zd=phase_speed)
aux = traj_point_sigma + np.dot(np.dot(PhiT.T, new_sigma_W), PhiT)
new_mean_W = new_mean_W + np.dot(np.dot(new_sigma_W, PhiT) * 1 / aux,
(viapoint[1] - np.dot(PhiT.T, new_mean_W)))
new_sigma_W = new_sigma_W - np.dot(np.dot(new_sigma_W, PhiT) * 1 / aux,
np.dot(PhiT.T, new_sigma_W))
# get mean and variance for the new trajectories
mean_of_sample_traj = np.dot(Phi.T, new_mean_W)
sigma_of_sample_traj = np.dot(np.dot(Phi.T, new_sigma_W), Phi)
The sampled distribution is shown in blue. The distribution in green is sampled after adding two via-points. The red one is generated after modulating the final position.
References:
1. Paraschos, Alexandros, et al. "Using probabilistic movement primitives in robotics." Autonomous Robots 42.3 (2018): 529-551.
Comments
Post a Comment