g = 9.81
grav = np.array([[0.], [0.], [-g * mass]])
""" contact points """
LF_foot = np.array([0.3, 0.2, -.5])
RF_foot = np.array([0.3, -0.2, -.5])
LH_foot = np.array([-0.3, 0.2, -.5])
RH_foot = np.array([-0.3, -0.2, -.5])
contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot))
""" normals of the surface in the contact points """
math = Math()
LF_normal = np.array([[0.0], [0.0], [1.0]])
RF_normal = np.array([[0.0], [0.0], [1.0]])
LH_normal = np.array([[0.0], [0.0], [1.0]])
RH_normal = np.array([[0.0], [0.0], [1.0]])
LF_normal, RF_normal, LH_normal, RH_normal = (
math.normalize(n) for n in [LF_normal, RF_normal, LH_normal, RH_normal])
normals = np.hstack((LF_normal, RF_normal, LH_normal, RH_normal))
#p = matrix(np.ones((3*nc,1)))
#
#cons1 = np.zeros((0,0))
#h_vec1 = np.zeros((0,1))
#cons2 = np.zeros((0,0))
#h_vec2 = np.zeros((0,1))
#
#kin = Kinematics()
#foot_vel = np.array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]])
#q, q_dot, J_LF, J_RF, J_LH, J_RH = kin.compute_xy_IK(np.transpose(contacts[:,0]),
# np.transpose(foot_vel[:,0]),
# np.transpose(contacts[:,2]),
# np.transpose(foot_vel[:,2]))
# number of decision variables of the problem
n = nc * 6
# contact positions
""" contact points """
LF_foot = np.array([0.3, 0.2, -.5])
print LF_foot
RF_foot = np.array([0.3, -0.2, -.5])
LH_foot = np.array([-0.3, 0.2, -.5])
RH_foot = np.array([-0.3, -0.2, -.5])
contacts = np.vstack((LF_foot, RF_foot, LH_foot, RH_foot))
''' parameters to be tuned'''
g = 9.81
mass = 100.
mu = 1.
n1 = array([0.0, 0.0, 1.0])
n2 = array([0.0, 0.0, 1.0])
n3 = array([0.0, 0.0, 1.0])
math = Math()
n1, n2, n3 = (math.normalize(n) for n in [n1, n2, n3])
normals = np.vstack([n1, n2, n3])
comp_dyn = ComputationalDynamics()
comp_dyn.iterative_projection_bretl(constraint_mode, contacts, normals, mass,
ng, mu)
print("--- %s seconds ---" % (time.time() - start_time))
# pypoman.plot_polygon(points)
def project(self, params):
start_t = time.time()
contacts = params.getContactsPosWF()
contactsNumber = np.size(contacts,0)
r1 = contacts[0,:]
r2 = contacts[1,:]
r3 = contacts[2,:]
g = 9.81
mg = params.getTotalMass()*g
normals = params.setContactNormals()
n1 = normals[0,:]
n2 = normals[1,:]
n3 = normals[2,:]
math_lp = Math()
constr = Constraints()
if constraint_mode == 'ONLY_ACTUATION':
tau_HAA = 80
tau_HFE = 120
tau_KFE = 120
dx = tau_HAA
dy = tau_HFE
dz = tau_KFE
vertices_cl = np.array([[dx, dx, -dx, -dx, dx, dx, -dx, -dx],
[dy, -dy, -dy, dy, dy, -dy, -dy, dy],
[dz, dz, dz, dz, -dz, -dz, -dz, -dz]])
kin = HyQKinematics()
foot_vel = np.array([[0, 0, 0],[0, 0, 0],[0, 0, 0],[0, 0, 0]])
contactsFourLegs = np.vstack([contacts, np.zeros((4-contactsNumber,3))])
q, q_dot, J_LF, J_RF, J_LH, J_RH,isOutOfWorkspace = kin.inverse_kin(np.transpose(contactsFourLegs[:,0]),
np.transpose(foot_vel[:,0]),
np.transpose(contactsFourLegs[:,1]),
np.transpose(foot_vel[:,1]),
np.transpose(contactsFourLegs[:,2]),
np.transpose(foot_vel[:,2]))
J_LF, J_RF, J_LH, J_RH = kin.update_jacobians(q)
vertices_1 = np.transpose(constr.computeActuationPolygon(J_LF))
vertices_2 = np.transpose(constr.computeActuationPolygon(J_RF))
vertices_3 = np.transpose(constr.computeActuationPolygon(J_LH))
#print vertices_1
#print vertices_2
#print vertices_3
elif constraint_mode == 'ONLY_FRICTION':
n1, n2, n3 = (math_lp.normalize(n) for n in [n1, n2, n3])
R1, R2, R3 = (math_lp.rotation_matrix_from_normal(n) for n in [n1, n2, n3])
# Inequality matrix for a contact force in local contact frame:
#constr = Constraints()
#C_force = constr.linearized_cone_local_frame(ng, mu)
vertices_cl = constr.linearized_cone_vertices(ng, mu, mg)
#vertices_cl = np.array([[0., dx, dx, -dx, -dx],
# [0., dy, -dy, -dy, dy],
# [0., dz, dz, dz, dz]])
vertices_1 = np.dot(vertices_cl, R1.T)
vertices_2 = np.dot(vertices_cl, R2.T)
vertices_3 = np.dot(vertices_cl, R3.T)
#vertices_1 = vertices_cl
#vertices_2 = vertices_cl
#vertices_3 = vertices_cl
vertices_1 = np.transpose(vertices_1)
vertices_2 = np.transpose(vertices_2)
vertices_3 = np.transpose(vertices_3)
tau1 = np.zeros((3,np.size(vertices_1,1)))
tau2 = np.zeros((3,np.size(vertices_2,1)))
tau3 = np.zeros((3,np.size(vertices_3,1)))
#print tau1
for j in range(0,np.size(vertices_cl,1)):
tau1[:,j] = np.cross(r1, vertices_1[:,j])
tau2[:,j] = np.cross(r2, vertices_2[:,j])
tau3[:,j] = np.cross(r3, vertices_3[:,j])
w1 = np.vstack([tau1, vertices_1])
w2 = np.vstack([tau2, vertices_2])
w3 = np.vstack([tau3, vertices_3]) #6XN
print w1, w2, w3
w12 = self.minksum(w1, w2)
w123 = self.minksum(w12, w3) #CWC con punti interni 6XN
#print 'number of vertex before convex hull', np.size(w123,1)
w123_hull = self.convex_hull(w123) #this speeds up significantly!
#print 'number of vertex after convex hull', np.size(w123_hull,1)
#compute edges and slice them with mg
pointsfZ, points_num = self.compute_section_points(w123_hull, np.array([0,0,0,0,0,1]), np.array([0,0,0,0,0,mg]))
#to avoid that convex hull complains because they are complanar remove the fz dimension
pointsfZhull = self.convex_hull(pointsfZ[0:5,:])
pointsfY, points_num = self.compute_section_points(pointsfZhull, np.array([0,0,0,0,1]), np.array([0,0,0,0,0]))
#to avoid that convex hull complains because they are complanar remove the fz dimension
pointsfYhull = self.convex_hull(pointsfY[0:4,:])
pointsfX, points_num = self.compute_section_points(pointsfYhull, np.array([0,0,0,1]), np.array([0,0,0,0]))
#to avoid that convex hull complains because they are complanar remove the fz dimension
pointsfXhull = self.convex_hull(pointsfX[0:3,:])
pointstZ, points_num = self.compute_section_points(pointsfXhull, np.array([0,0,1]), np.array([0,0,0]))
#to avoid that convex hull complains because they are complanar remove the fz dimension
pointstZhull = self.convex_hull(pointstZ[0:2,:])
# pointsfX, points_num = self.compute_section_points(pointsfZhull, np.array([0,0,0,1,0,0]), np.array([0,0,0,0,0,0]))
#pointstZ, points_num = self.compute_section_points(pointsfY, np.array([0,0,1,0,0,0]), np.array([0,0,0,0,0,0]))
print np.size(w123_hull,1)
print "pointsfZ"
print np.size(pointsfZ,1)
print np.size(pointsfZhull,1)
print "pointsfy"
print np.size(pointsfY,1)
print np.size(pointsfYhull,1) #
print "pointsfx"
print np.size(pointsfX,1)
print np.size(pointsfXhull,1) #
print "pointstZx"
print np.size(pointstZ,1)
print np.size(pointstZhull,1) #
print pointstZhull
# points, points_num = self.compute_section_points(w123_hull, mg)
#
# TODO: use pointsfZhull instead for "ONLY_FRICTION" option:
#points2d = self.project_points(pointsfZhull, mg) #cimpute com points
if constraint_mode == 'ONLY_ACTUATION':
points2d = self.project_points(pointstZhull, mg) #cimpute com points
elif constraint_mode == 'ONLY_FRICTION':
points2d = self.project_points(pointsfZhull, mg) #cimpute com points
#print points
vertices2d = np.transpose(points2d)
chull = scipy.spatial.ConvexHull(vertices2d)
#print chull.vertices[0]
print("Closed form algorith: --- %s seconds ---" % (time.time() - start_t))
return vertices2d, chull.simplices