def fmod(a, b): return ca.fmod(a, b)
opti.subject_to(-1 <= u) opti.subject_to(u <= 1) def convert_joint_space_to_cartesian(p): x = LINK_LENGTH * casadi.cos(p[0] + math.pi / 2) y = LINK_LENGTH * casadi.sin(p[0] + math.pi / 2) return x, y # boundary conditions opti.subject_to(pos[0] == 0) opti.subject_to(speed[0] == 0) opti.subject_to(u[0] == 0) opti.subject_to( casadi.fabs(casadi.fmod(pos[N_idx + 1], 2 * np.pi)) == FINAL_POSITION) # finish at final pose but include wrapping! opti.subject_to(speed[N_idx + 1] == 0) # finish at 0 velocity opti.subject_to(tf >= 0) # time has to be positive opti.subject_to(tf <= 8.0) # bang bang min is 3.9 # initial guess # linear_position_guess = np.linspace(0, FINAL_POSITION, N+1) linear_position_guess = np.cos(np.linspace(0, 2 * FINAL_POSITION, N + 1)) * FINAL_POSITION # opti.set_initial(pos, linear_position_guess) opti.set_initial(u, np.cos(np.linspace(0, 2 * np.pi, N))) opti.solver('ipopt')
def _convert(self, symbol, t, y, y_dot, inputs): """ See :meth:`CasadiConverter.convert()`. """ if isinstance( symbol, ( pybamm.Scalar, pybamm.Array, pybamm.Time, pybamm.InputParameter, pybamm.ExternalVariable, ), ): return casadi.MX(symbol.evaluate(t, y, y_dot, inputs)) elif isinstance(symbol, pybamm.StateVector): if y is None: raise ValueError( "Must provide a 'y' for converting state vectors") return casadi.vertcat(*[y[y_slice] for y_slice in symbol.y_slices]) elif isinstance(symbol, pybamm.StateVectorDot): if y_dot is None: raise ValueError( "Must provide a 'y_dot' for converting state vectors") return casadi.vertcat( *[y_dot[y_slice] for y_slice in symbol.y_slices]) elif isinstance(symbol, pybamm.BinaryOperator): left, right = symbol.children # process children converted_left = self.convert(left, t, y, y_dot, inputs) converted_right = self.convert(right, t, y, y_dot, inputs) if isinstance(symbol, pybamm.Modulo): return casadi.fmod(converted_left, converted_right) if isinstance(symbol, pybamm.Minimum): return casadi.fmin(converted_left, converted_right) if isinstance(symbol, pybamm.Maximum): return casadi.fmax(converted_left, converted_right) # _binary_evaluate defined in derived classes for specific rules return symbol._binary_evaluate(converted_left, converted_right) elif isinstance(symbol, pybamm.UnaryOperator): converted_child = self.convert(symbol.child, t, y, y_dot, inputs) if isinstance(symbol, pybamm.AbsoluteValue): return casadi.fabs(converted_child) if isinstance(symbol, pybamm.Floor): return casadi.floor(converted_child) if isinstance(symbol, pybamm.Ceiling): return casadi.ceil(converted_child) return symbol._unary_evaluate(converted_child) elif isinstance(symbol, pybamm.Function): converted_children = [ self.convert(child, t, y, y_dot, inputs) for child in symbol.children ] # Special functions if symbol.function == np.min: return casadi.mmin(*converted_children) elif symbol.function == np.max: return casadi.mmax(*converted_children) elif symbol.function == np.abs: return casadi.fabs(*converted_children) elif symbol.function == np.sqrt: return casadi.sqrt(*converted_children) elif symbol.function == np.sin: return casadi.sin(*converted_children) elif symbol.function == np.arcsinh: return casadi.arcsinh(*converted_children) elif symbol.function == np.arccosh: return casadi.arccosh(*converted_children) elif symbol.function == np.tanh: return casadi.tanh(*converted_children) elif symbol.function == np.cosh: return casadi.cosh(*converted_children) elif symbol.function == np.sinh: return casadi.sinh(*converted_children) elif symbol.function == np.cos: return casadi.cos(*converted_children) elif symbol.function == np.exp: return casadi.exp(*converted_children) elif symbol.function == np.log: return casadi.log(*converted_children) elif symbol.function == np.sign: return casadi.sign(*converted_children) elif symbol.function == special.erf: return casadi.erf(*converted_children) elif isinstance(symbol.function, (PchipInterpolator, CubicSpline)): return casadi.interpolant("LUT", "bspline", [symbol.x], symbol.y)(*converted_children) elif symbol.function.__name__.startswith("elementwise_grad_of_"): differentiating_child_idx = int(symbol.function.__name__[-1]) # Create dummy symbolic variables in order to differentiate using CasADi dummy_vars = [ casadi.MX.sym("y_" + str(i)) for i in range(len(converted_children)) ] func_diff = casadi.gradient( symbol.differentiated_function(*dummy_vars), dummy_vars[differentiating_child_idx], ) # Create function and evaluate it using the children casadi_func_diff = casadi.Function("func_diff", dummy_vars, [func_diff]) return casadi_func_diff(*converted_children) # Other functions else: return symbol._function_evaluate(converted_children) elif isinstance(symbol, pybamm.Concatenation): converted_children = [ self.convert(child, t, y, y_dot, inputs) for child in symbol.children ] if isinstance(symbol, (pybamm.NumpyConcatenation, pybamm.SparseStack)): return casadi.vertcat(*converted_children) # DomainConcatenation specifies a particular ordering for the concatenation, # which we must follow elif isinstance(symbol, pybamm.DomainConcatenation): slice_starts = [] all_child_vectors = [] for i in range(symbol.secondary_dimensions_npts): child_vectors = [] for child_var, slices in zip(converted_children, symbol._children_slices): for child_dom, child_slice in slices.items(): slice_starts.append( symbol._slices[child_dom][i].start) child_vectors.append( child_var[child_slice[i].start:child_slice[i]. stop]) all_child_vectors.extend([ v for _, v in sorted(zip(slice_starts, child_vectors)) ]) return casadi.vertcat(*all_child_vectors) else: raise TypeError(""" Cannot convert symbol of type '{}' to CasADi. Symbols must all be 'linear algebra' at this stage. """.format(type(symbol)))