def get_get_dV_c_from_target_T(urdf_file_path): w_gain = sp.Matrix(sp.symarray('w_gain', 3)) q_gain = sp.Matrix(sp.symarray('q_gain', 3)) v_gain = sp.Matrix(sp.symarray('v_gain', 3)) t_gain = sp.Matrix(sp.symarray('t_gain', 3)) ctrl_gains = (w_gain, q_gain, v_gain, t_gain) T_ct = (T_st.inverse() * T_sc).inverse() dw_c = sp.matrix_multiply_elementwise(-w_gain, w_c) + \ sp.matrix_multiply_elementwise(q_gain, T_ct.so3.q.vec) * T_ct.so3.q.real dv_c = sp.matrix_multiply_elementwise(-v_gain, v_c) + \ sp.matrix_multiply_elementwise(t_gain, T_ct.t) dV_c_target = dw_c.col_join(dv_c) get_ftz_from_V_c_dV_c = get_get_ftz_from_V_c_dV_c( urdf_file_path)["s_lambda"] ftz_from_V_c_dV_c_result = get_ftz_from_V_c_dV_c(V_c, dV_c_target) return { "lambda": sp.lambdify((t_sc, q_s_cog, w_c, v_c, t_st, q_s_target, ctrl_gains), ftz_from_V_c_dV_c_result, 'numpy'), "s_lambda": sp.lambdify((t_sc, q_s_cog, t_st, q_s_target, ctrl_gains), ftz_from_V_c_dV_c_result, 'sympy'), "result": ftz_from_V_c_dV_c_result, }
def _initialize_functions(self): """ _initialize_functions converts the symbolic mathematics of sympy to a matrix representation that is compatible with multi-dimentionality. """ # Parameters nDimensions = self.constants[self.nDimensions] self.position = sp.Matrix( [sp.symbols("r_" + str(i)) for i in range(nDimensions)]) self.multiplicity = sp.Matrix( [sp.symbols("mult_" + str(i)) for i in range(nDimensions)]) self.phase_shift = sp.Matrix( [sp.symbols("phase_" + str(i)) for i in range(nDimensions)]) self.amplitude = sp.Matrix( [sp.symbols("amp_" + str(i)) for i in range(nDimensions)]) self.yOffset = sp.Matrix( [sp.symbols("yOff_" + str(i)) for i in range(nDimensions)]) # Function self.V_dim = sp.matrix_multiply_elementwise( self.amplitude, (sp.matrix_multiply_elementwise( (self.position + self.phase_shift), self.multiplicity)).applyfunc(sp.cos)) + self.yOffset self.V_functional = sp.Sum(self.V_dim[self.i, 0], (self.i, 0, self.nDimensions - 1))
def get_pose_control(): w_gain = sp.Matrix(sp.symarray('w_gain', 3)) q_gain = sp.Matrix(sp.symarray('q_gain', 3)) v_gain = sp.Matrix(sp.symarray('v_gain', 3)) t_gain = sp.Matrix(sp.symarray('t_gain', 3)) ctrl_gains = (w_gain, q_gain, v_gain, t_gain) T_ct = (T_st.inverse() * T_sc).inverse() m_c = sp.matrix_multiply_elementwise(-w_gain, w_c) + \ sp.matrix_multiply_elementwise(q_gain, T_ct.so3.q.vec) * T_ct.so3.q.real f_c = sp.matrix_multiply_elementwise(-v_gain, v_c) + \ sp.matrix_multiply_elementwise(t_gain, T_ct.t) F_c = m_c.col_join(f_c) return { "lambda": sp.lambdify((t_sc, q_s_cog, w_c, v_c, t_st, q_s_target, ctrl_gains), F_c, 'numpy'), "s_lambda": sp.lambdify((t_sc, q_s_cog, t_st, q_s_target, ctrl_gains), F_c, 'sympy'), "result": F_c, }
class wavePotential(_potentialNDClsSymPY): name: str = "Wave Potential" nDim = sp.symbols("nDim") position: sp.Matrix = sp.Matrix([sp.symbols("r")]) multiplicity: sp.Matrix = sp.Matrix([sp.symbols("m")]) phase_shift: sp.Matrix = sp.Matrix([sp.symbols("omega")]) amplitude: sp.Matrix = sp.Matrix([sp.symbols("A")]) yOffset: sp.Matrix = sp.Matrix([sp.symbols("y_off")]) V_dim = sp.matrix_multiply_elementwise( amplitude, (sp.matrix_multiply_elementwise( (position + phase_shift), multiplicity)).applyfunc( sp.cos)) + yOffset i = sp.Symbol("i") V_orig = sp.Sum(V_dim[i, 0], (i, 0, nDim)) def __init__(self, amplitude, multiplicity, phase_shift, y_offset, nDim: int): self.constants.update( {"amp_" + str(j): amplitude[j] for j in range(nDim)}) self.constants.update( {"mult_" + str(j): multiplicity[j] for j in range(nDim)}) self.constants.update( {"phase_" + str(j): phase_shift[j] for j in range(nDim)}) self.constants.update( {"yOff_" + str(j): y_offset[j] for j in range(nDim)}) super().__init__(self.nDim) def _initialize_functions(self): # Parameters nDim = self.constants[self.nDim] self.position = sp.Matrix( [sp.symbols("pos_" + str(i)) for i in range(nDim)]) self.multiplicity = sp.Matrix( [sp.symbols("mult_" + str(i)) for i in range(nDim)]) self.phase_shift = sp.Matrix( [sp.symbols("phase_" + str(i)) for i in range(nDim)]) self.amplitude = sp.Matrix( [sp.symbols("amp_" + str(i)) for i in range(nDim)]) self.yOffset = sp.Matrix( [sp.symbols("yOff_" + str(i)) for i in range(nDim)]) #Function self.V_dim = sp.matrix_multiply_elementwise( self.amplitude, (sp.matrix_multiply_elementwise( (self.position + self.phase_shift), self.multiplicity)).applyfunc(sp.cos)) + self.yOffset self.V_orig = sp.Sum(self.V_dim[self.i, 0], (self.i, 0, self.nDim - 1))
def pexpect(self, expr): '''Computes the pseudoexpectation of a given polynomial EXPR''' poly = sp.poly(expr, self.symbols) self.basis.check_can_represent(poly) Qp = self.basis.sos_sym_poly_repr(poly) X = sp.Matrix(len(self.basis), len(self.basis), self.pic_const.dual) return sum(sp.matrix_multiply_elementwise(X, Qp))
def _initialize_functions(self): """ _initialize_functions converts the symbolic mathematics of sympy to a matrix representation that is compatible with multi-dimentionality. """ # Parameters nDimensions = self.constants[self.nDimensions] self.position = sp.Matrix( [sp.symbols("r_" + str(i)) for i in range(nDimensions)]) self.mean = sp.Matrix( [sp.symbols("mu_" + str(i)) for i in range(nDimensions)]) self.sigma = sp.Matrix( [sp.symbols("sigma_" + str(i)) for i in range(nDimensions)]) self.amplitude = sp.symbols("A_gauss") # Function self.V_dim = self.amplitude * (sp.matrix_multiply_elementwise( -(self.position - self.mean).applyfunc(lambda x: x**2), 0.5 * (self.sigma).applyfunc(lambda x: x**(-2))).applyfunc(sp.exp)) # self.V_functional = sp.Product(self.V_dim[self.i, 0], (self.i, 0, self.nDimensions- 1)) # Not too beautiful, but sp.Product raises errors if (self._negative_sign): self.V_functional = -(self.V_dim[0, 0] * self.V_dim[1, 0]) else: self.V_functional = self.V_dim[0, 0] * self.V_dim[1, 0]
def fluctuation_amplitude_from_temperature(method, temperature, c_s_sq=sp.Symbol("c_s") ** 2): """Produces amplitude equations according to (2.60) and (3.54) in Schiller08""" normalization_factors = sp.matrix_multiply_elementwise(method.moment_matrix, method.moment_matrix) * \ sp.Matrix(method.weights) density = method.zeroth_order_equilibrium_moment_symbol if method.conserved_quantity_computation.zero_centered_pdfs: density += 1 mu = temperature * density / c_s_sq return [sp.sqrt(mu * norm * (1 - (1 - rr) ** 2)) for norm, rr in zip(normalization_factors, method.relaxation_rates)]
def _initialize_functions(self): # Parameters nDim = self.constants[self.nDim] self.position = sp.Matrix( [sp.symbols("pos_" + str(i)) for i in range(nDim)]) self.multiplicity = sp.Matrix( [sp.symbols("mult_" + str(i)) for i in range(nDim)]) self.phase_shift = sp.Matrix( [sp.symbols("phase_" + str(i)) for i in range(nDim)]) self.amplitude = sp.Matrix( [sp.symbols("amp_" + str(i)) for i in range(nDim)]) self.yOffset = sp.Matrix( [sp.symbols("yOff_" + str(i)) for i in range(nDim)]) #Function self.V_dim = sp.matrix_multiply_elementwise( self.amplitude, (sp.matrix_multiply_elementwise( (self.position + self.phase_shift), self.multiplicity)).applyfunc(sp.cos)) + self.yOffset self.V_orig = sp.Sum(self.V_dim[self.i, 0], (self.i, 0, self.nDim - 1))
def weighted_term(weight, term): """Calculate components from dot product of weights and term Args: weight (MatrixSymbol): normalized log-mean divisia weights term (MatrixSymbol): The effect of the component (RHS) variable on the change in the LHS variable """ weighted_term = sp.matrix_multiply_elementwise(weight, term).doit() ones_ = sp.ones(weighted_term.shape[1], 1) component = weighted_term * ones_ return component
def _initialize_functions(self): """ Build up the nDimensionssymbolic definitions """ # Parameters nDimensions= self.constants[self.nDimensions] self.position = sp.Matrix([sp.symbols("r_" + str(i)) for i in range(nDimensions)]) self.r_shift = sp.Matrix([sp.symbols("r_shift" + str(i)) for i in range(nDimensions)]) self.V_off = sp.Matrix([sp.symbols("V_off_" + str(i)) for i in range(nDimensions)]) self.k = sp.Matrix([sp.symbols("k_" + str(i)) for i in range(nDimensions)]) # Function self.V_dim = 0.5 * sp.matrix_multiply_elementwise(self.k, ( (self.position - self.r_shift).applyfunc(lambda x: x ** 2))) # +self.Voff self.V_functional = sp.Sum(self.V_dim[self.i, 0], (self.i, 0, self.nDimensions - 1))
def additive_weights(self, log_mean_matrix, log_mean_matrix_total, log_mean_share): """Calculate log-mean divisia weights for the additive model in symbolic terms Args: log_mean_matrix ([type]): [description] log_mean_matrix_total ([type]): [description] """ if self.lmdi_type == 'I': weights = log_mean_matrix elif self.lmdi_type == 'II': numerator = sp.matrix_multiply_elementwise(log_mean_share, log_mean_matrix) weights = self.hadamard_division(numerator, log_mean_matrix_total) return weights
def create_symbolic_term(self, numerator, denominator): """Create LMDI RHS term e.g. the log change of structure (Ai/A) in symbolic matrix terms Args: numerator (Symbolic Matrix): Dividend (e.g. Ai) denominator (Symbolic Matrix): Divisor (e.g. A) Returns: term (Symbolic Matrix): the log change of the RHS term """ base_term = self.hadamard_division(numerator, denominator) shift_term, long_term = self.shift_matrices(base_term) # find change (divide every row by previous row) term = sp.matrix_multiply_elementwise(long_term, shift_term) term = term.applyfunc(sp.log) return term
def _initialize_functions(self): """ _initialize_functions converts the symbolic mathematics of sympy to a matrix representation that is compatible with multi-dimentionality. """ # Parameters nDimensions = self.constants[self.nDimensions] self.position = sp.Matrix( [sp.symbols("r_" + str(i)) for i in range(nDimensions)]) self.r_shift = sp.Matrix( [sp.symbols("r_shift" + str(i)) for i in range(nDimensions)]) self.V_off = sp.Matrix( [sp.symbols("V_off_" + str(i)) for i in range(nDimensions)]) self.k = sp.Matrix( [sp.symbols("k_" + str(i)) for i in range(nDimensions)]) # Function self.V_dim = 0.5 * sp.matrix_multiply_elementwise( self.k, ((self.position - self.r_shift).applyfunc(lambda x: x**2))) # +self.Voff self.V_functional = sp.Sum(self.V_dim[self.i, 0], (self.i, 0, self.nDimensions - 1))
def multiplicative_weights(self, log_mean_matrix, log_mean_share, log_mean_share_total, log_mean_total): """Calculate log-mean divisia weights for the multiplicative model in symbolic terms Args: log_mean_matrix ([type]): [description] log_mean_share ([type]): [description] log_mean_share_total ([type]): [description] Returns: [type]: [description] """ """[summary] """ if self.lmdi_type == 'I': weights = log_mean_matrix elif self.lmdi_type == 'II': numerator = sp.matrix_multiply_elementwise(log_mean_share, log_mean_matrix) log_mean_total = self.transform_col_vector(log_mean_total) weights = self.hadamard_division(numerator, log_mean_total) return weights
def hp(x, y): """Hadamard product: Elementwise product of two vectors""" return sp.matrix_multiply_elementwise(x, y)
class gaussPotential(_potential2DCls): ''' Gaussian like potential, usually used for metadynamics ''' name: str = "Gaussian Potential 2D" nDimensions: sp.Symbol = sp.symbols("nDimensions") position: sp.Matrix = sp.Matrix([sp.symbols("r")]) mean: sp.Matrix = sp.Matrix([sp.symbols("mu")]) sigma: sp.Matrix = sp.Matrix([sp.symbols("sigma")]) amplitude = sp.symbols("A_gauss") # we assume that the two dimentions are uncorrelated # V_dim = amplitude * (sp.matrix_multiply_elementwise((position - mean) ** 2, (2 * sigma ** 2) ** (-1)).applyfunc(sp.exp)) V_dim = amplitude * (sp.matrix_multiply_elementwise( -(position - mean).applyfunc(lambda x: x**2), 0.5 * (sigma).applyfunc(lambda x: x**(-2))).applyfunc(sp.exp)) i = sp.Symbol("i") V_functional = sp.summation(V_dim[i, 0], (i, 0, nDimensions)) # V_orig = V_dim[0, 0] * V_dim[1, 0] def __init__(self, amplitude=1., mu=(0., 0.), sigma=(1., 1.), negative_sign: bool = False): ''' __init__ This is the Constructor of a 2D Gauss Potential Parameters ---------- A: float, optional scaling of the gauss function, defaults to 1. mu: tupel, optional mean of the gauss function, defaults to (0., 0.) sigma: tupel, optional standard deviation of the gauss function, defaults to (1., 1.) negative_sign: bool, optional this option is switching the sign of the final potential energy landscape. ==> mu defines the minima location, not maxima location ''' nDimensions = 2 self.constants = {"A_gauss": amplitude} self.constants.update( {"mu_" + str(j): mu[j] for j in range(nDimensions)}) self.constants.update( {"sigma_" + str(j): sigma[j] for j in range(nDimensions)}) self.constants.update({self.nDimensions: nDimensions}) self._negative_sign = negative_sign super().__init__() def _initialize_functions(self): """ _initialize_functions converts the symbolic mathematics of sympy to a matrix representation that is compatible with multi-dimentionality. """ # Parameters nDimensions = self.constants[self.nDimensions] self.position = sp.Matrix( [sp.symbols("r_" + str(i)) for i in range(nDimensions)]) self.mean = sp.Matrix( [sp.symbols("mu_" + str(i)) for i in range(nDimensions)]) self.sigma = sp.Matrix( [sp.symbols("sigma_" + str(i)) for i in range(nDimensions)]) self.amplitude = sp.symbols("A_gauss") # Function self.V_dim = self.amplitude * (sp.matrix_multiply_elementwise( -(self.position - self.mean).applyfunc(lambda x: x**2), 0.5 * (self.sigma).applyfunc(lambda x: x**(-2))).applyfunc(sp.exp)) # self.V_functional = sp.Product(self.V_dim[self.i, 0], (self.i, 0, self.nDimensions- 1)) # Not too beautiful, but sp.Product raises errors if (self._negative_sign): self.V_functional = -(self.V_dim[0, 0] * self.V_dim[1, 0]) else: self.V_functional = self.V_dim[0, 0] * self.V_dim[1, 0] def _update_functions(self): """ This function is needed to simplyfiy the symbolic equation on the fly and to calculate the position derivateive. """ self.V = self.V_functional.subs(self.constants) self.dVdpos_functional = sp.diff(self.V_functional, self.position) # not always working! self.dVdpos = sp.diff(self.V, self.position) self.dVdpos = self.dVdpos.subs(self.constants) self._calculate_energies = sp.lambdify(self.position, self.V, "numpy") self._calculate_dVdpos = sp.lambdify(self.position, self.dVdpos, "numpy")
class wavePotential(_potential2DClsSymPY): name: str = "Wave Potential" nDim: sp.Symbol = sp.symbols("nDim") position: sp.Matrix = sp.Matrix([sp.symbols("r")]) multiplicity: sp.Matrix = sp.Matrix([sp.symbols("m")]) phase_shift: sp.Matrix = sp.Matrix([sp.symbols("omega")]) amplitude: sp.Matrix = sp.Matrix([sp.symbols("A")]) yOffset: sp.Matrix = sp.Matrix([sp.symbols("y_off")]) V_dim = sp.matrix_multiply_elementwise( amplitude, (sp.matrix_multiply_elementwise( (position + phase_shift), multiplicity)).applyfunc( sp.cos)) + yOffset i = sp.Symbol("i") V_orig = sp.Sum(V_dim[i, 0], (i, 0, nDim)) def __init__(self, amplitude=(1, 1), multiplicity=(1, 1), phase_shift=(0, 0), y_offset=(0, 0), degree: bool = True): nDim = 2 self.constants.update( {"amp_" + str(j): amplitude[j] for j in range(nDim)}) self.constants.update( {"mult_" + str(j): multiplicity[j] for j in range(nDim)}) self.constants.update( {"yOff_" + str(j): y_offset[j] for j in range(nDim)}) self.constants.update({"nDim": nDim}) if (degree): self.constants.update({ "phase_" + str(j): np.deg2rad(phase_shift[j]) for j in range(nDim) }) else: self.constants.update( {"phase_" + str(j): phase_shift[j] for j in range(nDim)}) super().__init__() if (degree): self.set_degree_mode() def _initialize_functions(self): # Parameters nDim = self.constants[self.nDim] self.position = sp.Matrix( [sp.symbols("pos_" + str(i)) for i in range(nDim)]) self.multiplicity = sp.Matrix( [sp.symbols("mult_" + str(i)) for i in range(nDim)]) self.phase_shift = sp.Matrix( [sp.symbols("phase_" + str(i)) for i in range(nDim)]) self.amplitude = sp.Matrix( [sp.symbols("amp_" + str(i)) for i in range(nDim)]) self.yOffset = sp.Matrix( [sp.symbols("yOff_" + str(i)) for i in range(nDim)]) #Function self.V_dim = sp.matrix_multiply_elementwise( self.amplitude, (sp.matrix_multiply_elementwise( (self.position + self.phase_shift), self.multiplicity)).applyfunc(sp.cos)) + self.yOffset self.V_orig = sp.Sum(self.V_dim[self.i, 0], (self.i, 0, self.nDim - 1)) def set_degree_mode(self): self.ene = lambda positions: np.squeeze( self._calculate_energies(*np.hsplit(np.deg2rad(positions), self. constants[self.nDim]))) self.dvdpos = lambda positions: np.squeeze( self._calculate_dVdpos(*np.hsplit(np.deg2rad(positions), self. constants[self.nDim]))) def set_radian_mode(self): self.ene = lambda positions: np.squeeze( self._calculate_energies(*np.hsplit(positions, self.constants[ self.nDim]))) self.dvdpos = lambda positions: np.squeeze( self._calculate_dVdpos(*np.hsplit(positions, self.constants[ self.nDim])))
class wavePotential(_potential2DCls): """ Simple 2D wave potential consisting of cosine functions with given multiplicity, that can be shifted and elongated """ name: str = "Wave Potential" nDimensions: sp.Symbol = sp.symbols("nDimensions") position: sp.Matrix = sp.Matrix([sp.symbols("r")]) multiplicity: sp.Matrix = sp.Matrix([sp.symbols("m")]) phase_shift: sp.Matrix = sp.Matrix([sp.symbols("omega")]) amplitude: sp.Matrix = sp.Matrix([sp.symbols("A")]) yOffset: sp.Matrix = sp.Matrix([sp.symbols("y_off")]) V_dim = sp.matrix_multiply_elementwise( amplitude, (sp.matrix_multiply_elementwise( (position + phase_shift), multiplicity)).applyfunc( sp.cos)) + yOffset i = sp.Symbol("i") V_functional = sp.Sum(V_dim[i, 0], (i, 0, nDimensions)) def __init__(self, amplitude=(1, 1), multiplicity=(1, 1), phase_shift=(0, 0), y_offset=(0, 0), radians: bool = False): """ __init__ This is the Constructor of the 2D wave potential function Parameters ---------- amplitude: tuple, optional absolute min and max of the potential for the cosines in x and y direction, defaults to (1, 1) multiplicity: tuple, optional amount of minima in one phase for the cosines in x and y direction, defaults to (1, 1) phase_shift: tuple, optional position shift of the potential for the cosines in x and y direction, defaults to (0, 0) y_offset: tuple, optional potential shift for the cosines in x and y direction, defaults to (0, 0) radians: bool, optional in radians or degrees, defaults to False """ self.radians = radians nDimensions = 2 self.constants = { "amp_" + str(j): amplitude[j] for j in range(nDimensions) } self.constants.update( {"yOff_" + str(j): y_offset[j] for j in range(nDimensions)}) self.constants.update( {"mult_" + str(j): multiplicity[j] for j in range(nDimensions)}) if (radians): self.constants.update({ "phase_" + str(j): phase_shift[j] for j in range(nDimensions) }) else: self.constants.update({ "phase_" + str(j): np.deg2rad(phase_shift[j]) for j in range(nDimensions) }) super().__init__() def _initialize_functions(self): """ _initialize_functions converts the symbolic mathematics of sympy to a matrix representation that is compatible with multi-dimentionality. """ # Parameters nDimensions = self.constants[self.nDimensions] self.position = sp.Matrix( [sp.symbols("r_" + str(i)) for i in range(nDimensions)]) self.multiplicity = sp.Matrix( [sp.symbols("mult_" + str(i)) for i in range(nDimensions)]) self.phase_shift = sp.Matrix( [sp.symbols("phase_" + str(i)) for i in range(nDimensions)]) self.amplitude = sp.Matrix( [sp.symbols("amp_" + str(i)) for i in range(nDimensions)]) self.yOffset = sp.Matrix( [sp.symbols("yOff_" + str(i)) for i in range(nDimensions)]) # Function self.V_dim = sp.matrix_multiply_elementwise( self.amplitude, (sp.matrix_multiply_elementwise( (self.position + self.phase_shift), self.multiplicity)).applyfunc(sp.cos)) + self.yOffset self.V_functional = sp.Sum(self.V_dim[self.i, 0], (self.i, 0, self.nDimensions - 1)) # OVERRIDE def _update_functions(self): """ _update_functions calculates the current energy and derivative of the energy """ super()._update_functions() self.tmp_Vfunc = self._calculate_energies self.tmp_dVdpfunc = self._calculate_dVdpos self.set_radians(self.radians) def set_phaseshift(self, phaseshift): nDimensions = self.constants[self.nDimensions] self.constants.update( {"phase_" + str(j): phaseshift[j] for j in range(nDimensions)}) self._update_functions() def set_degrees(self, degrees: bool = True): """ Sets output to either degrees or radians Parameters ---------- degrees: bool, optional, if True, output will be given in degrees, otherwise in radians, default: True """ self.radians = bool(not degrees) if (degrees): self._calculate_energies = lambda positions, positions2: self.tmp_Vfunc( np.deg2rad(positions), np.deg2rad(positions2)) self._calculate_dVdpos = lambda positions, positions2: self.tmp_dVdpfunc( np.deg2rad(positions), np.deg2rad(positions2)) else: self.set_radians(radians=not degrees) def set_radians(self, radians: bool = True): """ Sets output to either degrees or radians Parameters ---------- radians: bool, optional, if True, output will be given in radians, otherwise in degree, default: True """ self.radians = radians if (radians): self._calculate_energies = self.tmp_Vfunc self._calculate_dVdpos = self.tmp_dVdpfunc else: self.set_degrees(degrees=bool(not radians))