Esempio n. 1
0
    def compute_input(self, x, xd, initial_guess=None):
        prog = MathematicalProgram()

        # Joint configuration states & Contact forces
        q = prog.NewContinuousVariables(rows=self.T + 1,
                                        cols=self.nq,
                                        name='q')
        v = prog.NewContinuousVariables(rows=self.T + 1,
                                        cols=self.nq,
                                        name='v')
        u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
        contact = prog.NewContinuousVariables(rows=self.T,
                                              cols=self.nf,
                                              name='lambda1')

        z = prog.NewBinaryVariables(rows=self.T, cols=self.nf, name='z')

        # Add Initial Condition Constraint
        prog.AddConstraint(eq(q[0], np.array(x[0:3])))
        prog.AddConstraint(eq(v[0], np.array(x[3:6])))

        # Add Final Condition Constraint
        prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
        prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))

        prog.AddConstraint(z[0, 0] == 0)
        prog.AddConstraint(z[0, 1] == 0)

        # Add Dynamics Constraints
        for t in range(self.T):
            # Add Dynamics Constraints
            prog.AddConstraint(
                eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))

            prog.AddConstraint(v[t + 1, 0] == (
                v[t, 0] + self.sim.params['h'] *
                (-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
            prog.AddConstraint(v[t + 1,
                                 1] == (v[t, 1] + self.sim.params['h'] *
                                        (-self.sim.params['c'] * v[t, 1] +
                                         contact[t, 0] - contact[t, 1])))
            prog.AddConstraint(v[t + 1, 2] == (
                v[t, 2] + self.sim.params['h'] *
                (-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))

            # Add Contact Constraints with big M = self.contact
            prog.AddConstraint(ge(contact[t], 0))
            prog.AddConstraint(contact[t, 0] + self.sim.params['k'] *
                               (q[t, 1] - q[t, 0] - self.sim.params['d']) >= 0)
            prog.AddConstraint(contact[t, 1] + self.sim.params['k'] *
                               (q[t, 2] - q[t, 1] - self.sim.params['d']) >= 0)

            # Mixed Integer Constraints
            M = self.contact_max
            prog.AddConstraint(contact[t, 0] <= M)
            prog.AddConstraint(contact[t, 1] <= M)
            prog.AddConstraint(contact[t, 0] <= M * z[t, 0])
            prog.AddConstraint(contact[t, 1] <= M * z[t, 1])
            prog.AddConstraint(
                contact[t, 0] + self.sim.params['k'] *
                (q[t, 1] - q[t, 0] - self.sim.params['d']) <= M *
                (1 - z[t, 0]))
            prog.AddConstraint(
                contact[t, 1] + self.sim.params['k'] *
                (q[t, 2] - q[t, 1] - self.sim.params['d']) <= M *
                (1 - z[t, 1]))
            prog.AddConstraint(z[t, 0] + z[t, 1] == 1)

            # Add Input Constraints. Contact Constraints already enforced in big-M
            # prog.AddConstraint(le(u[t], self.input_max))
            # prog.AddConstraint(ge(u[t], -self.input_max))

            # Add Costs
            prog.AddCost(u[t].dot(u[t]))

        # Set Initial Guess as empty. Otherwise, start from last solver iteration.
        if (type(initial_guess) == type(None)):
            initial_guess = np.empty(prog.num_vars())

            # Populate initial guess by linearly interpolating between initial
            # and final states
            #qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
            qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
            vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
            uinit = np.tile(np.array([0, 0]), (self.T, 1))
            finit = np.tile(np.array([0, 0]), (self.T, 1))

            prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
            prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
            prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
            prog.SetDecisionVariableValueInVector(contact, finit,
                                                  initial_guess)

        # Solve the program
        if (self.solver == "ipopt"):
            solver_id = IpoptSolver().solver_id()
        elif (self.solver == "snopt"):
            solver_id = SnoptSolver().solver_id()
        elif (self.solver == "osqp"):
            solver_id = OsqpSolver().solver_id()
        elif (self.solver == "mosek"):
            solver_id = MosekSolver().solver_id()
        elif (self.solver == "gurobi"):
            solver_id = GurobiSolver().solver_id()

        solver = MixedIntegerBranchAndBound(prog, solver_id)

        #result = solver.Solve(prog, initial_guess)
        result = solver.Solve()

        if result != result.kSolutionFound:
            raise ValueError('Infeasible optimization problem')

        sol = result.GetSolution()
        q_opt = result.GetSolution(q)
        v_opt = result.GetSolution(v)
        u_opt = result.GetSolution(u)
        f_opt = result.GetSolution(contact)

        return sol, q_opt, v_opt, u_opt, f_opt
Esempio n. 2
0
    def compute_input(self, x, xd, initial_guess=None, tol=0.0):
        prog = MathematicalProgram()

        # Joint configuration states & Contact forces
        q = prog.NewContinuousVariables(rows=self.T + 1,
                                        cols=self.nq,
                                        name='q')
        v = prog.NewContinuousVariables(rows=self.T + 1,
                                        cols=self.nq,
                                        name='v')
        u = prog.NewContinuousVariables(rows=self.T, cols=self.nu, name='u')
        contact = prog.NewContinuousVariables(rows=self.T,
                                              cols=self.nf,
                                              name='lambda')

        #
        alpha = prog.NewContinuousVariables(rows=self.T, cols=2, name='alpha')
        beta = prog.NewContinuousVariables(rows=self.T, cols=2, name='beta')

        # Add Initial Condition Constraint
        prog.AddConstraint(eq(q[0], np.array(x[0:3])))
        prog.AddConstraint(eq(v[0], np.array(x[3:6])))

        # Add Final Condition Constraint
        prog.AddConstraint(eq(q[self.T], np.array(xd[0:3])))
        prog.AddConstraint(eq(v[self.T], np.array(xd[3:6])))

        # Add Dynamics Constraints
        for t in range(self.T):
            # Add Dynamics Constraints
            prog.AddConstraint(
                eq(q[t + 1], (q[t] + self.sim.params['h'] * v[t + 1])))

            prog.AddConstraint(v[t + 1, 0] == (
                v[t, 0] + self.sim.params['h'] *
                (-self.sim.params['c'] * v[t, 0] - contact[t, 0] + u[t, 0])))
            prog.AddConstraint(v[t + 1,
                                 1] == (v[t, 1] + self.sim.params['h'] *
                                        (-self.sim.params['c'] * v[t, 1] +
                                         contact[t, 0] - contact[t, 1])))
            prog.AddConstraint(v[t + 1, 2] == (
                v[t, 2] + self.sim.params['h'] *
                (-self.sim.params['c'] * v[t, 2] + contact[t, 1] + u[t, 1])))

            # Add Contact Constraints
            prog.AddConstraint(ge(alpha[t], 0))
            prog.AddConstraint(ge(beta[t], 0))
            prog.AddConstraint(alpha[t, 0] == contact[t, 0])
            prog.AddConstraint(alpha[t, 1] == contact[t, 1])
            prog.AddConstraint(
                beta[t, 0] == (contact[t, 0] + self.sim.params['k'] *
                               (q[t, 1] - q[t, 0] - self.sim.params['d'])))
            prog.AddConstraint(
                beta[t, 1] == (contact[t, 1] + self.sim.params['k'] *
                               (q[t, 2] - q[t, 1] - self.sim.params['d'])))

            # Complementarity constraints. Start with relaxed version and start constraining.
            prog.AddConstraint(alpha[t, 0] * beta[t, 0] <= tol)
            prog.AddConstraint(alpha[t, 1] * beta[t, 1] <= tol)

            # Add Input Constraints and Contact Constraints
            prog.AddConstraint(le(contact[t], self.contact_max))
            prog.AddConstraint(ge(contact[t], -self.contact_max))
            prog.AddConstraint(le(u[t], self.input_max))
            prog.AddConstraint(ge(u[t], -self.input_max))

            # Add Costs
            prog.AddCost(u[t].dot(u[t]))

        # Set Initial Guess as empty. Otherwise, start from last solver iteration.
        if (type(initial_guess) == type(None)):
            initial_guess = np.empty(prog.num_vars())

            # Populate initial guess by linearly interpolating between initial
            # and final states
            #qinit = np.linspace(x[0:3], xd[0:3], self.T + 1)
            qinit = np.tile(np.array(x[0:3]), (self.T + 1, 1))
            vinit = np.tile(np.array(x[3:6]), (self.T + 1, 1))
            uinit = np.tile(np.array([0, 0]), (self.T, 1))
            finit = np.tile(np.array([0, 0]), (self.T, 1))

            prog.SetDecisionVariableValueInVector(q, qinit, initial_guess)
            prog.SetDecisionVariableValueInVector(v, vinit, initial_guess)
            prog.SetDecisionVariableValueInVector(u, uinit, initial_guess)
            prog.SetDecisionVariableValueInVector(contact, finit,
                                                  initial_guess)

        # Solve the program
        if (self.solver == "ipopt"):
            solver = IpoptSolver()
        elif (self.solver == "snopt"):
            solver = SnoptSolver()

        result = solver.Solve(prog, initial_guess)

        if (self.solver == "ipopt"):
            print("Ipopt Solver Status: ",
                  result.get_solver_details().status, ", meaning ",
                  result.get_solver_details().ConvertStatusToString())
        elif (self.solver == "snopt"):
            val = result.get_solver_details().info
            status = self.snopt_status(val)
            print("Snopt Solver Status: ",
                  result.get_solver_details().info, ", meaning ", status)

        sol = result.GetSolution()
        q_opt = result.GetSolution(q)
        v_opt = result.GetSolution(v)
        u_opt = result.GetSolution(u)
        f_opt = result.GetSolution(contact)

        return sol, q_opt, v_opt, u_opt, f_opt