def compute_orientation_for_feet_placement(fb, pb, pId, moving, RF, prev_contactPhase, use_interpolated_orientation):
"""
Compute the rotation of the feet from the base orientation.
:param fb: an instance of rbprm.Fullbody
:param pb: the SL1M problem dictionary, containing all the phaseData
:param pId: the Id of the current phase (SL1M index)
:param moving: the Id of the moving feet
:param RF: the Id of the right feet in the SL1M solver
:param prev_contactPhase: the multicontact_api.ContactPhase of the previous phase
:param use_interpolated_orientation: If False, the desired contact rotation is the current base orientation.
If True, the desired contact rotation is the interpolation between the current base orientation
and the one for the next phase
:return: the rotation matrix of the new contact placement
"""
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if pId < len(pb["phaseData"]) - 1:
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
else:
quat1 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if use_interpolated_orientation:
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(prev_contactPhase.contactPatch(fb.lfoot).placement.rotation)
else:
ql = rot
qr = Quaternion(prev_contactPhase.contactPatch(fb.rfoot).placement.rotation)
rpy = matrixToRpy((qr * (ql.inverse())).matrix()) # rotation from the left foot pose to the right one
if rpy[2] > 0: # yaw positive, feet are crossing
rot = quat0 # rotation of the root, from the guide
else:
rot = quat0 # rotation of the root, from the guide
return rot
def exportWaist(path, waist_t):
filename = path + "/WaistOrientation.dat"
with open(filename, 'w') as f:
for waist in waist_t.T:
quat = Quaternion(waist[0, 6], waist[0, 3], waist[0, 4], waist[0, 5])
#rot = matrixToRpy(quat.matrix()) # DEBUG
rot = matrixToRpy(SE3.Identity().rotation) # DEBUG
line = ""
for i in range(3):
line += str(rot[i, 0]) + " "
for i in range(6):
line += "0 "
f.write(line.rstrip(" ") + "\n")
print("Motion exported to : ", filename)
return
def computePoseFromSurface(surface):
points = surface
#normal = surface[1]
normal = [0,0,1]
center = np.zeros(3)
for pt in points:
center += np.array(pt)
center /= len(points)
center -= np.array(normal)*(WIDTH/2.)
# rotation in rpy :
q_rot = Quaternion()
n_m = np.matrix(normal).T
q_rot.setFromTwoVectors(Z_AXIS,n_m)
rpy = matrixToRpy(q_rot.matrix())
pose = center.tolist() + rpy.T.tolist()[0]
return pose
def pinocchio_2_sot(q):
# PINOCCHIO Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36
# SOT Free flyer 0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35
qSot = np.matlib.zeros((36, 1))
qSot[:3] = q[:3]
quatMat = Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0]).matrix()
qSot[3:6] = matrixToRpy(quatMat)
qSot[18:22] = q[7:11]
# chest-head
qSot[29:] = q[11:18]
# larm
qSot[22:29] = q[18:25]
# rarm
qSot[12:18] = q[25:31]
# lleg
qSot[6:12] = q[31:]
# rleg
return qSot.A.squeeze()
def exportFoot(path, name, ref):
assert ref.shape[0] == 12, " feet trajectory must be of size 12 (3 translation + rotation matrice)"
filename = path + "/" + name + ".dat"
with open(filename, 'w') as f:
for k in range(ref.shape[1]):
placement = SE3FromVec(ref[:, k])
line = ""
# 3D position :
for i in range(3):
line += str(placement.translation[i, 0]) + " "
# orientation :
rot = matrixToRpy(placement.rotation)
#rot = matrixToRpy(SE3.Identity().rotation) # DEBUG
for i in range(3):
line += str(rot[i, 0]) + " "
# TODO : velocity and acceleration :
for i in range(12):
line += "0 "
f.write(line.rstrip(" ") + "\n")
print("Motion exported to : ", filename)
return
def generateContactSequence():
RF, root_init, pb, coms, footpos, allfeetpos, res = runLPScript()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, False)
q_init = fb.referenceConfig[::] + [0] * 6
q_init[0:7] = root_init
#q_init[2] += fb.referenceConfig[2] - 0.98 # 0.98 is in the _path script
v(q_init)
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequenceHumanoid(0)
addPhaseFromConfig(fb, v, cs, q_init, [fb.rLegId, fb.lLegId])
# loop over all phases of pb and add them to the cs :
for pId in range(
2, len(pb["phaseData"])
): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contact_phases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += 0.005 # FIXME it shouldn't be required !!
# compute desired foot rotation :
if USE_ORIENTATION:
if pId < len(pb["phaseData"]) - 1:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(
prev_contactPhase.LF_patch.placement.rotation)
else:
ql = rot
qr = Quaternion(
prev_contactPhase.RF_patch.placement.rotation)
rpy = matrixToRpy((qr * (ql.inverse())).matrix(
)) # rotation from the left foot pose to the right one
if rpy[2, 0] > 0: # yaw positive, feet are crossing
rot = Quaternion(phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = Quaternion(phase["rootOrientation"]
) # rotation of the root, from the guide
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.matrix(pos).T
placement.rotation = rot.matrix()
moveEffectorToPlacement(fb,
v,
cs,
movingID,
placement,
initStateCenterSupportPolygon=True)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = fb.referenceConfig[::] + [0] * 6
p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
for i in range(3):
q_end[i] += p_end[i]
setFinalState(cs, q=q_end)
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v
def generateContactSequence():
#RF,root_init,pb, coms, footpos, allfeetpos, res = runLPScript()
RF, root_init, root_end, pb, coms, footpos, allfeetpos, res = runLPFromGuideScript(
)
multicontact_api.switchToNumpyArray()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, True)
q_init = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
q_init[0:7] = root_init
feet_height_init = allfeetpos[0][2]
print("feet height initial = ", feet_height_init)
q_init[2] = feet_height_init + cfg.IK_REFERENCE_CONFIG[2]
q_init[2] += EPS_Z
#q_init[2] = fb.referenceConfig[2] # 0.98 is in the _path script
if v:
v(q_init)
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequence(0)
addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId])
# loop over all phases of pb and add them to the cs :
for pId in range(
2, len(pb["phaseData"])
): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contactPhases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += EPS_Z # FIXME it shouldn't be required !!
# compute desired foot rotation :
if cfg.SL1M_USE_ORIENTATION:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if pId < len(pb["phaseData"]) - 1:
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
else:
quat1 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if cfg.SL1M_USE_INTERPOLATED_ORIENTATION:
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(
prev_contactPhase.contactPatch(
fb.lfoot).placement.rotation)
else:
ql = rot
qr = Quaternion(
prev_contactPhase.contactPatch(
fb.rfoot).placement.rotation)
rpy = matrixToRpy((qr * (ql.inverse())).matrix(
)) # rotation from the left foot pose to the right one
if rpy[2, 0] > 0: # yaw positive, feet are crossing
rot = quat0 # rotation of the root, from the guide
else:
rot = quat0 # rotation of the root, from the guide
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.array(pos).T
placement.rotation = rot.matrix()
cs.moveEffectorToPlacement(movingID, placement)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
#p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
#for i in range(3):
# q_end[i] += p_end[i]
q_end[0:7] = root_end
feet_height_end = allfeetpos[-1][2]
print("feet height final = ", feet_height_end)
q_end[2] = feet_height_end + cfg.IK_REFERENCE_CONFIG[2]
q_end[2] += EPS_Z
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
if cfg.DISPLAY_CS_STONES:
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v
