class DMPs(object): """Implementation of Dynamic Motor Primitives, as described in Dr. Stefan Schaal's (2002) paper.""" def __init__(self, n_dmps, n_bfs, dt=.01, y0=0, goal=1, w=None, ay=None, by=None, axis_to_not_mirror=None, axis_not_to_scale=None, **kwargs): """ n_dmps int: number of dynamic motor primitives n_bfs int: number of basis functions per DMP dt float: timestep for simulation y0 list: initial state of DMPs goal list: goal state of DMPs w list: tunable parameters, control amplitude of basis functions ay int: gain on attractor term y dynamics by int: gain on attractor term y dynamics axis_to_not_mirror: an integer indicating the axis to not mirror. This is typically the height, for robotics applications. If we follow the usual convention for cartesian axis order then 0 is x, 1 is y, 2 is z and so on, so the height its usually 1 for 2D or 2 for 3D. axis_not_to_scale: an integer defining the axis for which scale will not be implemented. This is to avoid zero response when start and end positions are exactly the same. """ self.n_dmps = n_dmps self.n_bfs = n_bfs self.dt = dt if isinstance(y0, (int, float)): y0 = np.ones(self.n_dmps)*y0 self.y0 = y0 if isinstance(goal, (int, float)): goal = np.ones(self.n_dmps)*goal self.goal = goal if w is None: # default is f = 0 w = np.zeros((self.n_dmps, self.n_bfs)) self.w = w self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012 self.by = self.ay / 4. if by is None else by # Schaal 2012 # Here we set the flag for the axis that will not # be flipped (we will apply a different scalling factor # to this axis later) self.axis_to_not_mirror = axis_to_not_mirror # This is the axis for which we will not compute scale self.axis_not_to_scale = axis_not_to_scale # set up the CS self.cs = CanonicalSystem(dt=self.dt, **kwargs) self.timesteps = int(self.cs.run_time / self.dt) # We will need this for the special non-mirroring scale # factor later self.cs_rollout = self.cs.rollout() self.cs.reset_state() # set up the DMP system self.reset_state() def check_offset(self): """Check to see if initial position and goal are the same if they are, offset slightly so that the forcing term is not 0""" for d in range(self.n_dmps): if (self.y0[d] == self.goal[d]): self.goal[d] += 1e-4 def gen_front_term(self, x, dmp_num): raise NotImplementedError() def gen_goal(self, y_des): raise NotImplementedError() def gen_psi(self): raise NotImplementedError() def gen_weights(self, f_target): raise NotImplementedError() def imitate_path(self, y_des, plot=False): """Takes in a desired trajectory and generates the set of system parameters that best realize this path. y_des list/array: the desired trajectories of each DMP should be shaped [n_dmps, run_time] """ # set initial state and goal if y_des.ndim == 1: y_des = y_des.reshape(1, len(y_des)) self.y0 = y_des[:, 0].copy() self.y_des = y_des.copy() self.goal = self.gen_goal(y_des) self.check_offset() # generate function to interpolate the desired trajectory import scipy.interpolate path = np.zeros((self.n_dmps, self.timesteps)) x = np.linspace(0, self.cs.run_time, y_des.shape[1]) for d in range(self.n_dmps): path_gen = scipy.interpolate.interp1d(x, y_des[d]) for t in range(self.timesteps): path[d, t] = path_gen(t * self.dt) y_des = path # calculate velocity of y_des dy_des = np.diff(y_des) / self.dt # add zero to the beginning of every row dy_des = np.hstack((np.zeros((self.n_dmps, 1)), dy_des)) # calculate acceleration of y_des ddy_des = np.diff(dy_des) / self.dt # add zero to the beginning of every row ddy_des = np.hstack((np.zeros((self.n_dmps, 1)), ddy_des)) f_target = np.zeros((y_des.shape[1], self.n_dmps)) # find the force required to move along this trajectory for d in range(self.n_dmps): D = self.ay[d] # These are some auxiliary variables K = D * self.by[d] # added for clarity if d is self.axis_to_not_mirror: # non-mirroring scalling f_target[:, d] = (ddy_des[d] + D*dy_des[d])/K \ - (self.goal[d] - y_des[d]) \ + (self.goal[d] - self.y0[d]) * self.cs_rollout elif d is self.axis_not_to_scale: # without scale f_target[:, d] = (ddy_des[d] - K * (self.goal[d] - y_des[d]) + D*dy_des[d]) else: # original scalling f_target[:, d] = (ddy_des[d] - K * (self.goal[d] - y_des[d]) + D*dy_des[d])/(self.goal[d] - self.y0[d]) # efficiently generate weights to realize f_target self.gen_weights(f_target) if plot is True: # plot the basis function activations import matplotlib.pyplot as plt plt.figure() plt.subplot(211) psi_track = self.gen_psi(self.cs.rollout()) plt.plot(psi_track) plt.title('basis functions') # plot the desired forcing function vs approx plt.subplot(212) plt.plot(f_target[:,0]) plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt) plt.legend(['f_target', 'w*psi']) plt.title('DMP forcing function') plt.tight_layout() plt.show() self.reset_state() return y_des def rollout(self, timesteps=None, **kwargs): """Generate a system trial, no feedback is incorporated.""" self.reset_state() if timesteps is None: if 'tau' in kwargs: timesteps = int(self.timesteps / kwargs['tau']) else: timesteps = self.timesteps # set up tracking vectors y_track = np.zeros((timesteps, self.n_dmps)) dy_track = np.zeros((timesteps, self.n_dmps)) ddy_track = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): # run and record timestep y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs) return y_track, dy_track, ddy_track def reset_state(self): """Reset the system state""" self.y = self.y0.copy() self.dy = np.zeros(self.n_dmps) self.ddy = np.zeros(self.n_dmps) self.cs.reset_state() def step(self, tau=1.0, error=0.0, external_force=None): """Run the DMP system for a single timestep. tau float: scales the timestep increase tau to make the system execute faster error float: optional system feedback """ error_coupling = 1.0 / (1.0 + error) # run canonical system x = self.cs.step(tau=tau, error_coupling=error_coupling) # generate basis function activation psi = self.gen_psi(x) for d in range(self.n_dmps): # generate the forcing term f = (self.gen_front_term(x, d) * (np.dot(psi, self.w[d])) / np.sum(psi)) # DMP acceleration D = self.ay[d] K = D * self.by[d] if d is self.axis_to_not_mirror: self.ddy[d] = (K * (self.goal[d] - self.y[d]) \ - D * self.dy[d]/tau \ - K * (self.goal[d] - self.y0[d]) * x \ + K * f) * tau elif d is self.axis_not_to_scale: self.ddy[d] = (K * (self.goal[d] - self.y[d]) - D * self.dy[d]/tau + f) * tau else: self.ddy[d] = (K * (self.goal[d] - self.y[d]) - D * self.dy[d]/tau + f * (self.goal[d] - self.y0[d])) * tau if external_force is not None: self.ddy[d] += external_force[d] self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling self.y[d] += self.dy[d] * self.dt * error_coupling return self.y, self.dy, self.ddy
class DMPs(object): """Implementation of Dynamic Motor Primitives, as described in Dr. Stefan Schaal's (2002) paper.""" def __init__(self, n_dmps, n_bfs, dt=.001, y0=0, goal=1, w=None, ay=None, by=None, **kwargs): """ n_dmps int: number of dynamic motor primitives n_bfs int: number of basis functions per DMP dt float: timestep for simulation y0 list: initial state of DMPs goal list: goal state of DMPs w list: tunable parameters, control amplitude of basis functions ay int: gain on attractor term y dynamics by int: gain on attractor term y dynamics """ self.n_dmps = n_dmps self.n_bfs = n_bfs self.dt = dt if isinstance(y0, (int, float)): y0 = np.ones(self.n_dmps) * y0 self.y0 = y0 if isinstance(goal, (int, float)): goal = np.ones(self.n_dmps) * goal self.goal = goal if w is None: # default is f = 0 w = np.zeros((self.n_dmps, self.n_bfs)) self.w = w self.gain_a = np.ones(n_dmps) * 25. if ay is None else np.array( ay) # Schaal 2012 self.gain_b = self.gain_a / 4. if by is None else np.array( by) # Schaal 2012 np.sqrt(self.gain_a)*2 # set up the CS self.canonical_system = CanonicalSystem(dt=self.dt, **kwargs) self.timesteps = int(self.canonical_system.run_time / self.dt) # set up the DMP system self.reset_state() def check_offset(self): """Check to see if initial position and goal are the same if they are, offset slightly so that the forcing term is not 0""" for d in range(self.n_dmps): if (self.y0[d] == self.goal[d]): self.goal[d] += 1e-4 def gen_front_term(self, x, dmp_num): raise NotImplementedError() def gen_goal(self, y_des): raise NotImplementedError() def gen_psi(self): raise NotImplementedError() def gen_weights(self, f_target): raise NotImplementedError() def imitate_path(self, y_des, plot=False): """Takes in a desired trajectory and generates the set of system parameters that best realize this path. y_des list/array: the desired trajectories of each DMP should be shaped [n_dmps, run_time] """ # set initial state and goal if y_des.ndim == 1: y_des = y_des.reshape(1, len(y_des)) self.y0 = y_des[:, 0].copy().astype(np.float) self.y_des = y_des.copy() self.goal = self.gen_goal(y_des) self.check_offset() # generate function to interpolate the desired trajectory import scipy.interpolate print("self.timesteps", self.timesteps) path = np.zeros((self.n_dmps, self.timesteps)) x = np.linspace(0, self.canonical_system.run_time, y_des.shape[1]) #Normalizing the input trajectory in time (squashing it from [0-T] to [0-1]) for d in range(self.n_dmps): path_gen = scipy.interpolate.interp1d(x, y_des[d]) for t in range(self.timesteps): path[d, t] = path_gen(t * self.dt) y_des = path # calculate velocity of y_des dy_des = np.diff(y_des) / self.dt # add zero to the beginning of every row dy_des = np.hstack((np.zeros((self.n_dmps, 1)), dy_des)) # calculate acceleration of y_des ddy_des = np.diff(dy_des) / self.dt # add zero to the beginning of every row ddy_des = np.hstack((np.zeros((self.n_dmps, 1)), ddy_des)) f_target = np.zeros((y_des.shape[1], self.n_dmps)) # find the force required to move along this trajectory x_track = self.canonical_system.rollout( ) #(tau=1/(len(y_des[0])*self.dt)) print("track size", len(x_track)) for d in range(self.n_dmps): f_target[:, d] = ddy_des[d]/(self.gain_a[d]*self.gain_b[d]) \ - (float(self.goal[d]) - y_des[d]) \ + dy_des[d]/self.gain_b[d] \ + (float(self.goal[d]) - self.y0[d])*x_track #(ddy_des[d] - self.gain_a[d] * # (self.gain_b[d] * (float(self.goal[d]) - y_des[d]) - # dy_des[d])) # efficiently generate weights to realize f_target self.gen_weights(f_target) if plot is True: # plot the basis function activations import matplotlib.pyplot as plt plt.figure() plt.subplot(211) psi_track = self.gen_psi(self.canonical_system.rollout()) print(psi_track.shape) plt.plot(psi_track) plt.title('basis functions') # plot the desired forcing function vs approx plt.subplot(212) plt.plot(f_target[:, 0]) plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt) plt.legend(['f_target', 'w*psi']) plt.title('DMP forcing function') plt.tight_layout() plt.show() self.reset_state() return y_des def rollout(self, timesteps=None, **kwargs): """Generate a system trial, no feedback is incorporated.""" self.reset_state() if timesteps is None: if 'tau' in kwargs: print("kwargs['tau']", kwargs['tau']) timesteps = int(self.timesteps / kwargs['tau']) else: timesteps = self.timesteps print(timesteps, "timesteps") print("goal", self.goal) print("y0", self.y0) # set up tracking vectors y_track = np.zeros((timesteps, self.n_dmps)) dy_track = np.zeros((timesteps, self.n_dmps)) ddy_track = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): # run and record timestep y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs) print("dy_track", dy_track[t]) return y_track, dy_track, ddy_track def reset_state(self): """Reset the system state""" self.y = self.y0.copy() self.dy = np.zeros(self.n_dmps) self.ddy = np.zeros(self.n_dmps) self.canonical_system.reset_state() def step(self, tau=1.0, error=0.0, external_force=None): """Run the DMP system for a single timestep.0 tau float: scales the timestep increase tau to make the system execute faster error float: optional system feedback """ error_coupling = 1.0 / (1.0 + error) # run canonical system x = self.canonical_system.step(tau=tau, error_coupling=error_coupling) #print("x : ", x) # generate basis function activation psi = self.gen_psi(x) for d in range(self.n_dmps): # generate the forcing term #print("x : ",x) #print("d : ",d) #print("self.gen_front_term(x, d) : ",self.gen_front_term(x, d)) #print("psi : ",psi) #print("self.w[d] : ",self.w[d]) #print("(np.dot(psi, self.w[d])) : ",(np.dot(psi, self.w[d]))) #print("np.sum(psi) : ",np.sum(psi)) f = (x * (np.dot(psi, self.w[d])) / np.sum(psi)) #changed #print("f :",f) # DMP acceleration #print("self.gain_a[d] : ",self.gain_a[d]) #print("self.gain_b[d] : ", self.gain_b[d]) #print("tau : ",tau) self.ddy[d] = ( tau * tau * self.gain_a[d] * (self.gain_b[d] * (self.goal[d] - self.y[d] - (self.goal[d] - self.y0[d]) * x + f) - self.dy[d] / tau)) # #print("self.ddy[d] :",self.ddy[d]) if external_force is not None: self.ddy[d] += external_force[d] self.dy[d] += self.ddy[d] * self.dt * error_coupling #print(" self.dy[d] : ", self.dy[d]) self.y[d] += self.dy[d] * self.dt * error_coupling #print("self.y[d] :", self.y[d]) return self.y, self.dy, self.ddy
class DMPs(object): """Implementation of Dynamic Motor Primitives, as described in Dr. Stefan Schaal's (2002) paper.""" def __init__( self, n_dmps, n_bfs, dt=0.01, y0=0, goal=1, w=None, ay=None, by=None, **kwargs ): """ n_dmps int: number of dynamic motor primitives n_bfs int: number of basis functions per DMP dt float: timestep for simulation y0 list: initial state of DMPs goal list: goal state of DMPs w list: tunable parameters, control amplitude of basis functions ay int: gain on attractor term y dynamics by int: gain on attractor term y dynamics """ self.n_dmps = n_dmps self.n_bfs = n_bfs self.dt = dt if isinstance(y0, (int, float)): y0 = np.ones(self.n_dmps) * y0 self.y0 = y0 if isinstance(goal, (int, float)): goal = np.ones(self.n_dmps) * goal self.goal = goal if w is None: # default is f = 0 w = np.zeros((self.n_dmps, self.n_bfs)) self.w = w self.ay = np.ones(n_dmps) * 25.0 if ay is None else ay # Schaal 2012 self.by = self.ay / 4.0 if by is None else by # Schaal 2012 # set up the CS self.cs = CanonicalSystem(dt=self.dt, **kwargs) self.timesteps = int(self.cs.run_time / self.dt) # set up the DMP system self.reset_state() def check_offset(self): """Check to see if initial position and goal are the same if they are, offset slightly so that the forcing term is not 0""" for d in range(self.n_dmps): if abs(self.y0[d] - self.goal[d]) < 1e-4: self.goal[d] += 1e-4 def gen_front_term(self, x, dmp_num): raise NotImplementedError() def gen_goal(self, y_des): raise NotImplementedError() def gen_psi(self): raise NotImplementedError() def gen_weights(self, f_target): raise NotImplementedError() def imitate_path(self, y_des, plot=False): """Takes in a desired trajectory and generates the set of system parameters that best realize this path. y_des list/array: the desired trajectories of each DMP should be shaped [n_dmps, run_time] """ # set initial state and goal if y_des.ndim == 1: y_des = y_des.reshape(1, len(y_des)) self.y0 = y_des[:, 0].copy() self.y_des = y_des.copy() self.goal = self.gen_goal(y_des) # self.check_offset() # generate function to interpolate the desired trajectory import scipy.interpolate path = np.zeros((self.n_dmps, self.timesteps)) x = np.linspace(0, self.cs.run_time, y_des.shape[1]) for d in range(self.n_dmps): path_gen = scipy.interpolate.interp1d(x, y_des[d]) for t in range(self.timesteps): path[d, t] = path_gen(t * self.dt) y_des = path # calculate velocity of y_des with central differences dy_des = np.gradient(y_des, axis=1) / self.dt # calculate acceleration of y_des with central differences ddy_des = np.gradient(dy_des, axis=1) / self.dt f_target = np.zeros((y_des.shape[1], self.n_dmps)) # find the force required to move along this trajectory for d in range(self.n_dmps): f_target[:, d] = ddy_des[d] - self.ay[d] * ( self.by[d] * (self.goal[d] - y_des[d]) - dy_des[d] ) # efficiently generate weights to realize f_target self.gen_weights(f_target) if plot is True: # plot the basis function activations import matplotlib.pyplot as plt plt.figure() plt.subplot(211) psi_track = self.gen_psi(self.cs.rollout()) plt.plot(psi_track) plt.title("basis functions") # plot the desired forcing function vs approx for ii in range(self.n_dmps): plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii) plt.plot(f_target[:, ii], "--", label="f_target %i" % ii) for ii in range(self.n_dmps): plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii) print("w shape: ", self.w.shape) plt.plot( np.sum(psi_track * self.w[ii], axis=1) * self.dt, label="w*psi %i" % ii, ) plt.legend() plt.title("DMP forcing function") plt.tight_layout() plt.show() self.reset_state() return y_des def rollout(self, timesteps=None, custom_start = None, **kwargs): """Generate a system trial, no feedback is incorporated.""" self.reset_state() if custom_start is not None: self.y = np.asarray(custom_start).copy() assert (self.y.shape == self.dy.shape, "Dimensions of custom goal \ provided does not match the dimensions of the DMP") if timesteps is None: if "tau" in kwargs: timesteps = int(self.timesteps / kwargs["tau"]) else: timesteps = self.timesteps # set up tracking vectors y_track = np.zeros((timesteps, self.n_dmps)) dy_track = np.zeros((timesteps, self.n_dmps)) ddy_track = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): # run and record timestep y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs) return y_track, dy_track, ddy_track def reset_state(self): """Reset the system state""" self.y = self.y0.copy() self.dy = np.zeros(self.n_dmps) self.ddy = np.zeros(self.n_dmps) self.cs.reset_state() def step(self, tau=1.0, error=0.0, external_force=None): """Run the DMP system for a single timestep. tau float: scales the timestep increase tau to make the system execute faster error float: optional system feedback """ error_coupling = 1.0 / (1.0 + error) # run canonical system x = self.cs.step(tau=tau, error_coupling=error_coupling) # generate basis function activation psi = self.gen_psi(x) for d in range(self.n_dmps): # generate the forcing term f = self.gen_front_term(x, d) * (np.dot(psi, self.w[d])) / np.sum(psi) # DMP acceleration self.ddy[d] = ( self.ay[d] * (self.by[d] * (self.goal[d] - self.y[d]) - self.dy[d]) + f ) if external_force is not None: self.ddy[d] += external_force[d] self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling self.y[d] += self.dy[d] * tau * self.dt * error_coupling return self.y, self.dy, self.ddy
class DMPs(object): """Implementation of Dynamic Motor Primitives, as described in Dr. Stefan Schaal's (2002) paper.""" def __init__(self, n_dmps, n_bfs, dt=.01, y0=0, goal=1, w=None, ay=None, by=None, **kwargs): """ n_dmps int: number of dynamic motor primitives n_bfs int: number of basis functions per DMP dt float: timestep for simulation y0 list: initial state of DMPs goal list: goal state of DMPs w list: tunable parameters, control amplitude of basis functions ay int: gain on attractor term y dynamics by int: gain on attractor term y dynamics """ self.n_dmps = n_dmps self.n_bfs = n_bfs self.dt = dt if isinstance(y0, (int, float)): y0 = np.ones(self.n_dmps) * y0 self.y0 = y0 if isinstance(goal, (int, float)): goal = np.ones(self.n_dmps) * goal self.goal = goal if w is None: # default is f = 0 w = np.zeros((self.n_dmps, self.n_bfs)) self.w = w self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012 self.by = self.ay / 4. if by is None else by # Schaal 2012 # set up the CS self.cs = CanonicalSystem(dt=self.dt, **kwargs) self.timesteps = int(self.cs.run_time / self.dt) # set up the DMP system self.reset_state() def check_offset(self): """Check to see if initial position and goal are the same if they are, offset slightly so that the forcing term is not 0""" for d in range(self.n_dmps): if (self.y0[d] == self.goal[d]): self.goal[d] += 1e-4 def gen_front_term(self, x, dmp_num): raise NotImplementedError() def gen_goal(self, y_des): raise NotImplementedError() def gen_psi(self): raise NotImplementedError() def gen_weights(self, f_target): raise NotImplementedError() def imitate_path(self, y_des, plot=False): """Takes in a desired trajectory and generates the set of system parameters that best realize this path. y_des list/array: the desired trajectories of each DMP should be shaped [n_dmps, run_time] modified by lagrassa to take in a list of y_des of form [n_trajs, n_dmps, run_time] """ # set initial state and goal self.n_trajs = y_des.shape[0] if y_des.ndim == 1: y_des = y_des.reshape(1, len(y_des)) self.y0 = y_des[:, :, 0].copy() self.y_des = y_des.copy() self.goal = self.gen_goal(y_des) #self.check_offset() #disabling for multi, hopefully our starts won't be too close to the goal # generate function to interpolate the desired trajectory import scipy.interpolate path = np.zeros((self.n_trajs, self.n_dmps, self.timesteps)) x = np.linspace(0, self.cs.run_time, y_des.shape[-1]) for d in range(self.n_dmps): path_gen = scipy.interpolate.interp1d(x, y_des[:, d, :]) for t in range(self.timesteps): path[:, d, t] = path_gen(t * self.dt) y_des = path # calculate velocity of y_des dy_des = np.diff(y_des) / self.dt # add zero to the beginning of every row dy_des = np.concatenate((np.zeros( (self.n_trajs, self.n_dmps, 1)), dy_des), axis=2) # calculate acceleration of y_des ddy_des = np.diff(dy_des) / self.dt # add zero to the beginning of every row ddy_des = np.concatenate((np.zeros( (self.n_trajs, self.n_dmps, 1)), ddy_des), axis=2) f_target = np.zeros((self.n_trajs, y_des.shape[-1], self.n_dmps)) # find the force required to move along this trajectory for d in range(self.n_dmps): f_target[:, :, d] = ( ddy_des[:, d, :] - self.ay[d] * (self.by[d] * ((np.ones(y_des[:, d, :].T.shape) * self.goal[:, d]).T - y_des[:, d, :]) - dy_des[:, d, :])) # efficiently generate weights to realize f_target self.gen_weights(f_target) if plot is True: # plot the basis function activations import matplotlib.pyplot as plt plt.figure() plt.subplot(211) psi_track = self.gen_psi(self.cs.rollout()) plt.plot(psi_track) plt.title('basis functions') # plot the desired forcing function vs approx plt.subplot(212) plt.plot(f_target[:, 0]) plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt) plt.legend(['f_target', 'w*psi']) plt.title('DMP forcing function') plt.tight_layout() plt.show() self.reset_state() return y_des def rollout(self, timesteps=None, **kwargs): """Generate a system trial, no feedback is incorporated.""" self.reset_state() if timesteps is None: if 'tau' in kwargs: timesteps = int(self.timesteps / kwargs['tau']) else: timesteps = self.timesteps # set up tracking vectors y_track = np.zeros((timesteps, self.n_dmps)) dy_track = np.zeros((timesteps, self.n_dmps)) ddy_track = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): # run and record timestep y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs) return y_track, dy_track, ddy_track def reset_state(self, y0=None, goal=None): """Reset the system state""" if y0 is not None: self.y0 = y0 if goal is not None: self.goal = goal self.y = self.y0.copy() self.dy = np.zeros(self.n_dmps) self.ddy = np.zeros(self.n_dmps) self.cs.reset_state() def step(self, tau=1.0, error=0.0, external_force=None): """Run the DMP system for a single timestep. tau float: scales the timestep increase tau to make the system execute faster error float: optional system feedback """ error_coupling = 1.0 / (1.0 + error) # run canonical system x = self.cs.step(tau=tau, error_coupling=error_coupling) # generate basis function activation psi = self.gen_psi(x) for d in range(self.n_dmps): # generate the forcing term f = (self.gen_front_term(x, d) * (np.dot(psi, self.w[d])) / np.sum(psi)) # DMP acceleration self.ddy[d] = (self.ay[d] * (self.by[d] * (self.goal[d] - self.y[d]) - self.dy[d] / tau) + f) * tau if external_force is not None: self.ddy[d] += external_force[d] self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling self.y[d] += self.dy[d] * self.dt * error_coupling return self.y, self.dy, self.ddy
class DMPs(object): """Implementation of Dynamic Motor Primitives, as described in Dr. Stefan Schaal's (2002) paper.""" def __init__(self, n_dmps, n_bfs, dt=.01, y0=0, goal=1, w=None, ay=None, by=None, **kwargs): """ n_dmps int: number of dynamic motor primitives n_bfs int: number of basis functions per DMP dt float: timestep for simulation y0 list: initial state of DMPs goal list: goal state of DMPs w list: tunable parameters, control amplitude of basis functions ay int: gain on attractor term y dynamics by int: gain on attractor term y dynamics """ self.n_dmps = n_dmps self.n_bfs = n_bfs self.dt = dt if isinstance(y0, (int, float)): y0 = np.ones(self.n_dmps)*y0 self.y0 = y0 if isinstance(goal, (int, float)): goal = np.ones(self.n_dmps)*goal self.goal = goal if w is None: # default is f = 0 w = np.zeros((self.n_dmps, self.n_bfs)) self.w = w self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012 self.by = self.ay / 4. if by is None else by # Schaal 2012 # set up the CS self.cs = CanonicalSystem(dt=self.dt, **kwargs) self.timesteps = int(self.cs.run_time / self.dt) # set up the DMP system self.reset_state() def check_offset(self): """Check to see if initial position and goal are the same if they are, offset slightly so that the forcing term is not 0""" for d in range(self.n_dmps): if (self.y0[d] == self.goal[d]): self.goal[d] += 1e-4 def gen_front_term(self, x, dmp_num): raise NotImplementedError() def gen_goal(self, y_des): raise NotImplementedError() def gen_psi(self): raise NotImplementedError() def gen_weights(self, f_target): raise NotImplementedError() def imitate_path(self, y_des, plot=False): """Takes in a desired trajectory and generates the set of system parameters that best realize this path. y_des list/array: the desired trajectories of each DMP should be shaped [n_dmps, run_time] """ # set initial state and goal if y_des.ndim == 1: y_des = y_des.reshape(1, len(y_des)) self.y0 = y_des[:, 0].copy() self.y_des = y_des.copy() self.goal = self.gen_goal(y_des) self.check_offset() # generate function to interpolate the desired trajectory import scipy.interpolate path = np.zeros((self.n_dmps, self.timesteps)) x = np.linspace(0, self.cs.run_time, y_des.shape[1]) for d in range(self.n_dmps): path_gen = scipy.interpolate.interp1d(x, y_des[d]) for t in range(self.timesteps): path[d, t] = path_gen(t * self.dt) y_des = path # calculate velocity of y_des dy_des = np.diff(y_des) / self.dt # add zero to the beginning of every row dy_des = np.hstack((np.zeros((self.n_dmps, 1)), dy_des)) # calculate acceleration of y_des ddy_des = np.diff(dy_des) / self.dt # add zero to the beginning of every row ddy_des = np.hstack((np.zeros((self.n_dmps, 1)), ddy_des)) f_target = np.zeros((y_des.shape[1], self.n_dmps)) # find the force required to move along this trajectory for d in range(self.n_dmps): f_target[:, d] = (ddy_des[d] - self.ay[d] * (self.by[d] * (self.goal[d] - y_des[d]) - dy_des[d])) # efficiently generate weights to realize f_target self.gen_weights(f_target) if plot is True: # plot the basis function activations import matplotlib.pyplot as plt plt.figure() plt.subplot(211) psi_track = self.gen_psi(self.cs.rollout()) plt.plot(psi_track) plt.title('basis functions') # plot the desired forcing function vs approx plt.subplot(212) plt.plot(f_target[:,0]) plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt) plt.legend(['f_target', 'w*psi']) plt.title('DMP forcing function') plt.tight_layout() plt.show() self.reset_state() return y_des def rollout(self, timesteps=None, **kwargs): """Generate a system trial, no feedback is incorporated.""" self.reset_state() if timesteps is None: if 'tau' in kwargs: timesteps = int(self.timesteps / kwargs['tau']) else: timesteps = self.timesteps # set up tracking vectors y_track = np.zeros((timesteps, self.n_dmps)) dy_track = np.zeros((timesteps, self.n_dmps)) ddy_track = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): # run and record timestep y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs) return y_track, dy_track, ddy_track def reset_state(self): """Reset the system state""" self.y = self.y0.copy() self.dy = np.zeros(self.n_dmps) self.ddy = np.zeros(self.n_dmps) self.cs.reset_state() def step(self, tau=1.0, error=0.0, external_force=None): """Run the DMP system for a single timestep. tau float: scales the timestep increase tau to make the system execute faster error float: optional system feedback """ error_coupling = 1.0 / (1.0 + error) # run canonical system x = self.cs.step(tau=tau, error_coupling=error_coupling) # generate basis function activation psi = self.gen_psi(x) for d in range(self.n_dmps): # generate the forcing term f = (self.gen_front_term(x, d) * (np.dot(psi, self.w[d])) / np.sum(psi)) # DMP acceleration self.ddy[d] = (self.ay[d] * (self.by[d] * (self.goal[d] - self.y[d]) - self.dy[d]/tau) + f) * tau if external_force is not None: self.ddy[d] += external_force[d] self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling self.y[d] += self.dy[d] * self.dt * error_coupling return self.y, self.dy, self.ddy
class DMPs_discrete_modified(object): """ An implementation of Cartesian discrete DMPs Modified based on DMP++: https://github.com/mginesi/dmp_pp Using quaternion (w,i,j,k) DMPs for orientation in Cartesian space. """ def __init__(self, n_dmps, n_bfs, dt=0.01, ax=4.0, y0=0, goal=1, w=None, K=1050, D=None, form='mod', basis='gaussian', rescale='rotodilatation', dim='position', **kwargs): """ """ self.n_dmps = copy.deepcopy(n_dmps) self.n_bfs = n_bfs self.dt = copy.deepcopy(dt) self.ax = copy.deepcopy(ax) self.form = copy.deepcopy(form) self.rescale = copy.deepcopy(rescale) self.basis = copy.deepcopy(basis) self.dim = copy.deepcopy(dim) if isinstance(y0, (int, float)): y0 = np.ones(self.n_dmps) * y0 self.y0 = y0 if isinstance(goal, (int, float)): goal = np.ones(self.n_dmps) * goal self.goal = goal if w is None: w = np.zeros((self.n_dmps, self.n_bfs)) if self.dim == 'position' \ else np.zeros((self.n_dmps-1, self.n_bfs)) self.w = copy.deepcopy(w) self.K = copy.deepcopy(K) if D is None: D = 2 * np.sqrt(self.K) self.D = copy.deepcopy(D) # set up the CS self.cs = CanonicalSystem(dt=self.dt, ax=self.ax, **kwargs) self.timesteps = int(self.cs.run_time / self.dt) self.gen_centers() # set variance of Gaussian basis functions # trial and error to find this spacing self.gen_width() # set up the DMP system self.reset_state() # self.h = np.ones(self.n_bfs) * self.n_bfs ** 1.5 / self.c / self.cs.ax # self.check_offset() def check_offset(self): """Check to see if initial position and goal are the same if they are, offset slightly so that the forcing term is not 0""" for d in range(self.n_dmps): if abs(self.y0[d] - self.goal[d]) < 1e-4: self.goal[d] += 1e-4 def rollout(self, timesteps=None, **kwargs): """Generate a system trial, no feedback is incorporated.""" self.reset_state() if timesteps is None: if "tau" in kwargs: timesteps = int(self.timesteps / kwargs["tau"]) else: timesteps = self.timesteps # set up tracking vectors y_track = np.zeros((timesteps, self.n_dmps)) dy_track = np.zeros((timesteps, self.n_dmps)) ddy_track = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): # run and record timestep y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs) return y_track, dy_track, ddy_track def gen_width(self): ''' Set the "widths" for the basis functions. ''' if self.basis == 'gaussian': self.h = 1.0 / np.diff(self.c) / np.diff(self.c) self.h = np.append(self.h, self.h[-1]) else: self.h = 0.2 / np.diff(self.c) self.h = np.append(self.h[0], self.h) def gen_centers(self): """Set the centre of the Gaussian basis functions be spaced evenly throughout run time""" """x_track = self.cs.discrete_rollout() t = np.arange(len(x_track))*self.dt # choose the points in time we'd like centers to be at c_des = np.linspace(0, self.cs.run_time, self.n_bfs) self.c = np.zeros(len(c_des)) for ii, point in enumerate(c_des): diff = abs(t - point) self.c[ii] = x_track[np.where(diff == min(diff))[0][0]]""" # desired activations throughout time self.c = np.exp( -self.cs.ax * self.cs.run_time * ((np.cumsum(np.ones([1, self.n_bfs])) - 1) / self.n_bfs)) def gen_front_term(self, x, n): """Generates the diminishing front term on the forcing term. x float: the current value of the canonical system dmp_num int: the index of the current dmp """ if self.dim == 'position': return x * (self.goal - self.y0) if self.dim == 'orientation': return x * q_oper.quaternion_error(self.goal, self.y0) def gen_goal(self, y_des): """Generate the goal for path imitation. For rhythmic DMPs the goal is the average of the desired trajectory. y_des np.array: the desired trajectory to follow """ return np.copy(y_des[-1]) def gen_psi(self, x): """Generates the activity of the basis functions for a given canonical system rollout. x float, array: the canonical system state or path """ c = np.reshape(self.c, [self.n_bfs, 1]) h = np.reshape(self.h, [self.n_bfs, 1]) if self.basis == 'gaussian': xi = h * (x - c)**2 psi_set = np.exp(-xi) else: xi = np.abs(h * (x - c)) if self.basis == 'mollifier': psi_set = (np.exp(-1.0 / (1.0 - xi * xi))) * (xi < 1.0) elif self.basis == 'wendland2': psi_set = ((1.0 - xi)**2.0) * (xi < 1.0) elif self.basis == 'wendland3': psi_set = ((1.0 - xi)**3.0) * (xi < 1.0) elif self.basis == 'wendland4': psi_set = ((1.0 - xi)**4.0 * (4.0 * xi + 1.0)) * (xi < 1.0) elif self.basis == 'wendland5': psi_set = ((1.0 - xi)**5.0 * (5.0 * xi + 1)) * (xi < 1.0) elif self.basis == 'wendland6': psi_set = ((1.0 - xi)**6.0 * (35.0 * xi**2.0 + 18.0 * xi + 3.0)) * (xi < 1.0) elif self.basis == 'wendland7': psi_set = ((1.0 - xi)**7.0 * (16.0 * xi**2.0 + 7.0 * xi + 1.0)) * (xi < 1.0) elif self.basis == 'wendland8': psi_set = ( ((1.0 - xi)**8.0 * (32.0 * xi**3.0 + 25.0 * xi**2.0 + 8.0 * xi + 1.0)) * (xi < 1.0)) psi_set = np.nan_to_num(psi_set) return psi_set def gen_weights(self, f_target): """Generate a set of weights over the basis functions such that the target forcing term trajectory is matched. f_target np.array: the desired forcing term trajectory """ # calculate x and psi # f_target = np.transpose(f_target) s_track = self.cs.rollout() psi_track = self.gen_psi(s_track) # Compute useful quantities sum_psi = np.sum(psi_track, 0) sum_psi_2 = sum_psi * sum_psi s_track_2 = s_track * s_track # Set up the minimization problem A = np.zeros([self.n_bfs, self.n_bfs]) b = np.zeros([self.n_bfs]) # The matrix does not depend on f for k in range(self.n_bfs): A[k, k] = scipy.integrate.simps( psi_track[k] * psi_track[k] * s_track_2 / sum_psi_2, s_track) for h in range(k + 1, self.n_bfs): A[k, h] = scipy.integrate.simps( psi_track[k] * psi_track[h] * s_track_2 / sum_psi_2, s_track) A[h, k] = A[k, h].copy() LU = scipy.linalg.lu_factor(A) # The problem is decoupled for each dimension if self.dim == 'position': for d in range(self.n_dmps): # Create the vector of the regression problem for k in range(self.n_bfs): b[k] = scipy.integrate.simps( f_target[d] * psi_track[k] * s_track / sum_psi, s_track) # Solve the minimization problem self.w[d] = scipy.linalg.lu_solve(LU, b) else: for d in range(self.n_dmps - 1): # Create the vector of the regression problem for k in range(self.n_bfs): b[k] = scipy.integrate.simps( f_target[d] * psi_track[k] * s_track / sum_psi, s_track) # Solve the minimization problem self.w[d] = scipy.linalg.lu_solve(LU, b) self.w = np.nan_to_num(self.w) def imitate_path(self, y_des, dy_des=None, ddy_des=None, t_des=None, plot=False): """Takes in a desired trajectory and generates the set of system parameters that best realize this path. y_des list/array: the desired trajectories of each DMP should be shaped [run_time, n_dmps] """ # set initial state and goal self.y0 = y_des[0].copy() self.goal = y_des[-1].copy() # self.check_offset() # generate function to interpolate the desired trajectory import scipy.interpolate if t_des is None: # Default value for t_des t_des = np.linspace(0, self.cs.run_time, y_des.shape[0]) else: # Warp time to start from zero and end up to T t_des -= t_des[0] t_des /= t_des[-1] t_des *= self.cs.run_time time = np.linspace(0, self.cs.run_time, self.cs.timesteps) path_gen = scipy.interpolate.interp1d(t_des, y_des.transpose()) path = path_gen(time) y_des = path.transpose() # timesteps x n_dmps # calculate velocity of y_des with central differences if self.dim == 'position': if dy_des is None: dy_des = np.zeros([self.n_dmps, self.cs.timesteps]) for i in range(self.n_dmps): dy_des[i] = np.gradient(path[i]) / self.dt # dy_des = np.gradient(y_des, axis=1) / self.dt # calculate acceleration of y_des with central differences if ddy_des is None: ddy_des = np.zeros([self.n_dmps, self.cs.timesteps]) for i in range(self.n_dmps): ddy_des[i] = np.gradient(dy_des[i]) / self.dt dy_des = dy_des.transpose() # timesteps x n_dmps ddy_des = ddy_des.transpose() # find the force required to move along this trajectory f_target = np.zeros([self.n_dmps, self.cs.timesteps]) if self.form == 'mod': f_target = ( (ddy_des / self.K - (self.goal - y_des) + self.D * dy_des / self.K).transpose() + np.reshape( (self.goal - self.y0), [self.n_dmps, 1]) * self.cs.rollout()) # n_dmps x timesteps elif self.form == 'old': # f_target = ((ddy_des - self.K * (self.goal - y_des) + self.D * dy_des)/(self.goal - self.y0)).transpose() f_target = np.dot(np.linalg.inv(np.diag(self.goal - self.y0)), (ddy_des - self.K * (self.goal - y_des) + self.D * dy_des).transpose()) elif self.dim == 'orientation': dq_des = np.zeros([self.n_dmps, self.cs.timesteps]) # # Ude, 2014, "Orientation in Cartesian Space Dynamic Movement Primitives" angular_vel = np.zeros([self.timesteps, self.n_dmps - 1]) d_angular_vel = np.zeros([self.n_dmps - 1, self.timesteps]) for i in range(self.n_dmps): dq_des[i] = np.gradient( path[i]) / self.dt # dq, n_dmps x timesteps for index, dq in enumerate(dq_des.T): q = path.T[index] angular_vel[index] = 2 * rm.quaternion_multiply( dq, rm.quaternion_conjugate(q))[ 1:] # w(t)=Im(2*dq(t)*q_conj(t)) for i in range(self.n_dmps - 1): d_angular_vel[i] = np.gradient(angular_vel.T[i]) / self.dt if dy_des is None: dy_des = angular_vel if ddy_des is None: ddy_des = d_angular_vel ddy_des = ddy_des.transpose() # d_eta, timesteps x n_dmps import matplotlib.pyplot as plt # fig, axes = plt.subplots(3, 2) # axes[0, 0].plot(dy_des[:, 0], '--r', label='$\eta_x$'); axes[0, 0].legend() # axes[1, 0].plot(dy_des[:, 1], '--g', label='$\eta_y$'); axes[1, 0].legend() # axes[2, 0].plot(dy_des[:, 2], '--b', label='$\eta_z$'); axes[2, 0].legend() # axes[0, 1].plot(ddy_des[:, 0], '-r', label='$\dot{\eta}_x$'); axes[0, 1].legend() # axes[1, 1].plot(ddy_des[:, 1], '-g', label='$\dot{\eta}_y$'); axes[1, 1].legend() # axes[2, 1].plot(ddy_des[:, 2], '-b', label='$\dot{\eta}_z$'); axes[2, 1].legend() # fig.tight_layout() f_target = np.zeros([self.cs.timesteps, self.n_dmps - 1]) eq_g_0 = q_oper.quaternion_error(self.goal, self.y0) for t in range(self.cs.timesteps): eq = q_oper.quaternion_error(self.goal, y_des[t]) f_target[t] = np.dot( np.linalg.inv(np.diag(eq_g_0)), ddy_des[t] - self.K * eq + self.D * dy_des[t]) # # Koutras, 2019, "A correct formulation for the orientation DMPs for robot control in the Cartesian space" # if dy_des is None: # dy_des = np.zeros([self.n_dmps-1, self.cs.timesteps]) # # for index, dq in enumerate(dq_des.T): # q = path.T[index] # q_conj = rm.quaternion_conjugate(q) # j_q = q_oper.calculate_jacobian(rm.quaternion_multiply(self.goal, q_conj))[1] # Jacobian_q matrix [3x4] # d_eq = -2*np.dot(j_q, rm.quaternion_multiply(self.goal, # rm.quaternion_multiply(q_conj, rm.quaternion_multiply(dq, q_conj))).transpose()) # dy_des[:, index] = d_eq.flatten() # if ddy_des is None: # ddy_des = np.zeros([self.n_dmps-1, self.cs.timesteps]) # for i in range(self.n_dmps-1): # ddy_des[i] = np.gradient(dy_des[i]) / self.dt # dy_des = dy_des.transpose() # timesteps x n_dmps # ddy_des = ddy_des.transpose() # f_target = np.zeros([self.n_dmps-1, self.cs.timesteps]) # eq_g_0 = q_oper.quaternion_error(self.goal, self.y0) # f_target = f_target.T # for t in range(self.cs.timesteps): # eq = q_oper.quaternion_error(self.goal, y_des[t]) # # f_target[t] = np.dot(np.diag(eq_g_0), ddy_des[t] - self.K*eq + self.D*dy_des[t]) f_target = f_target.T # efficiently generate weights to realize f_target self.gen_weights(f_target) if plot is True: # plot the basis function activations import matplotlib.pyplot as plt plt.figure() plt.subplot(211) psi_track = self.gen_psi(self.cs.rollout()) plt.plot(np.transpose(psi_track)) plt.title("basis functions") # plot the desired forcing function vs approx for ii in range(self.n_dmps): plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii) plt.plot(f_target[ii, :], "--", label="f_target %i" % ii) for ii in range(self.n_dmps): plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii) plt.plot( np.sum(np.transpose(psi_track) * self.w[ii], axis=1) * self.dt, label="w*psi %i" % ii, ) plt.legend() plt.title("DMP forcing function") plt.tight_layout() plt.show() self.reset_state() self.learned_position = self.goal - self.y0 return y_des def rollout(self, timesteps=None, **kwargs): """Generate a system trial, no feedback is incorporated.""" self.reset_state() if timesteps is None: if "tau" in kwargs: timesteps = int(self.timesteps / kwargs["tau"]) else: timesteps = self.timesteps # set up tracking vectors if self.dim == 'orientation': y_track = np.zeros((timesteps, self.n_dmps)) # quaternion dy_track = np.zeros((timesteps, self.n_dmps - 1)) ddy_track = np.zeros((timesteps, self.n_dmps - 1)) elif self.dim == 'position': y_track = np.zeros((timesteps, self.n_dmps)) dy_track = np.zeros((timesteps, self.n_dmps)) ddy_track = np.zeros((timesteps, self.n_dmps)) for t in range(timesteps): # run and record timestep y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs) return y_track, dy_track, ddy_track def reset_state(self): """Reset the system state""" self.y = self.y0.copy() if self.dim == 'position': self.dy = np.zeros(self.n_dmps) self.ddy = np.zeros(self.n_dmps) elif self.dim == 'orientation': self.dy = np.zeros(self.n_dmps - 1) self.ddy = np.zeros(self.n_dmps - 1) self.cs.reset_state() def step(self, tau=1.0, error=0.0, external_force=None): """Run the DMP system for a single timestep. tau float: scales the timestep increase tau to make the system execute faster error float: optional system feedback """ if self.rescale == 'rotodilatation': M = roto_dilatation(self.learned_position, self.goal - self.y0) elif self.rescale == 'diagonal': M = np.diag((self.goal - self.y0) / self.learned_position) else: M = np.eye(self.n_dmps) error_coupling = 1.0 / (1.0 + error) # run canonical system x = self.cs.step(tau=tau, error_coupling=error_coupling) # generate basis function activation psi = self.gen_psi(x) if self.dim == 'position': if self.form == 'mod': # generate the forcing term f = (np.dot(self.w, psi[:, 0])) / np.sum(psi) * x f = np.nan_to_num(np.dot(M, f)) # DMP acceleration self.ddy = (self.K * (self.goal - self.y) - self.D * self.dy - self.K * (self.goal - self.y0) * x + self.K * f) if external_force is not None: self.ddy += external_force self.dy += self.ddy * tau * self.dt * error_coupling self.y += self.dy * tau * self.dt * error_coupling else: # generate the forcing term f = self.gen_front_term(x, self.n_dmps) * (np.dot( self.w, psi[:, 0])) / np.sum(psi) # DMP acceleration self.ddy = (self.K * (self.goal - self.y) - self.D * self.dy + f) if external_force is not None: self.ddy += external_force self.dy += self.ddy * tau * self.dt * error_coupling self.y += self.dy * tau * self.dt * error_coupling elif self.dim == 'orientation': # generate the forcing term f = self.gen_front_term(x, self.n_dmps - 1) * (np.dot( self.w, psi[:, 0])) / np.sum(psi) # # Ude, 2014, "Orientation in Cartesian Space Dynamic Movement Primitives" self.ddy = (self.K * q_oper.quaternion_error(self.goal, self.y) - self.D * self.dy + f) if external_force is not None: self.ddy += external_force self.dy += self.ddy * tau * self.dt * error_coupling # w_q = np.concatenate((np.array([0]), self.dy), axis=0) # w_q = [0, w] # dq = (rm.quaternion_multiply(1 / 2 * w_q, self.y) * tau * error_coupling).flatten() q_delta = (q_oper.vector_exp(1 / 2 * self.dy * tau * error_coupling * self.dt)).flatten() self.y = rm.quaternion_multiply(q_delta, self.y) # Koutras, 2019, "A correct formulation for the orientation DMPs for robot control in the Cartesian space" # # DMP acceleration # eq = q_oper.quaternion_error(self.goal, self.y) # self.ddy = (self.K * (0 - eq) - self.D * self.dy + f) # # print('ddy =', self.ddy) # if external_force is not None: # self.ddy += external_force # self.dy += self.ddy * tau * self.dt * error_coupling # # print('dy =', self.dy) # q_conj = rm.quaternion_conjugate(self.y) # goal_conj = rm.quaternion_conjugate(self.goal) # dq = -1/2 * self.y * goal_conj * \ # np.dot(q_oper.calculate_jacobian(rm.quaternion_multiply(self.goal, q_conj))[0], self.dy) * self.y # self.y += dq * tau * self.dt * error_coupling return self.y, self.dy, self.ddy