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
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
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())
# 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)