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)
A = np.hstack([A1, A2, A3, A4])
print A
return A
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))
#
class IterativeProjectionParameters:
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 setContactsPosBF(self, contactsBF):
self.contactsBF = contactsBF
def setContactsPosWF(self, contactsWF):
self.contactsWF = contactsWF
def setCoMPosWF(self, comWF):
self.comPositionWF = comWF
def setTorqueLims(self, torqueLims):
self.torque_limits = torqueLims
def setActiveContacts(self, activeContacts):
self.stanceFeet = activeContacts
#print self.stanceFeet
def setContactNormals(self, normals):
self.normals = normals
def setConstraintModes(self, constraintMode):
self.constraintMode = constraintMode
def setFrictionCoefficient(self, mu):
self.friction = mu
def setNumberOfFrictionConesEdges(self, ng):
self.numberOfGenerators = ng
def setTotalMass(self, mass):
self.robotMass = mass
def getContactsPosWF(self):
return self.contactsWF
def getContactsPosBF(self): # used only for IK inside constraints.py
return self.contactsBF
def getCoMPosWF(self):
return self.comPositionWF
def getCoMPosBF(self):
return self.comPositionBF
def getTorqueLims(self):
return self.torque_limits
def getStanceFeet(self):
return self.stanceFeet
def getNormals(self):
return self.normals
def getOrientation(self):
return self.roll, self.pitch, self.yaw
def getConstraintModes(self):
return self.constraintMode
def getFrictionCoefficient(self):
return self.friction
def getNumberOfFrictionConesEdges(self):
return self.numberOfGenerators
def getTotalMass(self):
return self.robotMass
def getStanceIndex(self, stanceLegs):
stanceIdx = []
# print 'stance', stanceLegs
for iter in range(0, 4):
if stanceLegs[iter] == 1:
# print 'new poly', stanceIndex, iter
stanceIdx = np.hstack([stanceIdx, iter])
return stanceIdx
def getCurrentFeetPos(self, received_data):
num_of_elements = np.size(received_data.data)
for j in range(0, num_of_elements):
if str(received_data.name[j]) == str("footPosDesLFx"):
self.footPosWLF[0] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesLFy"):
self.footPosWLF[1] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesLFz"):
self.footPosWLF[2] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesRFx"):
self.footPosWRF[0] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesRFy"):
self.footPosWRF[1] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesRFz"):
self.footPosWRF[2] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesLHx"):
self.footPosWLH[0] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesLHy"):
self.footPosWLH[1] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesLHz"):
self.footPosWLH[2] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesRHx"):
self.footPosWRH[0] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesRHy"):
self.footPosWRH[1] = received_data.data[j]
if str(received_data.name[j]) == str("footPosDesRHz"):
self.footPosWRH[2] = received_data.data[j]
self.contactsWF = np.array([
self.footPosWLF, self.footPosWRF, self.footPosWLH, self.footPosWRH
])
def getParamsFromRosDebugTopic(self, received_data):
num_of_elements = np.size(received_data.data)
# print 'number of elements: ', num_of_elements
for j in range(0, num_of_elements):
# print j, received_data.name[j], str(received_data.name[j]), str("footPosLFx")
if str(received_data.name[j]) == str("LF_HAAmaxVar"):
self.LF_tau_lim[0] = received_data.data[j]
if str(received_data.name[j]) == str("LF_HFEmaxVar"):
self.LF_tau_lim[1] = received_data.data[j]
if str(received_data.name[j]) == str("LF_KFEmaxVar"):
self.LF_tau_lim[2] = received_data.data[j]
if str(received_data.name[j]) == str("RF_HAAmaxVar"):
self.RF_tau_lim[0] = received_data.data[j]
if str(received_data.name[j]) == str("RF_HFEmaxVar"):
self.RF_tau_lim[1] = received_data.data[j]
if str(received_data.name[j]) == str("RF_KFEmaxVar"):
self.RF_tau_lim[2] = received_data.data[j]
if str(received_data.name[j]) == str("LH_HAAmaxVar"):
self.LH_tau_lim[0] = received_data.data[j]
if str(received_data.name[j]) == str("LH_HFEmaxVar"):
self.LH_tau_lim[1] = received_data.data[j]
if str(received_data.name[j]) == str("LH_KFEmaxVar"):
self.LH_tau_lim[2] = received_data.data[j]
if str(received_data.name[j]) == str("RH_HAAmaxVar"):
self.RH_tau_lim[0] = received_data.data[j]
if str(received_data.name[j]) == str("RH_HFEmaxVar"):
self.RH_tau_lim[1] = received_data.data[j]
if str(received_data.name[j]) == str("RH_KFEmaxVar"):
self.RH_tau_lim[2] = received_data.data[j]
self.torque_limits = np.array([
self.LF_tau_lim,
self.RF_tau_lim,
self.LH_tau_lim,
self.RH_tau_lim,
])
# the inputs are all in the WF this way we can compute generic regions for generic contact sets and generic com position
if str(received_data.name[j]) == str("contact_setWLFx"):
self.footPosWLF[0] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWLFy"):
self.footPosWLF[1] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWLFz"):
self.footPosWLF[2] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWRFx"):
self.footPosWRF[0] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWRFy"):
self.footPosWRF[1] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWRFz"):
self.footPosWRF[2] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWLHx"):
self.footPosWLH[0] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWLHy"):
self.footPosWLH[1] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWLHz"):
self.footPosWLH[2] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWRHx"):
self.footPosWRH[0] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWRHy"):
self.footPosWRH[1] = received_data.data[j]
if str(received_data.name[j]) == str("contact_setWRHz"):
self.footPosWRH[2] = received_data.data[j]
self.contactsWF = np.array([
self.footPosWLF, self.footPosWRF, self.footPosWLH,
self.footPosWRH
])
#print self.contactsWF
if str(received_data.name[j]) == str("actual_CoMX"):
self.comPositionWF[0] = received_data.data[j]
if str(received_data.name[j]) == str("actual_CoMY"):
self.comPositionWF[1] = received_data.data[j]
if str(received_data.name[j]) == str("actual_CoMZ"):
self.comPositionWF[2] = received_data.data[j]
if str(received_data.name[j]) == str("offCoMX"):
self.comPositionBF[0] = received_data.data[j]
if str(received_data.name[j]) == str("offCoMY"):
self.comPositionBF[1] = received_data.data[j]
if str(received_data.name[j]) == str("offCoMZ"):
self.comPositionBF[2] = received_data.data[j]
# external wrench
if str(received_data.name[j]) == str("extPerturbForceX"):
self.externalForceWF[0] = received_data.data[j]
if str(received_data.name[j]) == str("extPerturbForceY"):
self.externalForceWF[1] = received_data.data[j]
if str(received_data.name[j]) == str("extPerturbForceZ"):
self.externalForceWF[2] = received_data.data[j]
# print 'ext force ',self.externalForceWF
# print self.contactsWF
if str(received_data.name[j]) == str("LF_HAA_th"):
self.q[0] = received_data.data[j]
if str(received_data.name[j]) == str("LF_HFE_th"):
self.q[1] = received_data.data[j]
if str(received_data.name[j]) == str("LF_KFE_th"):
self.q[2] = received_data.data[j]
if str(received_data.name[j]) == str("RF_HAA_th"):
self.q[3] = received_data.data[j]
if str(received_data.name[j]) == str("RF_HFE_th"):
self.q[4] = received_data.data[j]
if str(received_data.name[j]) == str("RF_KFE_th"):
self.q[5] = received_data.data[j]
if str(received_data.name[j]) == str("LH_HAA_th"):
self.q[6] = received_data.data[j]
if str(received_data.name[j]) == str("LH_HFE_th"):
self.q[7] = received_data.data[j]
if str(received_data.name[j]) == str("LH_KFE_th"):
self.q[8] = received_data.data[j]
if str(received_data.name[j]) == str("RH_HAA_th"):
self.q[9] = received_data.data[j]
if str(received_data.name[j]) == str("RH_HFE_th"):
self.q[10] = received_data.data[j]
if str(received_data.name[j]) == str("RH_KFE_th"):
self.q[11] = received_data.data[j]
#they are in WF
if str(received_data.name[j]) == str("normalLFx"):
self.normals[0, 0] = received_data.data[j]
if str(received_data.name[j]) == str("normalLFy"):
self.normals[0, 1] = received_data.data[j]
if str(received_data.name[j]) == str("normalLFz"):
self.normals[0, 2] = received_data.data[j]
if str(received_data.name[j]) == str("normalRFx"):
self.normals[1, 0] = received_data.data[j]
if str(received_data.name[j]) == str("normalRFy"):
self.normals[1, 1] = received_data.data[j]
if str(received_data.name[j]) == str("normalRFz"):
self.normals[1, 2] = received_data.data[j]
if str(received_data.name[j]) == str("normalLHx"):
self.normals[2, 0] = received_data.data[j]
if str(received_data.name[j]) == str("normalLHy"):
self.normals[2, 1] = received_data.data[j]
if str(received_data.name[j]) == str("normalLHz"):
self.normals[2, 2] = received_data.data[j]
if str(received_data.name[j]) == str("normalRHx"):
self.normals[3, 0] = received_data.data[j]
if str(received_data.name[j]) == str("normalRHy"):
self.normals[3, 1] = received_data.data[j]
if str(received_data.name[j]) == str("normalRHz"):
self.normals[3, 2] = received_data.data[j]
#print 'normals',self.normals
if str(received_data.name[j]) == str("robotMass"):
self.robotMass = received_data.data[j]
if str(received_data.name[j]) == str("muEstimate"):
self.friction = received_data.data[j]
if str(received_data.name[j]) == str("roll"):
self.roll = received_data.data[j]
if str(received_data.name[j]) == str("pitch"):
self.pitch = received_data.data[j]
if str(received_data.name[j]) == str("yaw"):
self.yaw = received_data.data[j]
if str(received_data.name[j]) == str("actual_swing"):
self.actual_swing = int(received_data.data[j])
self.robotMass -= self.externalForceWF[2] / 9.81
def getFutureStanceFeetFlags(self, received_data):
num_of_elements = np.size(received_data.data)
for j in range(0, num_of_elements):
if str(received_data.name[j]) == str("future_stance_LF"):
# print 'future lf',received_data.data[j]
self.stanceFeet[0] = int(received_data.data[j])
if str(received_data.name[j]) == str("future_stance_RF"):
self.stanceFeet[1] = int(received_data.data[j])
if str(received_data.name[j]) == str("future_stance_LH"):
self.stanceFeet[2] = int(received_data.data[j])
if str(received_data.name[j]) == str("future_stance_RH"):
self.stanceFeet[3] = int(received_data.data[j])
print 'stance feet ', self.stanceFeet
self.numberOfContacts = np.sum(self.stanceFeet)
def getCurrentStanceFeetFlags(self, received_data):
num_of_elements = np.size(received_data.data)
for j in range(0, num_of_elements):
if str(received_data.name[j]) == str("state_machineLF"):
self.state_machineLF = int(received_data.data[j])
if str(received_data.name[j]) == str("state_machineRF"):
self.state_machineRF = int(received_data.data[j])
if str(received_data.name[j]) == str("state_machineLH"):
self.state_machineLH = int(received_data.data[j])
if str(received_data.name[j]) == str("state_machineRH"):
self.state_machineRH = int(received_data.data[j])
if self.state_machineLF < 4.0:
self.stanceFeet[0] = 1
else:
self.stanceFeet[0] = 0
if self.state_machineRF < 4.0:
self.stanceFeet[1] = 1
else:
self.stanceFeet[1] = 0
if self.state_machineLH < 4.0:
self.stanceFeet[2] = 1
else:
self.stanceFeet[2] = 0
if self.state_machineRH < 4.0:
self.stanceFeet[3] = 1
else:
self.stanceFeet[3] = 0
self.numberOfContacts = np.sum(self.stanceFeet)
set1 = PolygonTriple() polygon.clockwise_sort(polygon) hs = iterativeProjection.compute_halfspaces(polygon) #print hs nc = 3 contactsToStack = np.vstack((LF_foot,RF_foot,LH_foot,RH_foot)) contacts = contactsToStack[0:nc, :] axisZ= np.array([[0.0], [0.0], [1.0]]) math = Math() n1 = np.transpose(np.transpose(math.rpyToRot(1.5,1.5,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]) ''' initialize Y_inner and Y_outer ''' inner_vx, directions = iterativeProjection.initiliaze_inner_appriximation(constraint_mode, mass, nc, contacts, com, normals) outer_vx = iterativeProjection.initiliaze_outer_appriximation(inner_vx, directions) '''1) Compute the edges of Y inner''' '''2) Pick the edge cutting off the greatest fraction of Y outer ''' '''3) Find the point in Y furthest outside this edge '''
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