def getKinematics(self, q, dq):
p0 = [self.p0[0], self.p0[1]]
p = ca.MX.zeros(5, 2)
c = ca.MX.zeros(5, 2)
p0 = [self.p0[0], self.p0[1]]
p[0, 0], p[0, 1] = self.length[0] * ca.sin(
q[0]) + p0[0], self.length[0] * ca.cos(q[0]) + p0[1]
p[1, 0], p[1, 1] = self.length[1] * ca.sin(
q[1]) + p[0, 0], self.length[1] * ca.cos(q[1]) + p[0, 1]
p[2, 0], p[2, 1] = self.length[2] * ca.sin(
q[2]) + p[1, 0], self.length[2] * ca.cos(q[2]) + p[1, 1]
p[3, 0], p[3, 1] = self.length[3] * ca.sin(
q[3]) + p[1, 0], -self.length[3] * ca.cos(q[3]) + p[1, 1]
p[4, 0], p[4, 1] = self.length[4] * ca.sin(
q[4]) + p[3, 0], -self.length[4] * ca.cos(q[4]) + p[3, 1]
c[0, 0], c[0, 1] = self.length[0] * ca.sin(
q[0]) / 2 + p0[0], self.length[0] * ca.cos(q[0]) / 2 + p0[1]
c[1, 0], c[1, 1] = self.length[1] * ca.sin(
q[1]) / 2 + p[0, 0], self.length[1] * ca.cos(q[1]) / 2 + p[0, 1]
c[2, 0], c[2, 1] = self.length[2] * ca.sin(
q[2]) / 2 + p[1, 0], self.length[2] * ca.cos(q[2]) / 2 + p[1, 1]
c[3, 0], c[3, 1] = self.length[3] * ca.sin(
q[3]) / 2 + p[1, 0], -self.length[3] * ca.cos(q[3]) / 2 + p[1, 1]
c[4, 0], c[4, 1] = self.length[4] * ca.sin(
q[4]) / 2 + p[3, 0], -self.length[4] * ca.cos(q[4]) / 2 + p[3, 1]
dp = ca.jtimes(p, q, dq)
dc = ca.jtimes(c, q, dq)
ddp = ca.jtimes(dp, q, dq)
ddc = ca.jtimes(dc, q, dq)
return p, dp, ddp, c, dc, ddc
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 getKinematics(self, q, dq, left, right):
p0 = left
p5 = right
p = ca.MX.zeros(6, 2)
c = ca.MX.zeros(5, 2)
p[0, 0], p[0, 1] = p0[0], p0[1]
p[1, 0], p[1, 1] = self.length[0] * ca.sin(
q[0]) + p[0, 0], self.length[0] * ca.cos(q[0]) + p[0, 1]
p[2, 0], p[2, 1] = self.length[1] * ca.sin(
q[1]) + p[1, 0], self.length[1] * ca.cos(q[1]) + p[1, 1]
p[5, 0], p[5, 1] = p5[0], p5[1]
p[4, 0], p[4, 1] = self.length[4] * ca.sin(
q[4]) + p[5, 0], self.length[4] * ca.cos(q[4]) + p[5, 1]
prx, pry = self.length[3] * ca.sin(
q[3]) + p[4, 0], self.length[3] * ca.cos(q[3]) + p[4, 1]
self.opti.subject_to(p[2, 0] == prx)
self.opti.subject_to(p[2, 1] == pry)
p[3, 0], p[3, 1] = self.length[2] * ca.sin(
q[2]) + p[2, 0], self.length[2] * ca.cos(q[2]) + p[2, 1]
c[0, 0], c[0, 1] = self.length[0] * ca.sin(
q[0]) / 2 + p[0, 0], self.length[0] * ca.cos(q[0]) / 2 + p[0, 0]
c[1, 0], c[1, 1] = self.length[1] * ca.sin(
q[1]) / 2 + p[1, 0], self.length[1] * ca.cos(q[1]) / 2 + p[1, 1]
c[2, 0], c[2, 1] = self.length[2] * ca.sin(
q[2]) / 2 + p[2, 0], self.length[2] * ca.cos(q[2]) / 2 + p[2, 1]
c[3, 0], c[3, 1] = self.length[3] * ca.sin(
q[3]) / 2 + p[4, 0], self.length[3] * ca.cos(q[3]) / 2 + p[4, 1]
c[4, 0], c[4, 1] = self.length[4] * ca.sin(
q[4]) / 2 + p[5, 0], self.length[4] * ca.cos(q[4]) / 2 + p[5, 1]
dp = ca.jtimes(p, q, dq)
dc = ca.jtimes(c, q, dq)
ddp = ca.jtimes(dp, q, dq)
ddc = ca.jtimes(dc, q, dq)
# self.opti.subject_to(ca.norm_2(p[1, :]-p[0, :]) == self.length[0])
# self.opti.subject_to(ca.norm_2(p[2, :]-p[1, :]) == self.length[1])
# self.opti.subject_to(ca.norm_2(p[3, :]-p[2, :]) == self.length[2])
# self.opti.subject_to(ca.norm_2(p[4, :]-p[2, :]) == self.length[3])
# self.opti.subject_to(ca.norm_2(p[5, :]-p[4, :]) == self.length[4])
return p, dp, ddp, c, dc, ddc
def getModel(self, state, u):
q = state[0]
dq = state[1]
u = u[0]
p0 = [self.p0[0], self.p0[1]]
p, dp, g, dc = self.getKinematics(q, dq)
ddc10, ddc11 = ca.jtimes(dc[0][0], dq, dq), ca.jtimes(dc[0][1], dq, dq)
ddc20, ddc21 = ca.jtimes(dc[0][0], dq, dq), ca.jtimes(dc[0][1], dq, dq)
ddc30, ddc31 = ca.jtimes(dc[0][0], dq, dq), ca.jtimes(dc[0][1], dq, dq)
ddc40, ddc41 = ca.jtimes(dc[0][0], dq, dq), ca.jtimes(dc[0][1], dq, dq)
ddc50, ddc51 = ca.jtimes(dc[0][0], dq, dq), ca.jtimes(dc[0][1], dq, dq)
ddc = [[ddc10, ddc11], [ddc20, ddc21], [ddc30, ddc31], [ddc40, ddc41],
[ddc50, ddc51]]
s = [0, 0, 0, 0, 0]
for i in range(5):
s[0] += (((g[i][0] - p0[0]) * self.m[i] * self.g)) + (
((g[i][0] - p0[0]) * self.m[i] * ddc[i][1]) -
((g[i][1]) * self.m[i] * ddc[i][0]))
if i > 0:
s[1] += (((g[i][0] - p[0][0]) * self.m[i] * self.g)) + (
((g[i][0] - p[0][0]) * self.m[i] * ddc[i][1]) -
((g[i][1] - p[0][1]) * self.m[i] * ddc[i][0]))
if i > 1:
s[2] += (((g[i][0] - p[1][0]) * self.m[i] * self.g)) + (
((g[i][0] - p[1][0]) * self.m[i] * ddc[i][1]) -
((g[i][1] - p[1][1]) * self.m[i] * ddc[i][0]))
if i > 2:
s[3] += ((
(g[i][0] - p[1][0]) * self.m[i] * self.g)) + (
((g[i][0] - p[1][0]) * self.m[i] * ddc[i][1]) -
((g[i][1] - p[1][1]) * self.m[i] * ddc[i][0]))
if i > 3:
s[4] += (
((g[i][0] - p[3][0]) * self.m[i] * self.g)
) + ((
(g[i][0] - p[3][0]) * self.m[i] * ddc[i][1]) -
((g[i][1] - p[3][1]) * self.m[i] * ddc[i][0]))
ddq5 = (u[3] - s[4]) / self.i1
ddq4 = ((u[2] - s[3]) - (self.i1 * ddq5)) / self.i2
ddq3 = ((u[1] - s[2]) - (self.i1 * ddq5) - (self.i2 * ddq4)) / self.i3
ddq2 = ((u[0] - s[1]) - (self.i1 * ddq5) - (self.i2 * ddq4) -
(self.i3 * ddq3)) / self.i2
ddq1 = ((-s[0]) - (self.i1 * ddq5) - (self.i2 * ddq4) -
(self.i3 * ddq3) - (self.i2 * ddq4)) / self.i1
ddq = [ddq1, ddq2, ddq3, ddq4, ddq5]
return p, dp, g, dc, ddq
def der(self, expr):
if depends_on(expr, self.u):
raise Exception(
"Dependency on controls not supported yet for stage.der")
ode = self._ode()
return jtimes(
expr, self.x,
ode(x=self.x, u=self.u, z=self.z, p=self.p, t=self.t)["ode"])
def getKinematics(self):
# Assume p,c as 2x1
##################
#####--Leg--######
##################
p = ca.MX.zeros(2, 3)
c = ca.MX.zeros(2, 3)
p0 = self.p0
p[0, 0], p[1, 0] = self.length[0] * ca.sin(self.q[0]) + p0[
0, 0], self.length[0] * ca.cos(self.q[0]) + p0[1, 0]
p[0, 1], p[1, 1] = self.length[1] * ca.sin(
self.q[1]) + p[0, 0], self.length[1] * ca.cos(self.q[1]) + p[1, 0]
p[0, 2], p[1, 2] = self.length[2] * ca.sin(
self.q[2]) + p[0, 1], self.length[2] * ca.cos(self.q[2]) + p[1, 1]
c[0, 0], c[1, 0] = self.length[0] * ca.sin(self.q[0]) / 2 + p0[
0, 0], self.length[0] * ca.cos(self.q[0]) / 2 + p0[1, 0]
c[0, 1], c[1, 1] = self.length[1] * ca.sin(self.q[1]) / 2 + p[
0, 0], self.length[1] * ca.cos(self.q[1]) / 2 + p[1, 0]
c[0, 2], c[1, 2] = self.length[2] * ca.sin(self.q[2]) / 2 + p[
0, 1], self.length[2] * ca.cos(self.q[2]) / 2 + p[1, 1]
###--Derivatives--###
dp = ca.jtimes(p, self.q, self.dq) + self.dp0
dc = ca.jtimes(c, self.q, self.dq) + self.dp0
ddp = ca.jtimes(dp, self.q, self.dq)
ddc = ca.jtimes(dc, self.q, self.dq)
###--Form a dictionary--###
p = {'Leg': p, 'Constraint': (self.q[0] <= self.q[1])}
dp = {'Leg': dp}
ddp = {'Leg': ddp}
c = {'Leg': c}
dc = {'Leg': dc}
ddc = {'Leg': ddc}
return p, dp, ddp, c, dc, ddc
def get_in_tangent_cone_function(self, cnstr):
"""Returns a casadi function for the SetConstraint instance."""
if not isinstance(cnstr, SetConstraint):
raise TypeError("in_tangent_cone is only available for" +
" SetConstraint")
return None
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_names += ["input_var"]
if_low_inc = cs.if_else(dexpr > 0., True, False, True)
if_high_dec = cs.if_else(dexpr < 0., True, False, True)
leq_high = cs.if_else(expr - set_max < 1e-12, True, if_high_dec, True)
in_tc = cs.if_else(set_min - expr < 1e-12, leq_high, if_low_inc, 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 getKinematics(self, q, dq):
p10 = ca.MX.sym('p10', 1)
c10 = ca.MX.sym('g10', 1)
p11 = ca.MX.sym('p11', 1)
c11 = ca.MX.sym('g11', 1)
p20 = ca.MX.sym('p20', 1)
c20 = ca.MX.sym('g20', 1)
p21 = ca.MX.sym('p21', 1)
c21 = ca.MX.sym('g21', 1)
p30 = ca.MX.sym('p30', 1)
c30 = ca.MX.sym('g30', 1)
p31 = ca.MX.sym('p31', 1)
c31 = ca.MX.sym('g31', 1)
p40 = ca.MX.sym('p40', 1)
c40 = ca.MX.sym('g40', 1)
p41 = ca.MX.sym('p41', 1)
c41 = ca.MX.sym('g41', 1)
p50 = ca.MX.sym('p50', 1)
c50 = ca.MX.sym('g50', 1)
p51 = ca.MX.sym('p51', 1)
c51 = ca.MX.sym('g51', 1)
p0 = [self.p0[0], self.p0[1]]
p10, p11 = -self.l1 * ca.sin(q[0]) + p0[0], self.l1 * ca.cos(
q[0]) + p0[1]
p20, p21 = -self.l2 * ca.sin(q[1]) + p10, self.l2 * ca.cos(q[1]) + p11
p30, p31 = self.l3 * ca.sin(q[2]) + p20, self.l3 * ca.cos(q[2]) + p21
p40, p41 = (self.l2 * ca.sin(q[3])) + p20, (-self.l2 *
ca.cos(q[3])) + p21
p50, p51 = (self.l1 * ca.sin(q[4])) + p40, (-self.l1 *
ca.cos(q[4])) + p41
dp10, dp11 = ca.jtimes(p10, ca.MX(q), dq), ca.jtimes(p11, ca.MX(q), dq)
dp20, dp21 = ca.jtimes(p20, ca.MX(q), dq), ca.jtimes(p21, ca.MX(q), dq)
dp30, dp31 = ca.jtimes(p30, ca.MX(q), dq), ca.jtimes(p31, ca.MX(q), dq)
dp40, dp41 = ca.jtimes(p40, ca.MX(q), dq), ca.jtimes(p41, ca.MX(q), dq)
dp50, dp51 = ca.jtimes(p50, ca.MX(q), dq), ca.jtimes(p51, ca.MX(q), dq)
p = [[p10, p11], [p20, p21], [p30, p31], [p40, p41], [p50, p51]]
dp = [[dp10, dp11], [dp20, dp21], [dp30, dp31], [dp40, dp41],
[dp50, dp51]]
##########################################
c10, c11 = -self.l1 * ca.sin(q[0]) / 2 + p0[0], self.l1 * ca.cos(
q[0]) / 2 + p0[0]
c20, c21 = -self.l2 * ca.sin(q[1]) / 2 + p10, self.l2 * ca.cos(
q[1]) / 2 + p11
c30, c31 = self.l3 * ca.sin(q[2]) / 2 + p20, self.l3 * ca.cos(
q[2]) / 2 + p21
c40, c41 = (self.l2 * ca.sin(q[3]) / 2) + p20, (-self.l2 *
ca.cos(q[3]) / 2) + p21
c50, c51 = (self.l1 * ca.sin(q[4]) / 2) + p40, (-self.l1 *
ca.cos(q[4]) / 2) + p41
# print(c1)
# print(c2)
dc10, dc11 = ca.jtimes(c10, ca.MX(q), dq), ca.jtimes(c11, ca.MX(q), dq)
dc20, dc21 = ca.jtimes(c20, ca.MX(q), dq), ca.jtimes(c21, ca.MX(q), dq)
dc30, dc31 = ca.jtimes(c30, ca.MX(q), dq), ca.jtimes(c31, ca.MX(q), dq)
dc40, dc41 = ca.jtimes(c40, ca.MX(q), dq), ca.jtimes(c41, ca.MX(q), dq)
dc50, dc51 = ca.jtimes(c50, ca.MX(q), dq), ca.jtimes(c51, ca.MX(q), dq)
g = [[c10, c11], [c20, c21], [c30, c31], [c40, c41], [c50, c51]]
dc = [[dc10, dc11], [dc20, dc21], [dc30, dc31], [dc40, dc41],
[dc50, dc51]]
# print(dc1)
return p, dp, g, dc
def Kinematics(self, q, dq):
p1x = ca.MX.sym('p1x', 1)
p1y = ca.MX.sym('p1y', 1)
p2x = ca.MX.sym('p2x', 1)
p2y = ca.MX.sym('p2y', 1)
p3x = ca.MX.sym('p3x', 1)
p3y = ca.MX.sym('p3y', 1)
p4x = ca.MX.sym('p4x', 1)
p4y = ca.MX.sym('p4y', 1)
p5x = ca.MX.sym('p5x', 1)
p5y = ca.MX.sym('p5y', 1)
g1x = ca.MX.sym('g1x', 1)
g1y = ca.MX.sym('g1y', 1)
g2x = ca.MX.sym('g2x', 1)
g2y = ca.MX.sym('g2y', 1)
g3x = ca.MX.sym('g3x', 1)
g3y = ca.MX.sym('g3y', 1)
g4x = ca.MX.sym('g4x', 1)
g4y = ca.MX.sym('g4y', 1)
g5x = ca.MX.sym('g5x', 1)
g5y = ca.MX.sym('g5y', 1)
p1x = -self.l1 * ca.sin(q[0])
p1y = self.l1 * ca.cos(q[0])
g1x = -self.l1 * ca.sin(q[0]) / 2
g1y = self.l1 * ca.cos(q[0]) / 2
p2x = p1x + (-self.l2 * ca.sin(q[1]))
p2y = p1y + (self.l2 * ca.cos(q[1]))
g2x = p1x + (-self.l2 * ca.sin(q[1])) / 2
g2y = p1y + (self.l2 * ca.cos(q[1])) / 2
p3x = p2x + (self.l3 * ca.sin(q[2]))
p3y = p2y + (self.l3 * ca.cos(q[2]))
g3x = p2x + (self.l3 * ca.sin(q[2])) / 2
g3y = p2y + (self.l3 * ca.cos(q[2])) / 2
p4x = p2x + (self.l4 * ca.sin(q[3]))
p4y = p2y + (-self.l4 * ca.cos(q[3]))
g4x = p2x + (self.l4 * ca.sin(q[3])) / 2
g4y = p2y + (-self.l4 * ca.cos(q[3])) / 2
p5x = p4x + (self.l5 * ca.sin(q[4]))
p5y = p4y + (-self.l5 * ca.cos(q[4]))
g5x = p4x + (self.l5 * ca.sin(q[4])) / 2
g5y = p4y + (-self.l5 * ca.cos(q[4])) / 2
dp1x, dp1y = ca.jtimes(p1x, q, dq), ca.jtimes(p1y, q, dq)
dp2x, dp2y = ca.jtimes(p2x, q, dq), ca.jtimes(p2y, q, dq)
dp3x, dp3y = ca.jtimes(p3x, q, dq), ca.jtimes(p3y, q, dq)
dp4x, dp4y = ca.jtimes(p4x, q, dq), ca.jtimes(p4y, q, dq)
dp5x, dp5y = ca.jtimes(p5x, q, dq), ca.jtimes(p5y, q, dq)
dg1x, dg1y = ca.jtimes(g1x, q, dq), ca.jtimes(g1y, q, dq)
dg2x, dg2y = ca.jtimes(g2x, q, dq), ca.jtimes(g2y, q, dq)
dg3x, dg3y = ca.jtimes(g3x, q, dq), ca.jtimes(g3y, q, dq)
dg4x, dg4y = ca.jtimes(g4x, q, dq), ca.jtimes(g4y, q, dq)
dg5x, dg5y = ca.jtimes(g5x, q, dq), ca.jtimes(g5y, q, dq)
ddg1x, ddg1y = ca.jtimes(g1x, dq, dq), ca.jtimes(g1y, dq, dq)
ddg2x, ddg2y = ca.jtimes(g2x, dq, dq), ca.jtimes(g2y, dq, dq)
ddg3x, ddg3y = ca.jtimes(g3x, dq, dq), ca.jtimes(g3y, dq, dq)
ddg4x, ddg4y = ca.jtimes(g4x, dq, dq), ca.jtimes(g4y, dq, dq)
ddg5x, ddg5y = ca.jtimes(g5x, dq, dq), ca.jtimes(g5y, dq, dq)
P = [[p1x, p1y], [p2x, p2y], [p3x, p3y], [p4x, p4y], [p5x, p5y]]
dP = [[dp1x, dp1y], [dp2x, dp2y], [dp3x, dp3y], [dp4x, dp4y],
[dp5x, dp5y]]
G = [[g1x, g1y], [g2x, g2y], [g3x, g3y], [g4x, g4y], [g5x, g5y]]
dG = [[dg1x, dg1y], [dg2x, dg2y], [dg3x, dg3y], [dg4x, dg4y],
[dg5x, dg5y]]
ddG = [[ddg1x, ddg1y], [ddg2x, ddg2y], [ddg3x, ddg3y], [ddg4x, ddg4y],
[ddg5x, ddg5y]]
return P, dP, G, dG, ddG
def get_flat_maps(ntrailers):
'''
returns expressions for state variables [x, y, phi, theta...]
as functions of outputs [y0, y1, Dy0, Dy1, ...]
'''
out = SX.zeros(ntrailers + 3, 2)
out[0, 0] = SX.sym(f'x{ntrailers - 1}')
out[0, 1] = SX.sym(f'y{ntrailers - 1}')
for i in range(1, ntrailers + 3):
out[i, 0] = SX.sym(f'D{i}x{ntrailers-1}')
out[i, 1] = SX.sym(f'D{i}y{ntrailers-1}')
args = out[1:-1, :].T.reshape((-1, 1))
dargs = out[2:, :].T.reshape((-1, 1))
# 1. find all thetas
p = out[0, :].T
v = sxvec(args[0], args[1])
theta = arctan2(v[1], v[0])
dtheta = jtimes(theta, args, dargs)
thetas = [theta]
velocities = [v]
positions = [p]
dthetas = [dtheta]
for i in range(ntrailers - 1):
p_next = p
v_next = v
theta_next = theta
dtheta_next = dtheta
p = p_next + sxvec(cos(theta_next), sin(theta_next))
v = v_next + sxvec(-sin(theta_next), cos(theta_next)) * dtheta_next
theta = arctan2(v[1], v[0])
dtheta = jtimes(theta, args, dargs)
thetas += [theta]
velocities += [v]
dthetas += [dtheta]
positions += [p]
positions = positions[::-1]
velocities = velocities[::-1]
dthetas = dthetas[::-1]
thetas = thetas[::-1]
# 2. find phi
v0 = velocities[0]
dtheta0 = dthetas[0]
theta0 = thetas[0]
phi = arctan2(dtheta0, cos(theta0) * v0[0] + sin(theta0) * v0[1])
# 3. find controls
u1 = v0.T @ sxvec(cos(theta0), sin(theta0))
u2 = jtimes(phi, args, dargs)
return make_tuple('FlatMaps',
flat_out=out[0],
flat_derivs=out,
u1=u1,
u2=u2,
phi=phi,
theta=thetas,
velocities=velocities,
positions=positions)
def codgen_model(self, model, opts):
# from casadi import * # syntax valid only for the entire module
import casadi
# x
if model.x==None:
x = casadi.SX.sym('x', 0, 1)
else:
x = model.x
# xdot
if model.xdot==None:
xdot = casadi.SX.sym('xdot', 0, 1)
else:
xdot = model.xdot
# u
if model.u==None:
u = casadi.SX.sym('u', 0, 1)
else:
u = model.u
# z
if model.z==None:
z = casadi.SX.sym('z', 0, 1)
else:
z = model.z
# fun
fun = model.ode_expr
# sizes
nx = model.nx
nu = model.nu
nz = model.nz
# define functions & generate C code
casadi_opts = dict(casadi_int='int', casadi_real='double')
c_sources = ' '
if opts.scheme=='erk':
if opts.sens_forw=='false':
fun_name = 'expl_ode_fun'
casadi_fun = casadi.Function(fun_name, [x, u], [fun])
casadi_fun.generate(casadi_opts)
c_sources = c_sources + fun_name + '.c '
else:
fun_name = 'expl_vde_for'
Sx = casadi.SX.sym('Sx', nx, nx)
Su = casadi.SX.sym('Su', nx, nu)
vde_x = casadi.jtimes(fun, x, Sx)
vde_u = casadi.jacobian(fun, u) + casadi.jtimes(fun, x, Su)
casadi_fun = casadi.Function(fun_name, [x, Sx, Su, u], [fun, vde_x, vde_u])
casadi_fun.generate(casadi_opts)
c_sources = c_sources + fun_name + '.c '
elif opts.scheme=='irk':
fun_name = 'impl_ode_fun'
casadi_fun = casadi.Function(fun_name, [x, xdot, u, z], [fun])
casadi_fun.generate(casadi_opts)
c_sources = c_sources + fun_name + '.c '
fun_name = 'impl_ode_fun_jac_x_xdot_z'
jac_x = casadi.jacobian(fun, x)
jac_xdot = casadi.jacobian(fun, xdot)
jac_z = casadi.jacobian(fun, z)
casadi_fun = casadi.Function(fun_name, [x, xdot, u, z], [fun, jac_x, jac_xdot, jac_z])
casadi_fun.generate(casadi_opts)
c_sources = c_sources + fun_name + '.c '
if opts.sens_forw=='true':
fun_name = 'impl_ode_jac_x_xdot_u_z'
jac_x = casadi.jacobian(fun, x)
jac_xdot = casadi.jacobian(fun, xdot)
jac_u = casadi.jacobian(fun, u)
jac_z = casadi.jacobian(fun, z)
casadi_fun = casadi.Function(fun_name, [x, xdot, u, z], [jac_x, jac_xdot, jac_u, jac_z])
casadi_fun.generate(casadi_opts)
c_sources = c_sources + fun_name + '.c '
# create model library
lib_name = model.model_name
# lib_name = lib_name + '_' + str(id(self))
if opts.scheme=='erk':
lib_name = lib_name + '_erk'
elif opts.scheme=='irk':
lib_name = lib_name + '_irk'
if opts.sens_forw=='false':
lib_name = lib_name + '_0'
else:
lib_name = lib_name + '_1'
lib_name = lib_name + '_' + str(model.ode_expr_hash)
lib_name = lib_name + '.so'
system('gcc -fPIC -shared ' + c_sources + ' -o ' + lib_name)
def jtimes(self, varA, varB):
"""Returns the partial derivative of the expression with respect to
varA, times varB.
Return:
cs.MX: expression of partial derivative"""
return cs.jtimes(self.expression, varA, varB)
def getKinematics(self):
# Assume p,c as 2x1
##################
###--Left Leg--###
##################
lp = ca.MX.zeros(2, 2)
lc = ca.MX.zeros(2, 2)
lp0 = self.lp0
lp[0, 0], lp[1, 0] = self.length[0] * ca.sin(self.lq[0]) + lp0[
0, 0], self.length[0] * ca.cos(self.lq[0]) + lp0[1, 0]
lp[0, 1], lp[1, 1] = self.length[1] * ca.sin(self.lq[1]) + lp[
0, 0], self.length[1] * ca.cos(self.lq[1]) + lp[1, 0]
lc[0, 0], lc[1, 0] = self.length[0] * ca.sin(self.lq[0]) / 2 + lp0[
0, 0], self.length[0] * ca.cos(self.lq[0]) / 2 + lp0[1, 0]
lc[0, 1], lc[1, 1] = self.length[1] * ca.sin(self.lq[1]) / 2 + lp[
0, 0], self.length[1] * ca.cos(self.lq[1]) / 2 + lp[1, 0]
###--Derivatives--###
dlp = ca.jtimes(lp, self.lq, self.dlq) + self.dlp0
dlc = ca.jtimes(lc, self.lq, self.dlq) + self.dlp0
ddlp = ca.jtimes(dlp, self.lq, self.dlq) + self.ddlp0
ddlc = ca.jtimes(dlc, self.lq, self.dlq) + self.ddlp0
###################
###--Right Leg--###
###################
rp = ca.MX.zeros(2, 2)
rc = ca.MX.zeros(2, 2)
rp0 = self.rp0
rp[0, 0], rp[1, 0] = self.length[4] * ca.sin(self.rq[0]) + rp0[
0, 0], self.length[4] * ca.cos(self.rq[0]) + rp0[1, 0]
rp[0, 1], rp[1, 1] = self.length[3] * ca.sin(self.rq[1]) + rp[
0, 0], self.length[3] * ca.cos(self.rq[1]) + rp[1, 0]
rc[0, 0], rc[1, 0] = self.length[4] * ca.sin(self.rq[0]) / 2 + rp0[
0, 0], self.length[4] * ca.cos(self.rq[0]) / 2 + rp0[1, 0]
rc[0, 1], rc[1, 1] = self.length[3] * ca.sin(self.rq[1]) / 2 + rp[
0, 0], self.length[3] * ca.cos(self.rq[1]) / 2 + rp[1, 0]
###--Derivatives--###
drp = ca.jtimes(rp, self.rq, self.drq) + self.drp0
drc = ca.jtimes(rc, self.rq, self.drq) + self.drp0
ddrp = ca.jtimes(drp, self.rq, self.drq) + self.ddrp0
ddrc = ca.jtimes(drc, self.rq, self.drq) + self.ddrp0
###############
###--Torso--###
###############
tp = ca.MX.zeros(2)
tc = ca.MX.zeros(2)
tp[0], tp[1] = self.length[2] * ca.sin(self.tq[0]) + lp[
0, 1], self.length[2] * ca.cos(self.tq[0]) + lp[1, 1]
tc[0], tc[1] = self.length[2] * ca.sin(self.tq[0]) / 2 + lp[
0, 1], self.length[2] * ca.cos(self.tq[0]) / 2 + lp[1, 1]
###--Derivatives--###
dtp = ca.jtimes(tp, self.tq, self.dtq) + self.dlp0
dtc = ca.jtimes(tc, self.tq, self.dtq) + self.dlp0
ddtp = ca.jtimes(dtp, self.tq, self.dtq) + self.ddlp0
ddtc = ca.jtimes(dtc, self.tq, self.dtq) + self.ddlp0
###--Form a dictionary--###
p = {
'Left Leg':
lp,
'Right Leg':
rp,
'Torso':
tp,
'Constraint': [(lp[:, 1] == rp[:, 1]), self.lq[0] <= self.lq[1],
self.rq[0] <= self.rq[1]]
}
dp = {
'Left Leg': dlp,
'Right Leg': drp,
'Torso': dtp,
'Constraint': (dlp[:, 1] == drp[:, 1])
}
ddp = {
'Left Leg': ddlp,
'Right Leg': ddrp,
'Torso': ddtp,
'Constraint': (ddlp[:, 1] == ddrp[:, 1])
}
c = {'Left Leg': lc, 'Right Leg': rc, 'Torso': tc}
dc = {'Left Leg': dlc, 'Right Leg': drc, 'Torso': dtc}
ddc = {'Left Leg': ddlc, 'Right Leg': ddrc, 'Torso': ddtc}
return p, dp, ddp, c, dc, ddc
qdot_sym = cs.SX.sym("qsdot", n_joints)
qddot_sym = cs.SX.sym("qsddot", n_joints)
qdddot_sym = cs.SX.sym("qsdddot", n_joints)
# 2. Declare the vector of the variables to find the derivatives with respect to, and the vector with their time derivatives:
# In[19]:
id_vec = cs.vertcat(q_sym, qdot_sym, qddot_sym)
id_dvec = cs.vertcat(qdot_sym, qddot_sym, qdddot_sym)
# 3. Obtain the symbolic expression of the time derivative of ID with respect to q, qdot, and qddot using cs.jtimes():
# In[20]:
timederivative_id_expression = cs.jtimes(tau_sym(q_sym, qdot_sym, qddot_sym),
id_vec, id_dvec)
# 4. Use the symbolic expression to make a CasADi function that can be efficiently numerical evaluated:
# In[21]:
timederivative_id_function = cs.Function(
"didtimes", [q_sym, qdot_sym, qddot_sym, qdddot_sym],
[timederivative_id_expression], {
"jit": True,
"jit_options": {
"flags": "-Ofast"
}
})
print(
timederivative_id_function(np.ones(n_joints), np.ones(n_joints),
def generate_c_code_discrete_dynamics( model, opts ):
casadi_version = ca.CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
# load model
x = model.x
u = model.u
p = model.p
phi = model.disc_dyn_expr
model_name = model.name
nx = casadi_length(x)
if isinstance(phi, ca.MX):
symbol = ca.MX.sym
elif isinstance(phi, ca.SX):
symbol = ca.SX.sym
else:
Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi)))
# assume nx1 = nx !!!
lam = symbol('lam', nx, 1)
# generate jacobians
ux = ca.vertcat(u,x)
jac_ux = ca.jacobian(phi, ux)
# generate adjoint
adj_ux = ca.jtimes(phi, ux, lam, True)
# generate hessian
hess_ux = ca.jacobian(adj_ux, ux)
## change directory
code_export_dir = opts["code_export_directory"]
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
# set up & generate Functions
fun_name = model_name + '_dyn_disc_phi_fun'
phi_fun = ca.Function(fun_name, [x, u, p], [phi])
phi_fun.generate(fun_name, casadi_opts)
fun_name = model_name + '_dyn_disc_phi_fun_jac'
phi_fun_jac_ut_xt = ca.Function(fun_name, [x, u, p], [phi, jac_ux.T])
phi_fun_jac_ut_xt.generate(fun_name, casadi_opts)
fun_name = model_name + '_dyn_disc_phi_fun_jac_hess'
phi_fun_jac_ut_xt_hess = ca.Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux])
phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_opts)
os.chdir(cwd)
# 适用于矩阵或稀疏模式
# y = ca.SX.sym('y',10,1)
# print(y.shape)
# .size1() => 行数
# .size2() => 列数
# .numel() => 元素个数 == .size1() * .size2()
# .sparisity() => 稀疏模式
'''
Part 7: 微分计算
'''
# 微分计算可以从前向和反向分别计算，分别对应得到jacobian vector和jacobian-transposed vector
#1. jacobian
A = ca.SX.sym('A', 3, 2)
x = ca.SX.sym('x', 2)
print('A:', A, ' x: ', x, 'Ax: ', ca.mtimes(A, x))
print('J:', ca.jacobian(ca.mtimes(A, x), x))
print(ca.dot(A, A))
print(ca.gradient(ca.dot(A, A), A))
[H, g] = ca.hessian(ca.dot(x, x), x) #hessian（）会同时返回梯度和hessian矩阵
print('H：', H)
print('g:', g)
v = ca.SX.sym('v', 2)
f = ca.mtimes(A, x)
print(ca.jtimes(f, x, v)) # jtimes = jacobian function * vector
delta = alpha - beta
ddelta = dalpha - dbeta
tau = delta - gamma + pi / 2
dtau = ddelta - dgamma
q = vertcat(alpha, beta, gamma)
dq = vertcat(dalpha, dbeta, dgamma)
C = vertcat(L * sin(alpha), -L * cos(alpha))
Q = C + vertcat(-h * cos(delta), -h * sin(delta))
F = Q + vertcat(l * sin(tau), -l * cos(tau))
c = (Q + F) / 2
dC = jtimes(C, q, dq)
dQ = jtimes(Q, q, dq)
dc = jtimes(c, q, dq)
# Modeling with Lagrange mechanics
E_kin = 0.5 * dtau**2 * (m * l**2 / 12) + 0.5 * ddelta**2 * (
M * H**2 / 12) + 0.5 * M * sumsqr(dC) + 0.5 * m * sumsqr(dc)
E_pot = M * g * C[1] + m * g * c[1]
Lag = E_kin - E_pot
E_tot = E_kin + E_pot
Lag_q = gradient(Lag, q)
Lag_dq = gradient(Lag, dq)
