From a42e851ba4ba7ba00472cfbb94345986eec5aaf2 Mon Sep 17 00:00:00 2001 From: Rufus Wong Date: Tue, 9 Nov 2021 20:51:12 +0100 Subject: [PATCH] Use normalized contact forces --- Planner.py | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/Planner.py b/Planner.py index 6b8b720..d5ccf67 100644 --- a/Planner.py +++ b/Planner.py @@ -130,17 +130,20 @@ def velocity_dynamics_constraint(vars, context_index): # Contact forces num_contacts = robot.get_num_contacts() - contact_force = [prog.NewContinuousVariables(3, N-1, f"contact{contact}_contact_force") for contact in range(num_contacts)] + normalized_contact_force = [prog.NewContinuousVariables(3, N-1, f"contact{contact}_normalized_contact_force") for contact in range(num_contacts)] + max_contact_force = 4*g*total_mass for n in range(N-1): for contact in range(num_contacts): # Linear friction cone - prog.AddLinearConstraint(contact_force[contact][0,n] <= mu*contact_force[contact][2,n]) - prog.AddLinearConstraint(-contact_force[contact][0,n] <= mu*contact_force[contact][2,n]) - prog.AddLinearConstraint(contact_force[contact][1,n] <= mu*contact_force[contact][2,n]) - prog.AddLinearConstraint(-contact_force[contact][1,n] <= mu*contact_force[contact][2,n]) + prog.AddLinearConstraint(normalized_contact_force[contact][0,n] <= mu*normalized_contact_force[contact][2,n]) + prog.AddLinearConstraint(-normalized_contact_force[contact][0,n] <= mu*normalized_contact_force[contact][2,n]) + prog.AddLinearConstraint(normalized_contact_force[contact][1,n] <= mu*normalized_contact_force[contact][2,n]) + prog.AddLinearConstraint(-normalized_contact_force[contact][1,n] <= mu*normalized_contact_force[contact][2,n]) # normal force >=0, normal_force == 0 if not in_stance # max normal force assumed to be 4mg - prog.AddBoundingBoxConstraint(0, in_stance[contact,n]*4*g*total_mass, contact_force[contact][2,n]) + prog.AddBoundingBoxConstraint(0, in_stance[contact,n], normalized_contact_force[contact][2,n]) + + prog.SetInitialGuess(normalized_contact_force[contact][2,n], 0.1*in_stance[contact,n]) # Center of mass variables and constraints com = prog.NewContinuousVariables(3, N, "com") @@ -165,7 +168,8 @@ def velocity_dynamics_constraint(vars, context_index): # which is a little more consistent with the LCP contact models. prog.AddConstraint(eq(com[:, n+1], com[:,n] + h[n]*comdot[:,n])) prog.AddConstraint(eq(comdot[:, n+1], comdot[:,n] + h[n]*comddot[:,n])) - prog.AddConstraint(eq(total_mass*comddot[:,n], sum(contact_force[i][:,n] for i in range(4)) + total_mass*gravity)) + prog.AddConstraint(eq(total_mass*comddot[:,n], + sum(max_contact_force*normalized_contact_force[i][:,n] for i in range(num_contacts)) + total_mass*gravity)) # Angular momentum (about the center of mass) H = prog.NewContinuousVariables(3, N, "H") @@ -174,8 +178,8 @@ def velocity_dynamics_constraint(vars, context_index): prog.SetInitialGuess(Hdot, np.zeros((3,N-1))) # Hdot = sum_i cross(p_FootiW-com, contact_force_i) def angular_momentum_constraint(vars, context_index, active_contacts): - q, com, Hdot, contact_force = np.split(vars, [nq, nq+3, nq+6]) - contact_force = contact_force.reshape(3, num_contacts, order='F') + q, com, Hdot, normalized_contact_force = np.split(vars, [nq, nq+3, nq+6]) + contact_force = max_contact_force*(normalized_contact_force.reshape(3, num_contacts, order='F')) if isinstance(vars[0], AutoDiffXd): q = ExtractValue(q) if not np.array_equal(q, plant.GetPositions(context[context_index])): @@ -199,7 +203,7 @@ def angular_momentum_constraint(vars, context_index, active_contacts): for n in range(N-1): prog.AddConstraint(eq(H[:,n+1], H[:,n] + h[n]*Hdot[:,n])) active_contacts = np.where(in_stance[:,n])[0] - Fn = np.concatenate([contact_force[i][:,n] for i in range(num_contacts)]) + Fn = np.concatenate([normalized_contact_force[i][:,n] for i in range(num_contacts)]) prog.AddConstraint(partial(angular_momentum_constraint, context_index=n, active_contacts=active_contacts), lb=np.zeros(3), ub=np.zeros(3), vars=np.concatenate((q[:,n], com[:,n], Hdot[:,n], Fn)))