LF_tau_lim = [50.0, 100.0, 100.0] RF_tau_lim = [50.0, 100.0, 100.0] LH_tau_lim = [50.0, 100.0, 100.0] RH_tau_lim = [50.0, 100.0, 100.0] torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim]) nc = np.sum(stanceFeet) comp_dyn = ComputationalDynamics() params = IterativeProjectionParameters() params.setContactsPosWF(contacts) params.setTorqueLims(torque_limits) params.setActiveContacts(stanceFeet) params.setConstraintModes(constraint_mode_IP) params.setContactNormals(normals) params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) #inputs for foothold planning foothold_params = FootholdPlanningInterface() foothold_params.com_position_to_validateW = [0.05, 0.05, 0.55] maxCorrection = 0.2 #square #predictedLF_foot = np.add(LF_foot,np.array([0.1,0.3,0.0])) #footOption0 = [0., 0., 0.] + predictedLF_foot #footOption1 = [0.,res, 0.] + predictedLF_foot #footOption2 = [res, 0., 0.] + predictedLF_foot #footOption3 = [res, res, 0.] + predictedLF_foot
def test_LP_friction_constraints_only(self):
math = Math()
# number of contacts
nc = 3
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_FRICTION
constraint_mode_IP = ['ONLY_FRICTION',
'ONLY_FRICTION',
'ONLY_FRICTION',
'ONLY_FRICTION']
useVariableJacobian = True
LF_foot = np.array([0.3, 0.2, -0.65])
RF_foot = np.array([0.3, -0.2, -0.65])
LH_foot = np.array([-0.2, 0.2, -0.4])
RH_foot = np.array([-0.3, -0.2, -0.65])
contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot))
contacts = contactsToStack[0:4, :]
''' parameters to be tuned'''
g = 9.81
trunk_mass = 90.
mu = 0.8
axisZ= np.array([[0.0], [0.0], [1.0]])
n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
# %% Cell 2
''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air'''
stanceFeet = [1, 1, 1, 1]
randomSwingLeg = random.randint(0, 3)
tripleStance = True # if you want you can define a swing leg using this variable
if tripleStance:
print 'Swing leg', randomSwingLeg
stanceFeet[randomSwingLeg] = 0
print 'stanceLegs ', stanceFeet
normals = np.vstack([n1, n2, n3, n4])
comp_dyn = ComputationalDynamics()
params = IterativeProjectionParameters()
params.setContactsPosWF(contacts)
#params.setCoMPosWF(comWF)
# params.setTorqueLims(torque_limits)
params.setActiveContacts(stanceFeet)
params.setConstraintModes(constraint_mode_IP)
params.setContactNormals(normals)
params.setFrictionCoefficient(mu)
params.setNumberOfFrictionConesEdges(ng)
params.setTotalMass(trunk_mass)
feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params)
print 'result',feasible, unfeasible, contact_forces
expected_feasible = np.array([[ -1.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ -1.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ -1.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ -1.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ -1.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 1.50000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 1.00000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 1.00000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 1.00000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 1.50000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 1.50000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 1.50000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -2.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -1.50000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -1.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -5.00000000e-02],
[ -1.11022302e-16, 5.00000000e-02, -5.55111512e-17],
[ -1.11022302e-16, 5.00000000e-02, 5.00000000e-02],
[ -1.11022302e-16, 5.00000000e-02, 1.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 1.00000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 1.00000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 1.00000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 1.50000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 1.50000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 1.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -2.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -1.50000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -1.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -5.00000000e-02],
[ 5.00000000e-02, -1.11022302e-16, -5.55111512e-17],
[ 5.00000000e-02, -1.11022302e-16, 5.00000000e-02],
[ 5.00000000e-02, -1.11022302e-16, 1.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -2.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -1.50000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -1.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -5.00000000e-02],
[ 5.00000000e-02, 5.00000000e-02, -5.55111512e-17],
[ 5.00000000e-02, 5.00000000e-02, 5.00000000e-02],
[ 5.00000000e-02, 5.00000000e-02, 1.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 1.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 1.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 1.00000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 1.50000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 1.50000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 1.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 2.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 2.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 2.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -2.00000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -1.50000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -1.00000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -5.00000000e-02],
[ 1.00000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 1.00000000e-01, -1.11022302e-16, 5.00000000e-02],
[ 1.00000000e-01, -1.11022302e-16, 1.00000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -2.00000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -1.50000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -1.00000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -5.00000000e-02],
[ 1.00000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 1.00000000e-01, 5.00000000e-02, 5.00000000e-02],
[ 1.00000000e-01, 5.00000000e-02, 1.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -2.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -1.50000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 1.00000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 1.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 1.50000000e-01, 1.00000000e-01],
[ 1.50000000e-01, -5.00000000e-02, -2.00000000e-01],
[ 1.50000000e-01, -5.00000000e-02, -1.50000000e-01],
[ 1.50000000e-01, -5.00000000e-02, -1.00000000e-01],
[ 1.50000000e-01, -5.00000000e-02, -5.00000000e-02],
[ 1.50000000e-01, -5.00000000e-02, -5.55111512e-17],
[ 1.50000000e-01, -5.00000000e-02, 5.00000000e-02],
[ 1.50000000e-01, -1.11022302e-16, -2.00000000e-01],
[ 1.50000000e-01, -1.11022302e-16, -1.50000000e-01],
[ 1.50000000e-01, -1.11022302e-16, -1.00000000e-01],
[ 1.50000000e-01, -1.11022302e-16, -5.00000000e-02],
[ 1.50000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 1.50000000e-01, -1.11022302e-16, 5.00000000e-02],
[ 1.50000000e-01, 5.00000000e-02, -2.00000000e-01],
[ 1.50000000e-01, 5.00000000e-02, -1.50000000e-01],
[ 1.50000000e-01, 5.00000000e-02, -1.00000000e-01],
[ 1.50000000e-01, 5.00000000e-02, -5.00000000e-02],
[ 1.50000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 1.50000000e-01, 5.00000000e-02, 5.00000000e-02],
[ 1.50000000e-01, 1.00000000e-01, -2.00000000e-01],
[ 1.50000000e-01, 1.00000000e-01, -1.50000000e-01],
[ 1.50000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 1.50000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 1.50000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 1.00000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 1.50000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 2.00000000e-01, -1.00000000e-01, -2.00000000e-01],
[ 2.00000000e-01, -1.00000000e-01, -1.50000000e-01],
[ 2.00000000e-01, -1.00000000e-01, -1.00000000e-01],
[ 2.00000000e-01, -1.00000000e-01, -5.00000000e-02],
[ 2.00000000e-01, -1.00000000e-01, -5.55111512e-17],
[ 2.00000000e-01, -1.00000000e-01, 5.00000000e-02],
[ 2.00000000e-01, -5.00000000e-02, -2.00000000e-01],
[ 2.00000000e-01, -5.00000000e-02, -1.50000000e-01],
[ 2.00000000e-01, -5.00000000e-02, -1.00000000e-01],
[ 2.00000000e-01, -5.00000000e-02, -5.00000000e-02],
[ 2.00000000e-01, -5.00000000e-02, -5.55111512e-17],
[ 2.00000000e-01, -5.00000000e-02, 5.00000000e-02],
[ 2.00000000e-01, -1.11022302e-16, -2.00000000e-01],
[ 2.00000000e-01, -1.11022302e-16, -1.50000000e-01],
[ 2.00000000e-01, -1.11022302e-16, -1.00000000e-01],
[ 2.00000000e-01, -1.11022302e-16, -5.00000000e-02],
[ 2.00000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 2.00000000e-01, -1.11022302e-16, 5.00000000e-02],
[ 2.00000000e-01, 5.00000000e-02, -2.00000000e-01],
[ 2.00000000e-01, 5.00000000e-02, -1.50000000e-01],
[ 2.00000000e-01, 5.00000000e-02, -1.00000000e-01],
[ 2.00000000e-01, 5.00000000e-02, -5.00000000e-02],
[ 2.00000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 2.00000000e-01, 5.00000000e-02, 5.00000000e-02],
[ 2.00000000e-01, 1.00000000e-01, -2.00000000e-01],
[ 2.00000000e-01, 1.00000000e-01, -1.50000000e-01],
[ 2.00000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 2.00000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 2.00000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 2.00000000e-01, 1.00000000e-01, 5.00000000e-02],
[ 2.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 2.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 2.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 2.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 2.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 2.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 2.00000000e-01, 2.00000000e-01, -2.00000000e-01],
[ 2.00000000e-01, 2.00000000e-01, -1.50000000e-01],
[ 2.00000000e-01, 2.00000000e-01, -1.00000000e-01],
[ 2.00000000e-01, 2.00000000e-01, -5.00000000e-02],
[ 2.00000000e-01, 2.00000000e-01, -5.55111512e-17],
[ 2.00000000e-01, 2.00000000e-01, 5.00000000e-02],
[ 2.50000000e-01, -1.50000000e-01, -2.00000000e-01],
[ 2.50000000e-01, -1.50000000e-01, -1.50000000e-01],
[ 2.50000000e-01, -1.50000000e-01, -1.00000000e-01],
[ 2.50000000e-01, -1.50000000e-01, -5.00000000e-02],
[ 2.50000000e-01, -1.50000000e-01, -5.55111512e-17],
[ 2.50000000e-01, -1.00000000e-01, -2.00000000e-01],
[ 2.50000000e-01, -1.00000000e-01, -1.50000000e-01],
[ 2.50000000e-01, -1.00000000e-01, -1.00000000e-01],
[ 2.50000000e-01, -1.00000000e-01, -5.00000000e-02],
[ 2.50000000e-01, -1.00000000e-01, -5.55111512e-17],
[ 2.50000000e-01, -5.00000000e-02, -2.00000000e-01],
[ 2.50000000e-01, -5.00000000e-02, -1.50000000e-01],
[ 2.50000000e-01, -5.00000000e-02, -1.00000000e-01],
[ 2.50000000e-01, -5.00000000e-02, -5.00000000e-02],
[ 2.50000000e-01, -5.00000000e-02, -5.55111512e-17],
[ 2.50000000e-01, -1.11022302e-16, -2.00000000e-01],
[ 2.50000000e-01, -1.11022302e-16, -1.50000000e-01],
[ 2.50000000e-01, -1.11022302e-16, -1.00000000e-01],
[ 2.50000000e-01, -1.11022302e-16, -5.00000000e-02],
[ 2.50000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 2.50000000e-01, 5.00000000e-02, -2.00000000e-01],
[ 2.50000000e-01, 5.00000000e-02, -1.50000000e-01],
[ 2.50000000e-01, 5.00000000e-02, -1.00000000e-01],
[ 2.50000000e-01, 5.00000000e-02, -5.00000000e-02],
[ 2.50000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 2.50000000e-01, 1.00000000e-01, -2.00000000e-01],
[ 2.50000000e-01, 1.00000000e-01, -1.50000000e-01],
[ 2.50000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 2.50000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 2.50000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 2.50000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 2.50000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 2.50000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 2.50000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 2.50000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 3.00000000e-01, -1.50000000e-01, -2.00000000e-01],
[ 3.00000000e-01, -1.50000000e-01, -1.50000000e-01],
[ 3.00000000e-01, -1.50000000e-01, -1.00000000e-01],
[ 3.00000000e-01, -1.50000000e-01, -5.00000000e-02],
[ 3.00000000e-01, -1.50000000e-01, -5.55111512e-17],
[ 3.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 3.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 3.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 3.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 3.00000000e-01, 1.50000000e-01, -5.55111512e-17]]);
# print expected_feasible
rows = np.size(expected_feasible, 0)
cols = np.size(expected_feasible, 1)
for i in range(0, rows):
for j in range (0, cols):
self.assertAlmostEquals(expected_feasible[i,j], feasible[i,j], self.assertPrecision)
def test_LP_actuation_constraints_only(self):
math = Math()
# number of contacts
nc = 3
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_ACTUATION or ONLY_FRICTION
constraint_mode = 'ONLY_ACTUATION'
useVariableJacobian = True
""" contact points """
LF_foot = np.array([0.3, 0.2, -0.65])
RF_foot = np.array([0.3, -0.2, -0.65])
LH_foot = np.array([-0.2, 0.2, -0.4])
RH_foot = np.array([-0.3, -0.2, -0.65])
contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot))
contacts = contactsToStack[0:nc, :]
''' parameters to be tuned'''
g = 9.81
trunk_mass = 90.
mu = 0.8
axisZ= np.array([[0.0], [0.0], [1.0]])
n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
# %% Cell 2
normals = np.vstack([n1, n2, n3, n4])
comp_dyn = ComputationalDynamics()
params = IterativeProjectionParameters()
params.setContactsPosWF(contacts)
# params.setCoMPosWF(comWF)
# params.setTorqueLims(torque_limits)
# params.setActiveContacts(stanceFeet)
# params.setConstraintModes(constraint_mode_IP)
params.setContactNormals(normals)
params.setFrictionCoefficient(mu)
params.setNumberOfFrictionConesEdges(ng)
params.setTotalMass(trunk_mass)
feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params)
expected_feasible = np.array([[ -1.50000000e-01, 2.50000000e-01, 5.00000000e-02],
[ -1.50000000e-01, 2.50000000e-01, 1.00000000e-01],
[ -1.50000000e-01, 3.00000000e-01, 5.00000000e-02],
[ -1.50000000e-01, 3.00000000e-01, 1.00000000e-01],
[ -1.50000000e-01, 3.50000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 1.00000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 1.00000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 1.00000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 1.50000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 2.00000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 2.00000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 2.00000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 2.50000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 2.50000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 2.50000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 3.00000000e-01, -5.55111512e-17],
[ -1.00000000e-01, 3.00000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 3.00000000e-01, 1.00000000e-01],
[ -1.00000000e-01, 3.50000000e-01, 5.00000000e-02],
[ -1.00000000e-01, 3.50000000e-01, 1.00000000e-01],
[ -5.00000000e-02, -1.11022302e-16, -1.00000000e-01],
[ -5.00000000e-02, -1.11022302e-16, -5.00000000e-02],
[ -5.00000000e-02, -1.11022302e-16, -5.55111512e-17],
[ -5.00000000e-02, -1.11022302e-16, 5.00000000e-02],
[ -5.00000000e-02, -1.11022302e-16, 1.00000000e-01],
[ -5.00000000e-02, 5.00000000e-02, -1.50000000e-01],
[ -5.00000000e-02, 5.00000000e-02, -1.00000000e-01],
[ -5.00000000e-02, 5.00000000e-02, -5.00000000e-02],
[ -5.00000000e-02, 5.00000000e-02, -5.55111512e-17],
[ -5.00000000e-02, 5.00000000e-02, 5.00000000e-02],
[ -5.00000000e-02, 5.00000000e-02, 1.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 1.00000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 1.00000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 1.00000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 1.00000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 1.50000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 1.50000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 1.50000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 1.50000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 2.00000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 2.00000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 2.00000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 2.00000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 2.00000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 2.00000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 2.00000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 2.50000000e-01, -2.00000000e-01],
[ -5.00000000e-02, 2.50000000e-01, -1.50000000e-01],
[ -5.00000000e-02, 2.50000000e-01, -1.00000000e-01],
[ -5.00000000e-02, 2.50000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 2.50000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 2.50000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 2.50000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 3.00000000e-01, -5.00000000e-02],
[ -5.00000000e-02, 3.00000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 3.00000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 3.00000000e-01, 1.00000000e-01],
[ -5.00000000e-02, 3.50000000e-01, -5.55111512e-17],
[ -5.00000000e-02, 3.50000000e-01, 5.00000000e-02],
[ -5.00000000e-02, 3.50000000e-01, 1.00000000e-01],
[ -1.11022302e-16, -5.00000000e-02, -1.00000000e-01],
[ -1.11022302e-16, -5.00000000e-02, -5.00000000e-02],
[ -1.11022302e-16, -5.00000000e-02, -5.55111512e-17],
[ -1.11022302e-16, -5.00000000e-02, 5.00000000e-02],
[ -1.11022302e-16, -5.00000000e-02, 1.00000000e-01],
[ -1.11022302e-16, -1.11022302e-16, -1.50000000e-01],
[ -1.11022302e-16, -1.11022302e-16, -1.00000000e-01],
[ -1.11022302e-16, -1.11022302e-16, -5.00000000e-02],
[ -1.11022302e-16, -1.11022302e-16, -5.55111512e-17],
[ -1.11022302e-16, -1.11022302e-16, 5.00000000e-02],
[ -1.11022302e-16, -1.11022302e-16, 1.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -1.50000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -1.00000000e-01],
[ -1.11022302e-16, 5.00000000e-02, -5.00000000e-02],
[ -1.11022302e-16, 5.00000000e-02, -5.55111512e-17],
[ -1.11022302e-16, 5.00000000e-02, 5.00000000e-02],
[ -1.11022302e-16, 5.00000000e-02, 1.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 1.00000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 1.00000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 1.00000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 1.00000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 1.50000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 1.50000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 1.50000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 1.50000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 2.00000000e-01, -2.00000000e-01],
[ -1.11022302e-16, 2.00000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 2.00000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 2.00000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 2.00000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 2.00000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 2.00000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 2.50000000e-01, -1.50000000e-01],
[ -1.11022302e-16, 2.50000000e-01, -1.00000000e-01],
[ -1.11022302e-16, 2.50000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 2.50000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 2.50000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 2.50000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 3.00000000e-01, -5.00000000e-02],
[ -1.11022302e-16, 3.00000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 3.00000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 3.00000000e-01, 1.00000000e-01],
[ -1.11022302e-16, 3.50000000e-01, -5.55111512e-17],
[ -1.11022302e-16, 3.50000000e-01, 5.00000000e-02],
[ -1.11022302e-16, 3.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, -5.00000000e-02, -1.00000000e-01],
[ 5.00000000e-02, -5.00000000e-02, -5.00000000e-02],
[ 5.00000000e-02, -5.00000000e-02, -5.55111512e-17],
[ 5.00000000e-02, -5.00000000e-02, 5.00000000e-02],
[ 5.00000000e-02, -5.00000000e-02, 1.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -1.00000000e-01],
[ 5.00000000e-02, -1.11022302e-16, -5.00000000e-02],
[ 5.00000000e-02, -1.11022302e-16, -5.55111512e-17],
[ 5.00000000e-02, -1.11022302e-16, 5.00000000e-02],
[ 5.00000000e-02, -1.11022302e-16, 1.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -1.00000000e-01],
[ 5.00000000e-02, 5.00000000e-02, -5.00000000e-02],
[ 5.00000000e-02, 5.00000000e-02, -5.55111512e-17],
[ 5.00000000e-02, 5.00000000e-02, 5.00000000e-02],
[ 5.00000000e-02, 5.00000000e-02, 1.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 1.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 1.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 1.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 1.00000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 1.50000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 1.50000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 1.50000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 1.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -2.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 2.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 2.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 2.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 2.00000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 2.50000000e-01, -1.50000000e-01],
[ 5.00000000e-02, 2.50000000e-01, -1.00000000e-01],
[ 5.00000000e-02, 2.50000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 2.50000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 2.50000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 2.50000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 3.00000000e-01, -5.00000000e-02],
[ 5.00000000e-02, 3.00000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 3.00000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 3.00000000e-01, 1.00000000e-01],
[ 5.00000000e-02, 3.50000000e-01, -5.55111512e-17],
[ 5.00000000e-02, 3.50000000e-01, 5.00000000e-02],
[ 5.00000000e-02, 3.50000000e-01, 1.00000000e-01],
[ 1.00000000e-01, -5.00000000e-02, -5.55111512e-17],
[ 1.00000000e-01, -5.00000000e-02, 5.00000000e-02],
[ 1.00000000e-01, -5.00000000e-02, 1.00000000e-01],
[ 1.00000000e-01, -1.11022302e-16, -5.55111512e-17],
[ 1.00000000e-01, -1.11022302e-16, 5.00000000e-02],
[ 1.00000000e-01, -1.11022302e-16, 1.00000000e-01],
[ 1.00000000e-01, 5.00000000e-02, -5.55111512e-17],
[ 1.00000000e-01, 5.00000000e-02, 5.00000000e-02],
[ 1.00000000e-01, 5.00000000e-02, 1.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 1.00000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 1.00000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 1.00000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 1.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -1.50000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 1.50000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 1.50000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 2.00000000e-01, -2.00000000e-01],
[ 1.00000000e-01, 2.00000000e-01, -1.50000000e-01],
[ 1.00000000e-01, 2.00000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 2.00000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 2.00000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 2.00000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 2.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 2.50000000e-01, -1.00000000e-01],
[ 1.00000000e-01, 2.50000000e-01, -5.00000000e-02],
[ 1.00000000e-01, 2.50000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 2.50000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 2.50000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 3.00000000e-01, -5.55111512e-17],
[ 1.00000000e-01, 3.00000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 3.00000000e-01, 1.00000000e-01],
[ 1.00000000e-01, 3.50000000e-01, 5.00000000e-02],
[ 1.00000000e-01, 3.50000000e-01, 1.00000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 1.50000000e-01, 1.50000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 1.50000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 2.00000000e-01, -2.00000000e-01],
[ 1.50000000e-01, 2.00000000e-01, -1.50000000e-01],
[ 1.50000000e-01, 2.00000000e-01, -1.00000000e-01],
[ 1.50000000e-01, 2.00000000e-01, -5.00000000e-02],
[ 1.50000000e-01, 2.00000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 2.00000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 2.50000000e-01, -1.00000000e-01],
[ 1.50000000e-01, 2.50000000e-01, -5.00000000e-02],
[ 1.50000000e-01, 2.50000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 2.50000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 3.00000000e-01, -5.55111512e-17],
[ 1.50000000e-01, 3.00000000e-01, 5.00000000e-02],
[ 1.50000000e-01, 3.50000000e-01, 5.00000000e-02],
[ 2.00000000e-01, 1.50000000e-01, -2.00000000e-01],
[ 2.00000000e-01, 2.00000000e-01, -1.50000000e-01],
[ 2.00000000e-01, 2.50000000e-01, -5.00000000e-02],
[ 2.00000000e-01, 2.50000000e-01, -5.55111512e-17],
[ 2.00000000e-01, 3.00000000e-01, -5.55111512e-17]]);
# print expected_feasible
rows = np.size(expected_feasible, 0)
cols = np.size(expected_feasible, 1)
for i in range(0, rows):
for j in range (0, cols):
self.assertAlmostEquals(expected_feasible[i,j], feasible[i,j], self.assertPrecision)
def test_lp_stability_check(self):
math = Math()
# number of contacts
nc = 3
# number of generators, i.e. rays used to linearize the friction cone
ng = 4
# ONLY_FRICTION
# ONLY_ACTUATION
constraint_mode_IP = ['ONLY_ACTUATION',
'ONLY_ACTUATION',
'ONLY_ACTUATION',
'ONLY_ACTUATION']
useVariableJacobian = True
comWF = np.array([1.25, 0.0, 0.0])
""" contact points in the World Frame"""
LF_foot = np.array([1.3, 0.2, -0.6])
RF_foot = np.array([1.3, -0.2, -0.5])
LH_foot = np.array([0.7, 0.2, -0.45])
RH_foot = np.array([0.7, -0.2, -0.5])
contactsToStack = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot))
contacts = contactsToStack[0:4, :]
''' parameters to be tuned'''
g = 9.81
trunk_mass = 50.
mu = 0.8
axisZ = np.array([[0.0], [0.0], [1.0]])
n1 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n2 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n3 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n4 = np.transpose(np.transpose(math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
# %% Cell 2
''' stanceFeet vector contains 1 is the foot is on the ground and 0 if it is in the air'''
stanceFeet = [1, 1, 1, 1]
randomSwingLeg = random.randint(0, 3)
tripleStance = True # if you want you can define a swing leg using this variable
if tripleStance:
print 'Swing leg', randomSwingLeg
#stanceFeet[randomSwingLeg] = 0
print 'stanceLegs ', stanceFeet
normals = np.vstack([n1, n2, n3, n4])
comp_dyn = ComputationalDynamics()
params = IterativeProjectionParameters()
params.setContactsPosWF(contacts)
params.setCoMPosWF(comWF)
# params.setTorqueLims(torque_limits)
params.setActiveContacts(stanceFeet)
params.setConstraintModes(constraint_mode_IP)
params.setContactNormals(normals)
params.setFrictionCoefficient(mu)
params.setNumberOfFrictionConesEdges(ng)
params.setTotalMass(trunk_mass)
status, x, force_polytopes = comp_dyn.check_equilibrium(params)
print status, x
'''Plotting the contact points in the 3D figure'''
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')
''' plotting Iterative Projection points '''
plotter = Plotter()
scaling_factor = 2000
stanceID = params.getStanceIndex(stanceFeet)
for j in range(0, nc): # this will only show the force polytopes of the feet that are defined to be in stance
idx = int(stanceID[j])
if (constraint_mode_IP[idx] == 'ONLY_ACTUATION') or (constraint_mode_IP[idx] == 'FRICTION_AND_ACTUATION'):
plotter.plot_actuation_polygon(ax, force_polytopes[idx], contacts[idx, :], scaling_factor)
plt.show()
def talker():
compDyn = ComputationalDynamics()
math = Math()
p=HyQSim()
p.start()
p.register_node()
name = "Actuation_region"
point = Point()
actuationParams = ActuationParameters()
i = 0
start_t_IP = time.time()
for j in range (0,100):
vertices = [point]
# print("Time: " + str(i*0.004) + "s and Simulation time: " + str(p.get_sim_time()/60))
p.get_sim_wbs()
actuationParams.getParams(p.hyq_rcf_debug)
trunk_mass = 85.
axisZ= np.array([[0.0], [0.0], [1.0]])
''' normals '''
n1 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n2 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n3 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
n4 = np.transpose(np.transpose(math.rpyToRot(0.0,0.0,0.0)).dot(axisZ))
normals = np.vstack([n1, n2, n3])
""" contact points """
nc = actuationParams.numberOfContacts
contacts = actuationParams.contacts[0:nc+1, :]
LF_tau_lim = [50.0, 100.0, 100.0]
RF_tau_lim = [50.0, 100.0, 100.0]
LH_tau_lim = [50.0, 100.0, 100.0]
RH_tau_lim = [50.0, 100.0, 100.0]
torque_limits = np.array([LF_tau_lim, RF_tau_lim, LH_tau_lim, RH_tau_lim])
comWF = np.array([0.0, 0.0, 0.0])
extForceW = np.array([0.0,0.0, 0.0])
constraint_mode_IP = ['FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION']
mu = 0.8
ng = 4
# print 'contacts: ',contacts
# print contacts, actuationParams.stanceFeet
params = IterativeProjectionParameters()
stanceFeet = [1,1,1,1]
params.setContactsPosWF(contacts)
params.setCoMPosWF(comWF)
params.setTorqueLims(torque_limits)
params.setActiveContacts(stanceFeet)
params.setConstraintModes(constraint_mode_IP)
params.setContactNormals(normals)
params.setFrictionCoefficient(mu)
params.setNumberOfFrictionConesEdges(ng)
params.setTotalMass(trunk_mass + extForceW[2]/9.81)
params.externalForceWF = extForceW
''' compute iterative projection '''
IAR, actuation_polygons, computation_time = compDyn.iterative_projection_bretl(params)
number_of_vertices = np.size(IAR, 0)
# number_of_vertices = 10
# print IAR
for i in range(0, number_of_vertices):
point = Point()
point.x = IAR[i][0]
point.y = IAR[i][1]
point.z = 0.0
vertices = np.hstack([vertices, point])
# print'vertices', vertices
p.send_polygons(name, vertices)
# time.sleep(1.0/5.0)
i+=1
print 'de registering...'
p.deregister_node()
computation_time = (time.time() - start_t_IP)
print("Total time: --- %s seconds ---" % computation_time)
print 'number of published messages ', actuationParams.numberOfPublishedMessages
avgTime = computation_time/actuationParams.numberOfPublishedMessages
print 'average publishing time [ms]', avgTime
print 'average publishing frequency [Hz]', 1.0/avgTime
print 'number of received messages ', p.numberOfReceivedMessages
avgTime = computation_time/p.numberOfReceivedMessages
print 'average subscription time [ms]', avgTime
print 'average subscription frequency [Hz]', 1.0/avgTime