def __init__(self,
ik,
execution_time=1.0,
dt=0.01,
n_features=50,
configuration_file=None):
self.ik = ik
self.csdmp = CartesianDMPBehavior(execution_time, dt, n_features,
configuration_file)
self.csdmp.init(7, 7)
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()
class IKCartesianDMPBehavior(BlackBoxBehavior):
"""Cartesian Space Dynamical Movement Primitive using Inverse Kinematics.
This is a cartesian space dmp which uses the invesrse kinematics to
position an end-effector.
Parameters
----------
ik : object
Inverse kinematics solver
execution_time : float, optional (default: 1)
Execution time of the DMP in seconds.
dt : float, optional (default: 0.01)
Time between successive steps in seconds.
n_features : int, optional (default: 50)
Number of RBF features for each dimension of the DMP.
configuration_file : string, optional (default: None)
Name of a configuration file that should be used to initialize the DMP.
If it is set all other arguments will be ignored.
"""
def __init__(self, ik, execution_time=1.0, dt=0.01, n_features=50,
configuration_file=None):
self.ik = ik
self.csdmp = CartesianDMPBehavior(execution_time, dt, n_features,
configuration_file)
self.csdmp.init(7, 7)
def init(self, n_inputs, n_outputs):
"""Initialize the behavior.
Parameters
----------
n_inputs : int
number of inputs
n_outputs : int
number of outputs
"""
self.n_joints = self.ik.get_n_joints()
if self.n_joints != n_inputs:
raise ValueError("Expected %d inputs, got %d"
% (self.n_joints, n_inputs))
if self.n_joints != n_outputs:
raise ValueError("Expected %d inputs, got %d"
% (self.n_joints, n_inputs))
self.q = np.empty(self.n_joints)
self.q[:] = np.nan
self.p = np.empty(7)
self.p[:] = np.nan
self.success = None
def reset(self):
self.csdmp.reset()
self.q[:] = 0.0
def set_inputs(self, inputs):
self.q[:] = inputs[:]
def can_step(self):
return self.csdmp.can_step()
def step(self):
"""Compute forward kin., execute DMP step, perform inverse kin."""
self.ik.jnt_to_cart(self.q, self.p)
self.csdmp.set_inputs(self.p)
self.csdmp.step()
self.csdmp.get_outputs(self.p)
self.success = self.ik.cart_to_jnt(self.p, self.q)
def get_outputs(self, outputs):
if self.success is not None and not self.success:
outputs[:] = np.nan
else:
outputs[:] = self.q[:]
def get_n_params(self):
return self.csdmp.get_n_params()
def get_params(self):
return self.csdmp.get_params()
def set_params(self, params):
self.csdmp.set_params(params)
def set_meta_parameters(self, keys, values):
self.csdmp.set_meta_parameters(keys, values)
def trajectory(self):
return self.csdmp.trajectory()
for i in range(numTrunkJoints):
print(currentQ[i])
x = []
y = []
z = []
frames = []
qx = []
qy = []
qz = []
qw = []
model = 'model_dmp_approach1.yaml'
config = 'config_dmp_aproach1.yaml'
dmp = CartesianDMPBehavior(configuration_file=model)
dmp.init(7, 7)
dmp.load_config(config)
x0 = np.array([0.12432408, -0.29592085, 0.68519397])
q0 = np.array([-0.57013293, 0.11130143, -0.47013887, 0.66447715])
q0 = q0 / np.linalg.norm(q0)
g = np.array([0.45566228, -0.11897716, 0.9401124])
qg = np.array([-0.2427928, -0.29033049, -0.44532354, 0.81144736])
g = g / np.linalg.norm(g)
execution_time = 1.0
dmp.set_meta_parameters(["x0", "g", "q0", "qg", "execution_time"],
[x0, g, q0, qg, execution_time])
#Get the new generated trajectory from dmp.
trajectory = dmp.trajectory()
def plot_trajectory(ax, X, color="k"):
ax.plot(X[:, 0], X[:, 1], X[:, 2], lw=2, color=color)
for x in X[50:-50:50]:
plot_pose(ax, x, s=0.03, lw=2, alpha=0.5)
plot_pose(ax, X[0], s=0.05, lw=3)
plot_pose(ax, X[-1], s=0.05, lw=3)
try:
dirname = os.path.dirname(os.path.realpath(__file__))
except NameError:
dirname = "."
model = os.path.join(dirname, "cart_dmp_model.yaml")
config = os.path.join(dirname, "cart_dmp_config.yaml")
dmp = CartesianDMPBehavior(configuration_file=model)
dmp.init(7, 7)
dmp.load_config(config)
plt.figure(figsize=(18, 10))
ax = plt.subplot(221, projection="3d", aspect="equal")
plt.setp(ax, xlim=(0.3, 0.6), ylim=(-0.15, 0.15), zlim=(0.7, 1.0),
xlabel="X", ylabel="Y", zlabel="Z")
X = dmp.trajectory()
plot_trajectory(ax, X, "k")
ax = plt.subplot(223)
ax.plot(X[:, 0], label="X", c="r")
ax.plot(X[:, 1], label="Y", c="g")
ax.plot(X[:, 2], label="Z", c="b")
ax.legend(loc="upper right")
float(row[3]),
float(row[4]),
float(row[5]),
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")