def __init__(self):
self.math = Math()
self.q = [0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.]
self.comPositionBF = [0., 0.,
0.] #var used only for IK inside constraints.py
self.comPositionWF = [0., 0., 0.]
self.footPosWLF = [0.3, 0.2, -.0]
self.footPosWRF = [0.3, -0.2, -.0]
self.footPosWLH = [-0.3, 0.2, -.0]
self.footPosWRH = [-0.3, -0.2, -.0]
self.externalForceWF = [0., 0., 0.]
self.roll = 0.0
self.pitch = 0.0
self.yaw = 0.0
self.LF_tau_lim = [50.0, 50.0, 50.0]
self.RF_tau_lim = [50.0, 50.0, 50.0]
self.LH_tau_lim = [50.0, 50.0, 50.0]
self.RH_tau_lim = [50.0, 50.0, 50.0]
self.torque_limits = np.array([
self.LF_tau_lim, self.RF_tau_lim, self.LH_tau_lim, self.RH_tau_lim
])
self.state_machineLF = True
self.state_machineRF = True
self.state_machineLH = True
self.state_machineRH = True
self.stanceFeet = [0, 0, 0, 0]
self.numberOfContacts = 0
# self.contactsHF = np.zeros((4,3))
self.contactsBF = np.zeros((4, 3))
self.contactsWF = np.zeros((4, 3))
axisZ = np.array([[0.0], [0.0], [1.0]])
n1 = np.transpose(
np.transpose(self.math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n2 = np.transpose(
np.transpose(self.math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n3 = np.transpose(
np.transpose(self.math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
n4 = np.transpose(
np.transpose(self.math.rpyToRot(0.0, 0.0, 0.0)).dot(axisZ))
# %% Cell 2
self.normals = np.vstack([n1, n2, n3, n4])
self.constraintMode = [
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION',
'FRICTION_AND_ACTUATION', 'FRICTION_AND_ACTUATION'
]
self.friction = 0.8
self.robotMass = 85 #Kg
self.numberOfGenerators = 4
self.actual_swing = 0
def talker():
gridmap = GridMap()
math = Math()
p = HyQSim()
p.start()
p.register_node()
name = "Actuation_region"
point = Point()
polygonVertex = Point()
actPolygon = Polygon3D()
converter = gridMapConverter()
i = 0
for j in range(0, 50):
vertices1 = [point]
actPolygons = [polygonVertex]
# poly = []
# print("Time: " + str(i*0.004) + "s and Simulation time: " + str(p.get_sim_time()/60))
p.get_sim_wbs()
converter.getParamsFromRosDebugTopic(p.hyq_rcf_debug)
trunk_mass = 85.
mu = 0.8
ng = 4
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, n4])
print 'contacts: ', 1
time.sleep(1.0 / 25.0)
i += 1
print 'de registering...'
p.deregister_node()
def computeVirtualImpedanceWrench(conf, act_pose, act_twist, W_contacts,
des_pose, des_twist, des_acc, stance_legs,
W_base_to_com, isCoMControlled, GravityComp,
ffwdOn):
util = Utils()
# The inertia matrix (for simplicity we will use the same for com and base frame)
B_Inertia = np.array(
[[conf.robotInertia.Ixx, conf.robotInertia.Ixy, conf.robotInertia.Ixz],
[conf.robotInertia.Ixy, conf.robotInertia.Iyy, conf.robotInertia.Iyz],
[conf.robotInertia.Ixz, conf.robotInertia.Iyz,
conf.robotInertia.Izz]])
# Load math functions
mathJet = Math()
# Feedback wrench (Virtual PD)
Kp_lin = np.diag([conf.Kp_lin_x, conf.Kp_lin_y, conf.Kp_lin_z])
Kd_lin = np.diag([conf.Kd_lin_x, conf.Kd_lin_y, conf.Kd_lin_z])
Kp_ang = np.diag([conf.KpRoll, conf.KpPitch, conf.KpYaw])
Kd_ang = np.diag([conf.KdRoll, conf.KdPitch, conf.KdYaw])
Wfbk = np.zeros(6)
# linear part
Wfbk[util.sp_crd["LX"]:util.sp_crd["LX"] +
3] = Kp_lin.dot(util.linPart(des_pose) -
util.linPart(act_pose)) + Kd_lin.dot(
util.linPart(des_twist) - util.linPart(act_twist))
# angular part
# actual orientation
b_R_w = mathJet.rpyToRot(util.angPart(act_pose))
# Desired Orientation
Rdes = mathJet.rpyToRot(util.angPart(des_pose))
# compute orientation error
Re = Rdes.dot(b_R_w.transpose())
# express orientation error in angle-axis form
err = rotMatToRotVec(Re)
#the orient error is expressed in the base_frame so it should be rotated wo have the wrench in the world frame
w_err = b_R_w.transpose().dot(err)
# map des euler tates into des omega
Jomega = mathJet.Tomega(util.angPart(act_pose))
# Note we defined the angular part of the des twist as euler rates not as omega so we need to map them to an Euclidean space with Jomega
Wfbk[util.sp_crd["AX"]:util.sp_crd["AX"] +
3] = Kp_ang.dot(w_err) + Kd_ang.dot(
Jomega.dot((util.angPart(des_twist) - util.angPart(act_twist))))
#EXERCISE 3: Compute graviy wrench
Wg = np.zeros(6)
if (GravityComp):
mg = conf.robotMass * np.array([0, 0, conf.gravity])
Wg[util.sp_crd["LX"]:util.sp_crd["LX"] + 3] = mg
#in case you are closing the loop on base frame
if (not isCoMControlled):
Wg[util.sp_crd["AX"]:util.sp_crd["AX"] + 3] = np.cross(
W_base_to_com, mg)
# EXERCISE 4: Feed-forward wrench
Wffwd = np.zeros(6)
if (ffwdOn):
ffdLinear = conf.robotMass * util.linPart(des_acc)
# compute inertia in the WF w_I = R' * B_I * R
W_Inertia = np.dot(b_R_w.transpose(), np.dot(B_Inertia, b_R_w))
# compute w_des_omega_dot Jomega*des euler_rates_dot + Jomega_dot*des euler_rates
Jomega_dot = mathJet.Tomega_dot(util.angPart(des_pose),
util.angPart(des_twist))
w_des_omega_dot = Jomega.dot(util.angPart(des_acc)) + Jomega_dot.dot(
util.angPart(des_twist))
ffdAngular = W_Inertia.dot(w_des_omega_dot)
#ffdAngular = W_Inertia.dot(util.angPart(des_acc))
Wffwd = np.hstack([ffdLinear, ffdAngular])
return Wffwd, Wfbk, Wg
# 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