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)