def integrate_trap(fc,u,t1,t2,N,implementation=2):
grid = np.linspace(t1,t2,N+1)
dt = grid[1] - grid[0]
fs = [fc(u,t) for t in grid] # f on grid
if type(fs[0]) == casadi.SXMatrix:
assert fs[0].numel() == 1
hs = [fabs(f) for f in fs] # absolute values of f on grid
F=0
if implementation==1:
# This formulation theoretically allows short-circuiting, branch prediction
for i in xrange(len(fs)-1):
fa=fs[i]
fb=fs[i+1]
ha=hs(i)
hb=hs(i+1)
samesign=casadi.sign(fa)==casadi.sign(fb)
F=F+casadi.if_else(samesign,trapezoid_area(ha,hb,dt),
bowtie_area(ha,hb,dt))
if implementation==2:
# This formulation theoretically allows use of SIMD (vector) extensions
for i in xrange(len(fs)-1):
fa=fs[i]
fb=fs[i+1]
ha=hs[i]
hb=hs[i+1]
ha_plus_hb=ha+hb
F=F+ha_plus_hb + (fa*fb-ha*hb)/ha_plus_hb
F=F*dt/2
if type(F) == casadi.SXMatrix:
assert F.numel() == 1
return F,grid
def get_in_tangent_cone_function_multidim(self, cnstr):
"""Returns a casadi function for the SetConstraint instance when the
SetConstraint is multidimensional."""
if not isinstance(cnstr, SetConstraint):
raise TypeError("in_tangent_cone is only available for" +
" SetConstraint")
time_var = self.skill_spec.time_var
robot_var = self.skill_spec.robot_var
list_vars = [time_var, robot_var]
list_names = ["time_var", "robot_var"]
robot_vel_var = self.skill_spec.robot_vel_var
opt_var = [robot_vel_var]
opt_var_names = ["robot_vel_var"]
virtual_var = self.skill_spec.virtual_var
virtual_vel_var = self.skill_spec.virtual_vel_var
input_var = self.skill_spec.input_var
expr = cnstr.expression
set_min = cnstr.set_min
set_max = cnstr.set_max
dexpr = cs.jacobian(expr, time_var)
dexpr += cs.jtimes(expr, robot_var, robot_vel_var)
if virtual_var is not None:
list_vars += [virtual_var]
list_names += ["virtual_var"]
opt_var += [virtual_vel_var]
opt_var_names += ["virtual_vel_var"]
dexpr += cs.jtimes(expr, virtual_var, virtual_vel_var)
if input_var is not None:
list_vars += [input_var]
list_vars += ["input_var"]
le = expr - set_min
ue = expr - set_max
le_good = le >= 1e-12
ue_good = ue <= 1e-12
above = cs.dot(le_good - 1, le_good - 1) == 0
below = cs.dot(ue_good - 1, ue_good - 1) == 0
inside = cs.logic_and(above, below)
out_dir = (cs.sign(le) + cs.sign(ue)) / 2.0
# going_in = cs.dot(out_dir, dexpr) <= 0.0
same_signs = cs.sign(le) == cs.sign(ue)
corner = cs.dot(same_signs - 1, same_signs - 1) == 0
dists = (cs.norm_2(dexpr) + 1e-10) * cs.norm_2(out_dir)
corner_handler = cs.if_else(
cs.dot(out_dir, dexpr) < 0.0,
cs.fabs(cs.dot(-out_dir, dexpr)) / dists < cs.np.cos(cs.np.pi / 4),
False, True)
going_in = cs.if_else(corner, corner_handler,
cs.dot(out_dir, dexpr) < 0.0, True)
in_tc = cs.if_else(
inside, # Are we inside?
True, # Then true.
going_in, # If not, check if we're "going_in"
True)
return cs.Function("in_tc_" + cnstr.label.replace(" ", "_"),
list_vars + opt_var, [in_tc],
list_names + opt_var_names,
["in_tc_" + cnstr.label])
def if_greater_zero(condition, if_result, else_result):
"""
:type condition: Union[float, Symbol]
:type if_result: Union[float, Symbol]
:type else_result: Union[float, Symbol]
:return: if_result if condition > 0 else else_result
:rtype: Union[float, Symbol]
"""
_condition = sign(condition) # 1 or -1
_if = Max(0, _condition) * if_result # 0 or if_result
_else = -Min(0, _condition) * else_result # 0 or else_result
return _if + _else + (1 - Abs(_condition)) * else_result # if_result or else_result
def diffable_sign(x):
"""
!Returns shit if x is very close to but not equal to zero!
if x > 0:
return 1
if x < 0:
return -1
if x == 0:
return 0
:type x: Union[float, Symbol]
:return: sign(x)
:rtype: Union[float, Symbol]
"""
return ca.sign(x)
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.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)
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 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)
)
)
# Declare variables (use scalar graph) t = ca.SX.sym( 't' ) # time u = ca.SX.sym( 'u' ) # control x = ca.SX.sym( 'x' , 4 ) # state # ODE rhs function # x[0] = dtheta # x[1] = theta # x[2] = dx # x[3] = x sint = ca.sin( x[1] ) cost = ca.cos( x[1] ) fric = mic * ca.sign( x[2] ) num = g * sint + cost * ( -u - mp * l * x[0] * x[0] * sint + fric ) / ( mc + mp ) - mip * x[0] / ( mp * l ) denom = l * ( 4. / 3. - mp * cost*cost / (mc + mp) ) ddtheta = num / denom ddx = (-u + mp * l * ( x[0] * x[0] * sint - ddtheta * cost ) - fric) / (mc + mp) x_err = x-x_target cost_mat = np.diag( [1,1,1,1] ) ode = ca.vertcat([ ddtheta, \ x[0], \ ddx, \ x[2] ] ) # ca.mul( [ x_err.T, cost_mat, x_err ] )
# Declare variables (use scalar graph)
t = ca.SX.sym('t') # time
u = ca.SX.sym('u') # control
x = ca.SX.sym('x', 4) # state
# ODE rhs function
# x[0] = dtheta
# x[1] = theta
# x[2] = dx
# x[3] = x
sint = ca.sin(x[1])
cost = ca.cos(x[1])
fric = mic * ca.sign(x[2])
num = g * sint + cost * (-u - mp * l * x[0] * x[0] * sint +
fric) / (mc + mp) - mip * x[0] / (mp * l)
denom = l * (4. / 3. - mp * cost * cost / (mc + mp))
ddtheta = num / denom
ddx = (-u + mp * l * (x[0] * x[0] * sint - ddtheta * cost) - fric) / (mc + mp)
x_err = x - x_target
cost_mat = np.diag([1, 1, 1, 1])
ode = ca.vertcat([ ddtheta, \
x[0], \
ddx, \
x[2]
] )
def force_moment(x: State, u: Control, p: Parameters):
"""
The function computes the forces and moments acting on the aircraft.
It is important to separate this from the dynamics as the Gazebo
simulator will be used to simulate extra forces and moments
from collision.
"""
# functions
cos = ca.cos
sin = ca.sin
# parameters
weight = p.weight
g = p.g
hx = p.hx
b = p.b
cbar = p.cbar
s = p.s
xcg = p.xcg
xcgr = p.xcgr
# state
VT = x.VT
alpha = x.alpha
beta = x.beta
phi = x.phi
theta = x.theta
P = x.P
Q = x.Q
R = x.R
alt = x.alt
power = x.power
ail_deg = x.ail_deg
elv_deg = x.elv_deg
rdr_deg = x.rdr_deg
# mass properties
mass = weight / g
# air data computer and engine model
amach = tables['amach'](VT, alt)
qbar = tables['qbar'](VT, alt)
thrust = tables['thrust'](power, alt, amach)
# force component buildup
rad2deg = 180 / np.pi
alpha_deg = rad2deg * alpha
beta_deg = rad2deg * beta
dail = ail_deg / 20.0
drdr = rdr_deg / 30.0
cxt = tables['Cx'](alpha_deg, elv_deg)
cyt = tables['Cy'](beta_deg, ail_deg, rdr_deg)
czt = tables['Cz'](alpha_deg, beta_deg, elv_deg)
clt = ca.sign(beta_deg)*tables['Cl'](alpha_deg, beta_deg) \
+ tables['DlDa'](alpha_deg, beta_deg)*dail \
+ tables['DlDr'](alpha_deg, beta_deg)*drdr
cmt = tables['Cm'](alpha_deg, elv_deg)
cnt = ca.sign(beta_deg)*tables['Cn'](alpha_deg, beta_deg) \
+ tables['DnDa'](alpha_deg, beta_deg)*dail \
+ tables['DnDr'](alpha_deg, beta_deg)*drdr
# damping
tvt = 0.5 / VT
b2v = b * tvt
cq = cbar * Q * tvt
cxt += cq * tables['CXq'](alpha_deg)
cyt += b2v * (tables['CYr'](alpha_deg) * R + tables['CYp'](alpha_deg) * P)
czt += cq * tables['CZq'](alpha_deg)
clt += b2v * (tables['Clr'](alpha_deg) * R + tables['Clp'](alpha_deg) * P)
cmt += cq * tables['Cmq'](alpha_deg) + czt * (xcgr - xcg)
cnt += b2v * (tables['Cnr'](alpha_deg) * R + tables['Cnp']
(alpha_deg) * P) - cyt * (xcgr - xcg) * cbar / b
# get ready for state equations
sth = sin(theta)
cth = cos(theta)
sph = sin(phi)
cph = cos(phi)
qs = qbar * s
qsb = qs * b
rmqs = qs / mass
gcth = g * cth
ay = rmqs * cyt
az = rmqs * czt
qhx = Q * hx
# force
Fx = -mass * g * sth + qs * cxt + thrust
Fy = mass * (gcth * sph + ay)
Fz = mass * (gcth * cph + az)
# moment
Mx = qsb * clt # roll
My = qs * cbar * cmt - R * hx # pitch
Mz = qsb * cnt + qhx # yaw
return ca.vertcat(Fx, Fy, Fz), ca.vertcat(Mx, My, Mz)
def _get_diff_eq(self, cost_func):
'''Function that returns the RhS of the differential equations. See the papers for additional info'''
RHS = []
# RHS that's in common for all cases
rhs1 = self.q_dot
rhs2 = self.M_inv @ (-self.Cq - self.G - self.Fd @ self.q_dot - self.Ff @ cs.sign(self.q_dot))
# Adjusting RHS for SEA with known inertia. Check the paper for more info.
if self.sea and self.SEAdynamics:
rhs2 -= self.M_inv @ self.tau_sea
rhs3 = self.theta_dot
rhs4 = cs.pinv(self.B) @ (-self.FDsea @ self.theta_dot + self.u + self.tau_sea - self.FMusea @ cs.sign(self.theta_dot))
RHS = [rhs1, rhs2, rhs3, rhs4]
# Adjusting the lenght of the variables
self.x = cs.vertcat(self.q, self.q_dot, self.theta, self.theta_dot)
self.num_state_var = self.num_joints * 2
self.lower_q = self.lower_q + self.lower_theta
self.lower_qd = self.lower_qd + self.lower_thetad
self.upper_q = self.upper_q + self.upper_theta
self.upper_qd = self.upper_qd + self.upper_thetad
# Adjusting RHS for SEA modeling, with motor inertia unknown
elif self.sea and not self.SEAdynamics:
rhs2 += -self.M_inv @ self.K @ (self.q - self.u)
# self.upper_u, self.lower_u = self.upper_q, self.lower_q
RHS = [rhs1, rhs2]
# State variable
self.x = cs.vertcat(self.q, self.q_dot)
self.num_state_var = self.num_joints
# Adjusting RHS for classic robot modeling, without any SEA
else:
rhs2 += self.M_inv @ self.u
RHS = [rhs1, rhs2]
# State variable
self.x = cs.vertcat(self.q, self.q_dot)
self.num_state_var = self.num_joints
# Evaluating q_ddot, in order to handle it when given as an input of cost function or constraints
self.q_ddot_val = self._get_joints_accelerations(rhs2)
# The differentiation of J will be the cost function given by the user, with our symbolic
# variables as inputs
if self.sea and self.SEAdynamics:
q_ddot_J = self.q_ddot_val(self.q, self.q_dot, self.theta, self.theta_dot, self.u)
else:
q_ddot_J = self.q_ddot_val(self.q,self.q_dot, self.u)
if self.sea and not self.SEAdynamics:
u_J = self.u - self.q # the instantaneous spent energy is prop to |u-q|
else:
u_J = self.u
J_dot = cost_func( self.q - self.traj,
self.q_dot - self.traj_dot,
q_ddot_J,
self.ee_pos(self.q),
u_J,
self.t
)
# Setting the relationship
self.x_dot = cs.vertcat(*RHS)
# Defining the differential equation
f = cs.Function('f', [self.x, self.u, self.t], # inputs
[self.x_dot, J_dot]) # outputs
return f
