def _initial_cart_params(ik):
joint_dmp = DMPBehavior(execution_time, dt)
n_joints = ik.get_n_joints()
joint_dmp.init(n_joints * 3, n_joints * 3)
joint_dmp.set_meta_parameters(["x0", "g"], [x0, g])
Q = joint_dmp.trajectory()[0]
P = np.empty((Q.shape[0], 7))
for t in range(Q.shape[0]):
ik.jnt_to_cart(Q[t], P[t])
cart_dmp = CartesianDMPBehavior(execution_time, dt)
cart_dmp.init(7, 7)
cart_dmp.imitate(P.T[:, :, np.newaxis])
return cart_dmp.get_params()
float(row[6]),
float(row[7])
]
trajectory.append(pose)
return np.array(trajectory).T
trajectory = getCartesianTrajectoryFromCsvFile(
'trajectories/graspcup1/test-right-arm-motion-smooth1-optimized.csv')
execution_time = 1.0
dt = execution_time / (trajectory.shape[1] - 1)
dmp = CartesianDMPBehavior(execution_time, dt, n_features=15)
dmp.init(7, 7)
print(trajectory.shape)
dmp.imitate(trajectory, 0.0, False)
x0 = trajectory[:3, 0]
q0 = trajectory[3:, 0]
g = np.array(trajectory[:3, trajectory.shape[1] - 1], copy=True)
qg = trajectory[3:, trajectory.shape[1] - 1]
print(x0)
dmp.set_meta_parameters(["x0", "g", "q0", "qg", "execution_time"],
[x0, g, q0, qg, execution_time])
X = dmp.trajectory()
plt.figure(figsize=(18, 10))
ax = plt.subplot(221, projection="3d", aspect="auto")
plt.setp(ax,
xlim=(-0.5, 0.5),
ylim=(-0.5, 0.0),