Esempio n. 1
0
def findMaxForce(theta1, theta2, theta3):
    theta12 = theta2 + theta1
    theta123 = theta3 + theta12
    s1 = sin(theta1)
    s2 = sin(theta2)
    s3 = sin(theta3)
    s12 = sin(theta12)
    s123 = sin(theta123)
    c1 = cos(theta1)
    c2 = cos(theta2)
    c3 = cos(theta3)
    c12 = cos(theta12)
    c123 = cos(theta123)

    J = np.array([[
        -l_1 * s1 - l_2 * s12 - l_3 * s123, -l_2 * s12 - l_3 * s123,
        -l_3 * s123
    ], [l_1 * c1 + l_2 * c12 + l_3 * c123, l_2 * c12 + l_3 * c123,
        l_3 * c123]])

    tau1 = (
        -l_1 * c1 * m_2 * g  # torque due to l_1 - l_2 link motor
        + (-l_1 * c1 + l_2 / 2.0 * c12) * m_m *
        g  # torque due to battery link motor
        + (-l_1 * c1 + l_2 / 2.0 * c12 + l_b * cos(theta12 + np.pi / 2.0)) *
        m_b * g  # torque due to battery
        + (-l_1 * c1 + l_2) * m_3 * g  # torque due to l_2 - l_3 link motor
        + (-l_1 * c1 + l_2 + l_3 * c3) * m_w * g  # torque due to front wheel
    )

    # print("tau1 = " + str(tau1))

    mp = MathematicalProgram()

    tau23 = mp.NewContinuousVariables(2, "tau23")
    tau123 = np.append(tau1, tau23)
    mp.AddCost(J.dot(tau123)[0])
    mp.AddConstraint(tau23[0]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2)
    mp.AddConstraint(tau23[1]**2 <= LINK_MOTOR_CONTINUOUS_STALL_TORQUE**2)
    mp.AddLinearConstraint(J.dot(tau123)[1] >= -HUB_MOTOR_FORCE / 2.0)

    result = Solve(mp)
    is_success = result.is_success()
    torques = result.GetSolution(tau23)
    full_tau = np.append(tau1, torques)
    output_force = J.dot(full_tau)
    # print("is_success = " + str(is_success))
    # print("tau = " + str(full_tau))
    # print("F = " + str(J.dot(full_tau)))
    return is_success, output_force, torques
Esempio n. 2
0
    def create_qp1(self, plant_context, V, q_des, v_des, vd_des):
        # Determine contact points
        contact_positions_per_frame = {}
        active_contacts_per_frame = {}  # Note this should be in frame space
        for frame, contacts in self.contacts_per_frame.items():
            contact_positions = self.plant.CalcPointsPositions(
                plant_context, self.plant.GetFrameByName(frame), contacts,
                self.plant.world_frame())
            active_contacts_per_frame[
                frame] = contacts[:,
                                  np.where(contact_positions[2, :] <= 0.0)[0]]

        N_c = sum([
            active_contacts.shape[1]
            for active_contacts in active_contacts_per_frame.values()
        ])  # num contact points
        if N_c == 0:
            print("Not in contact!")
            return None
        ''' Eq(7) '''
        H = self.plant.CalcMassMatrixViaInverseDynamics(plant_context)
        # Note that CalcGravityGeneralizedForces assumes the form Mv̇ + C(q, v)v = tau_g(q) + tau_app
        # while Eq(7) assumes gravity is accounted in C (on the left hand side)
        C_7 = self.plant.CalcBiasTerm(
            plant_context) - self.plant.CalcGravityGeneralizedForces(
                plant_context)
        B_7 = self.B_7

        # TODO: Double check
        Phi_foots = []
        for frame, active_contacts in active_contacts_per_frame.items():
            if active_contacts.size:
                Phi_foots.append(
                    self.plant.CalcJacobianTranslationalVelocity(
                        plant_context, JacobianWrtVariable.kV,
                        self.plant.GetFrameByName(frame), active_contacts,
                        self.plant.world_frame(), self.plant.world_frame()))
        Phi = np.vstack(Phi_foots)
        ''' Eq(8) '''
        v_idx_act = self.v_idx_act
        H_f = H[0:v_idx_act, :]
        H_a = H[v_idx_act:, :]
        C_f = C_7[0:v_idx_act]
        C_a = C_7[v_idx_act:]
        B_a = self.B_a
        Phi_f_T = Phi.T[0:v_idx_act:, :]
        Phi_a_T = Phi.T[v_idx_act:, :]
        ''' Eq(9) '''
        # Assume flat ground for now
        n = np.array([[0], [0], [1.0]])
        d = np.array([[1.0, -1.0, 0.0, 0.0], [0.0, 0.0, 1.0, -1.0],
                      [0.0, 0.0, 0.0, 0.0]])
        v = np.zeros((N_d, N_c, N_f))
        for i in range(N_d):
            for j in range(N_c):
                v[i, j] = (n + mu * d)[:, i]
        ''' Quadratic Program I '''
        prog = MathematicalProgram()
        qdd = prog.NewContinuousVariables(
            self.plant.num_velocities(),
            name="qdd")  # To ignore 6 DOF floating base
        self.qdd = qdd
        beta = prog.NewContinuousVariables(N_d, N_c, name="beta")
        self.beta = beta
        lambd = prog.NewContinuousVariables(N_f * N_c, name="lambda")
        self.lambd = lambd

        # Jacobians ignoring the 6DOF floating base
        J_foots = []
        for frame, active_contacts in active_contacts_per_frame.items():
            if active_contacts.size:
                num_active_contacts = active_contacts.shape[1]
                J_foot = np.zeros((N_f * num_active_contacts, Atlas.TOTAL_DOF))
                # TODO: Can this be simplified?
                for i in range(num_active_contacts):
                    J_foot[N_f * i:N_f *
                           (i +
                            1), :] = self.plant.CalcJacobianSpatialVelocity(
                                plant_context, JacobianWrtVariable.kV,
                                self.plant.GetFrameByName(frame),
                                active_contacts[:,
                                                i], self.plant.world_frame(),
                                self.plant.world_frame())[3:]
                J_foots.append(J_foot)
        J = np.vstack(J_foots)
        assert (J.shape == (N_c * N_f, Atlas.TOTAL_DOF))

        eta = prog.NewContinuousVariables(J.shape[0], name="eta")
        self.eta = eta

        x = prog.NewContinuousVariables(
            self.x_size, name="x")  # x_com, y_com, x_com_d, y_com_d
        self.x = x
        u = prog.NewContinuousVariables(self.u_size,
                                        name="u")  # x_com_dd, y_com_dd
        self.u = u
        ''' Eq(10) '''
        w = 0.01
        epsilon = 1.0e-8
        K_p = 10.0
        K_d = 4.0
        frame_weights = np.ones((Atlas.TOTAL_DOF))

        q = self.plant.GetPositions(plant_context)
        qd = self.plant.GetVelocities(plant_context)

        # Convert q, q_nom to generalized velocities form
        q_err = self.plant.MapQDotToVelocity(plant_context, q_des - q)
        # print(f"Pelvis error: {q_err[0:3]}")
        ## FIXME: Not sure if it's a good idea to ignore the x, y, z position of pelvis
        # ignored_pose_indices = {3, 4, 5} # Ignore x position, y position
        ignored_pose_indices = {}  # Ignore x position, y position
        relevant_pose_indices = list(
            set(range(Atlas.TOTAL_DOF)) - set(ignored_pose_indices))
        self.relevant_pose_indices = relevant_pose_indices
        qdd_ref = K_p * q_err + K_d * (v_des - qd) + vd_des  # Eq(27) of [1]
        qdd_err = qdd_ref - qdd
        qdd_err = qdd_err * frame_weights
        qdd_err = qdd_err[relevant_pose_indices]
        prog.AddCost(
            V(x, u) + w * ((qdd_err).dot(qdd_err)) +
            epsilon * np.sum(np.square(beta)) + eta.dot(eta))
        ''' Eq(11) - 0.003s '''
        eq11_lhs = H_f.dot(qdd) + C_f
        eq11_rhs = Phi_f_T.dot(lambd)
        prog.AddLinearConstraint(eq(eq11_lhs, eq11_rhs))
        ''' Eq(12) - 0.005s '''
        alpha = 0.1
        # TODO: Double check
        Jd_qd_foots = []
        for frame, active_contacts in active_contacts_per_frame.items():
            if active_contacts.size:
                Jd_qd_foot = self.plant.CalcBiasTranslationalAcceleration(
                    plant_context, JacobianWrtVariable.kV,
                    self.plant.GetFrameByName(frame), active_contacts,
                    self.plant.world_frame(), self.plant.world_frame())
                Jd_qd_foots.append(Jd_qd_foot.flatten())
        Jd_qd = np.concatenate(Jd_qd_foots)
        assert (Jd_qd.shape == (N_c * 3, ))
        eq12_lhs = J.dot(qdd) + Jd_qd
        eq12_rhs = -alpha * J.dot(qd) + eta
        prog.AddLinearConstraint(eq(eq12_lhs, eq12_rhs))
        ''' Eq(13) - 0.015s '''
        def tau(qdd, lambd):
            return self.B_a_inv.dot(H_a.dot(qdd) + C_a - Phi_a_T.dot(lambd))

        self.tau = tau
        eq13_lhs = self.tau(qdd, lambd)
        prog.AddLinearConstraint(ge(eq13_lhs, -self.sorted_max_efforts))
        prog.AddLinearConstraint(le(eq13_lhs, self.sorted_max_efforts))
        ''' Eq(14) '''
        for j in range(N_c):
            beta_v = beta[:, j].dot(v[:, j])
            prog.AddLinearConstraint(eq(lambd[N_f * j:N_f * j + 3], beta_v))
        ''' Eq(15) '''
        for b in beta.flat:
            prog.AddLinearConstraint(b >= 0.0)
        ''' Eq(16) '''
        prog.AddLinearConstraint(ge(eta, eta_min))
        prog.AddLinearConstraint(le(eta, eta_max))
        ''' Enforce x as com '''
        com = self.plant.CalcCenterOfMassPosition(plant_context)
        com_d = self.plant.CalcJacobianCenterOfMassTranslationalVelocity(
            plant_context, JacobianWrtVariable.kV, self.plant.world_frame(),
            self.plant.world_frame()).dot(qd)
        prog.AddLinearConstraint(x[0] == com[0])
        prog.AddLinearConstraint(x[1] == com[1])
        prog.AddLinearConstraint(x[2] == com_d[0])
        prog.AddLinearConstraint(x[3] == com_d[1])
        ''' Enforce u as com_dd '''
        com_dd = (
            self.plant.CalcBiasCenterOfMassTranslationalAcceleration(
                plant_context, JacobianWrtVariable.kV,
                self.plant.world_frame(), self.plant.world_frame()) +
            self.plant.CalcJacobianCenterOfMassTranslationalVelocity(
                plant_context, JacobianWrtVariable.kV,
                self.plant.world_frame(), self.plant.world_frame()).dot(qdd))
        prog.AddLinearConstraint(u[0] == com_dd[0])
        prog.AddLinearConstraint(u[1] == com_dd[1])
        ''' Respect joint limits '''
        for name, limit in Atlas.JOINT_LIMITS.items():
            # Get the corresponding joint value
            joint_pos = self.plant.GetJointByName(name).get_angle(
                plant_context)
            # Get the corresponding actuator index
            act_idx = getActuatorIndex(self.plant, name)
            # Use the actuator index to find the corresponding generalized coordinate index
            # q_idx = np.where(B_7[:,act_idx] == 1)[0][0]
            q_idx = getJointIndexInGeneralizedVelocities(self.plant, name)

            if joint_pos >= limit.upper:
                # print(f"Joint {name} max reached")
                prog.AddLinearConstraint(qdd[q_idx] <= 0.0)
            elif joint_pos <= limit.lower:
                # print(f"Joint {name} min reached")
                prog.AddLinearConstraint(qdd[q_idx] >= 0.0)

        return prog
Esempio n. 3
0
class PPTrajectory():
    def __init__(self, sample_times, num_vars, degree, continuity_degree):
        self.sample_times = sample_times
        self.n = num_vars
        self.degree = degree

        self.prog = MathematicalProgram()
        self.coeffs = []
        for i in range(len(sample_times)):
            self.coeffs.append(
                self.prog.NewContinuousVariables(num_vars, degree + 1, "C"))
        self.result = None

        # Add continuity constraints
        for s in range(len(sample_times) - 1):
            trel = sample_times[s + 1] - sample_times[s]
            coeffs = self.coeffs[s]
            for var in range(self.n):
                for deg in range(continuity_degree + 1):
                    # Don't use eval here, because I want left and right
                    # values of the same time
                    left_val = 0
                    for d in range(deg, self.degree + 1):
                        left_val += coeffs[var, d]*np.power(trel, d-deg) * \
                               math.factorial(d)/math.factorial(d-deg)
                    right_val = self.coeffs[s + 1][var,
                                                   deg] * math.factorial(deg)
                    self.prog.AddLinearConstraint(left_val == right_val)

        # Add cost to minimize highest order terms
        for s in range(len(sample_times) - 1):
            self.prog.AddQuadraticCost(np.eye(num_vars), np.zeros(
                (num_vars, 1)), self.coeffs[s][:, -1])

    def eval(self, t, derivative_order=0):
        if derivative_order > self.degree:
            return 0

        s = 0
        while s < len(self.sample_times) - 1 and t >= self.sample_times[s + 1]:
            s += 1
        trel = t - self.sample_times[s]

        if self.result is None:
            coeffs = self.coeffs[s]
        else:
            coeffs = self.result.GetSolution(self.coeffs[s])

        deg = derivative_order
        val = 0 * coeffs[:, 0]
        for var in range(self.n):
            for d in range(deg, self.degree + 1):
                val[var] += coeffs[var, d]*np.power(trel, d-deg) * \
                       math.factorial(d)/math.factorial(d-deg)

        return val

    def add_constraint(self, t, derivative_order, lb, ub=None):
        '''
        Adds a constraint of the form d^deg lb <= x(t) / dt^deg <= ub
        '''
        if ub is None:
            ub = lb

        assert (derivative_order <= self.degree)
        val = self.eval(t, derivative_order)
        self.prog.AddLinearConstraint(val, lb, ub)

    def generate(self):
        self.result = Solve(self.prog)
        assert (self.result.is_success())
Esempio n. 4
0
# Can also add costs in a vector coefficient form
cost_3 = prog.AddLinearCost([3., 4.], 5.0, x)
print(cost_3)

# Can also just call the generic "AddCost".
# If drake thinks its linear, gets added to linear costs list
cost_4 = prog.AddCost(x[0] + 3 * x[1] + 5)
print(f"Number of linear cost objects after calling AddCost: {len(prog.linear_costs())}")

# New program, now with constraints
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2, "x")
y = prog.NewContinuousVariables(3, "y")

bounding_box = prog.AddLinearConstraint(x[1] <= 2)
linear_eq = prog.AddLinearConstraint(x[1] + 2 * y[2] == 1)
linear_ineq = prog.AddLinearConstraint(x[1] + 4 * y[1] >= 2)

# New program, all the way to solving...
prog = MathematicalProgram()
# Declare x as decision variables.
x = prog.NewContinuousVariables(4)
# Add linear costs. To show that calling AddLinearCosts results in the sum of each individual
# cost, we add two costs -3x[0] - x[1] and -5x[2]-x[3]+2
prog.AddLinearCost(-3*x[0] -x[1])
prog.AddLinearCost(-5*x[2] - x[3] + 2)
# Add linear equality constraint 3x[0] + x[1] + 2x[2] == 30
prog.AddLinearConstraint(3*x[0] + x[1] + 2*x[2] == 30)
# Add Linear inequality constraints
prog.AddLinearConstraint(2*x[0] + x[1] + 3*x[2] + x[3] >= 15)