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])
params = IterativeProjectionParameters()
params.setContactsPos(contacts)
params.setCoMPos(comWF)
params.setTorqueLims(torque_limits)
params.setActiveContacts(stanceFeet)
params.setConstraintModes(constraint_mode_IP)
params.setContactNormals(normals)
params.setFrictionCoefficient(mu)
params.setNumberOfFrictionConesEdges(ng)
params.setTrunkMass(trunk_mass)
feasible, unfeasible, contact_forces = compDyn.LP_projection(
params, useVariableJacobian, 0.2, 0.2)
print feasible
print unfeasible
#desired_direction = [-1.0, -1.0]
IP_points1, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(
params)
comWF = np.array([-0.205, 0.1, 0.0])
params.setCoMPos(comWF)
IP_points2, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(
params)
comWF = np.array([-0.131, 0.1, 0.0])
params.setCoMPos(comWF)
IP_points3, actuation_polygons, comp_time = compDyn.iterative_projection_bretl(
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)
(time.time() - start_t_IPVC))
''' plotting Iterative Projection points '''
axisZ = 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])
IP_points, actuation_polygons = compDyn.iterative_projection_bretl(
constraint_mode, contacts, normals, trunk_mass, ng, mu)
feasible, unfeasible, contact_forces = compDyn.LP_projection(
constraint_mode, contacts, normals, trunk_mass, mu, ng, nc, mu,
useVariableJacobian, 0.025, 0.025)
#IP_points_saturated_friction, actuation_polygons = compDyn.iterative_projection_bretl('ONLY_FRICTION', contacts, normals, trunk_mass, ng, mu, saturate_normal_force = True)
'''Plotting'''
#plt.close('all')
plotter = Plotter()
plt.figure()
plt.grid()
plt.xlabel("X [m]")
plt.ylabel("Y [m]")
h1 = plt.plot(contacts[0:nc, 0],
contacts[0:nc, 1],
'ko',
markersize=15,
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)
params.setFrictionCoefficient(mu) params.setNumberOfFrictionConesEdges(ng) params.setTotalMass(trunk_mass) params.externalForceWF = extForceW # params.externalForceWF is actually used anywhere at the moment ''' compute iterative projection Outputs of "iterative_projection_bretl" are: IP_points = resulting 2D vertices actuation_polygons = these are the vertices of the 3D force polytopes (one per leg) computation_time = how long it took to compute the iterative projection ''' IP_points, force_polytopes, IP_computation_time = comp_dyn.iterative_projection_bretl(params) ''' plotting Iterative Projection points ''' feasible, unfeasible, contact_forces = comp_dyn.LP_projection(params) #print contact_forces #for i in range(0, np.size(contact_forces,0)): # for j in range(0,nc): # a = Arrow3D([contacts[j,0], contacts[j,0]+contact_forces[i,j*3]/200], [contacts[j,1], contacts[j,1]+contact_forces[i,j*3+1]/200],[contacts[j,2], contacts[j,2]+contact_forces[i,j*3+2]/200], mutation_scale=20, lw=3, arrowstyle="-|>", color="g") # ax.add_artist(a) #a1 = Arrow3D([0.0, 0.0],[ 0.0,0.0],[ 0.0, -trunk_mass*g/scaling_factor], mutation_scale=20, lw=3, arrowstyle="-|>", color="b") #ax.add_artist(a1) ''' plotting LP test points ''' #if np.size(feasible,0) != 0: # ax.scatter(feasible[:,0], feasible[:,1], feasible[:,2],c='g',s=50) #if np.size(unfeasible,0) != 0: # ax.scatter(unfeasible[:,0], unfeasible[:,1], unfeasible[:,2],c='r',s=50)