def computeRootYawAngleBetwwenConfigs(q0, q1):
quat0 = Quaternion(q0[6], q0[3], q0[4], q0[5])
quat1 = Quaternion(q1[6], q1[3], q1[4], q1[5])
v_angular = np.array(log3(quat0.matrix().T.dot(quat1.matrix())))
#print ("q_prev : ",q0)
#print ("q : ",q1)
#print ("v_angular = ",v_angular)
return v_angular[2]
def SE3FromConfig(q):
if isinstance(q, types.ListType):
q = np.matrix(q).T
placement = SE3.Identity()
placement.translation = q[0:3]
r = Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0])
placement.rotation = r.matrix()
return placement
def SE3FromConfig(q):
if isinstance(q, list):
q = np.array(q)
placement = SE3.Identity()
tr = np.array(q[0:3])
placement.translation = tr
r = Quaternion(q[6], q[3], q[4], q[5])
placement.rotation = r.matrix()
return placement
def SE3FromConfig(q):
"""
Convert a vector of size >=7 to a pinocchio.SE3 object.
Assume that the first 3 values of the vector are the translation part, followed by a quaternion(x,y,z,w)
:param q: a list or a numpy array of size >=7
:return: a SE3 object
"""
if isinstance(q, list):
q = np.array(q)
placement = SE3.Identity()
tr = np.array(q[0:3])
placement.translation = tr
r = Quaternion(q[6], q[3], q[4], q[5])
placement.rotation = r.matrix()
return placement
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 sampleRandomTranstionFromState(fullBody, s0, limbsInContact, movingLimb,
z):
it = 0
success = False
n = [0, 0, 1]
vz = np.matrix(n).T
while not success and it < 10000:
it += 1
# sample a random position for movingLimb and try to project s0 to this position
qr = fullBody.shootRandomConfig()
q1 = s0.q()[::]
q1[limb_ids[movingLimb][0]:limb_ids[movingLimb][1]] = qr[
limb_ids[movingLimb][0]:limb_ids[movingLimb][1]]
s1 = State(fullBody, q=q1, limbsIncontact=limbsInContact)
fullBody.setCurrentConfig(s1.q())
p = fullBody.getJointPosition(
fullBody.dict_limb_joint[movingLimb])[0:3]
p[0] += random.uniform(eff_x_range[0], eff_x_range[1])
p[1] += random.uniform(eff_y_range[0], eff_y_range[1])
p[2] = z
s1, success = StateHelper.addNewContact(s1, movingLimb, p, n)
# force root orientation : (align current z axis with vertical)
if success:
quat_1 = Quaternion(s1.q()[6], s1.q()[3], s1.q()[4], s1.q()[5])
v_1 = quat_1.matrix() * vz
align = Quaternion.FromTwoVectors(v_1, vz)
rot = align * quat_1
q_root = s1.q()[0:7]
q_root[3:7] = rot.coeffs().T.tolist()[0]
success = s1.projectToRoot(q_root)
# check if new state is in static equilibrium
if success:
# check stability
success = fullBody.isStateBalanced(s1.sId, 3)
# check if transition is feasible according to CROC
if success:
#success = fullBody.isReachableFromState(s0.sId,s1.sId) or (len(fullBody.isDynamicallyReachableFromState(s0.sId,s1.sId, numPointsPerPhases=0)) > 0)
success = fullBody.isReachableFromState(s0.sId, s1.sId)
return success, s1
def printFootPositionRelativeToOther(nbConfigs):
for i in range(0, nbConfigs):
if i > 0 and not i % IT_DISPLAY_PROGRESS:
print(int((i * 100) / nbConfigs), " % done")
q, succ, s, pos = genFlat()
if succ:
global success
success += 1
addCom = True
for j in range(0, len(effectors)):
# for j in range(1):
fullBody.setCurrentConfig(q)
otheridx = (j + 1) % 2
# print("otheridx", otheridx)
# print("q ", q[:3])
oeffectorName = effectors[otheridx]
# oqEffector = fullBody.getJointPosition(oeffectorName)
pos_other = fullBody.getJointPosition(oeffectorName)
# print("other pos 1", pos_other[:3])
effectorName = effectors[j]
# limbId = limbIds[j]
qEffector = fullBody.getJointPosition(effectorName)
# print("pos 1", qEffector[:3])
qtr = q[:]
qtr[:3] = [qtr[0] - pos_other[0], qtr[1] - pos_other[1], qtr[2] - pos_other[2]]
# for l in range(3):
# qtr[l] -= pos_other[l]
# qtr[i] -= qEffector[i]
fullBody.setCurrentConfig(qtr)
effectorName = effectors[j]
# limbId = limbIds[j]
qEffector = fullBody.getJointPosition(effectorName)
# print("pos 2", qEffector[3:])
# print("pos 2", qEffector[:3])
# print("other effectorName 1", oeffectorName)
# print("effectorName 1", effectorName)
pos_other = fullBody.getJointPosition(oeffectorName)
# check current joint pos is now zero
# print("other pos 2", pos_other[:3])
# v(qtr)
q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4], qEffector[5])
rot = q0.matrix() # compute rotation matrix world -> local
p = qEffector[0:3] # (0,0,0) coordinate expressed in effector fram
rm = np.zeros((4, 4))
for k in range(0, 3):
for l in range(0, 3):
rm[k, l] = rot[k, l]
for m in range(0, 3):
rm[m, 3] = qEffector[m]
rm[3, 3] = 1
invrm = np.linalg.inv(rm)
# print(invrm)
# p = invrm.dot([0,0,0,1])
p = invrm.dot([0, 0, 0., 1])
# print("p ", p)
# print(norm (array(posrf) - array(poslf) ))
if (j == 0 and p[1] > MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
points[j].append(p[:3])
elif (j == 1 and p[1] < -MIN_DIST_BETWEEN_FEET_Y and abs(p[0]) < MAX_DIST_BETWEEN_FEET_X):
points[j].append(p[:3])
else:
addCom = False
# print(points[j])
# now compute coms
fullBody.setCurrentConfig(q)
com = fullBody.getCenterOfMass()
for x in range(0, 3):
q[x] = -com[x]
for j in range(0, len(effectors)):
effectorName = effectors[j]
# limbId = limbIds[j]
qEffector = fullBody.getJointPosition(effectorName)
q0 = Quaternion(qEffector[6], qEffector[3], qEffector[4], qEffector[5])
rot = q0.matrix() # compute rotation matrix world -> local
p = qEffector[0:3] # (0,0,0) coordinate expressed in effector fram
rm = np.zeros((4, 4))
for k in range(0, 3):
for l in range(0, 3):
rm[k, l] = rot[k, l]
for m in range(0, 3):
rm[m, 3] = qEffector[m]
rm[3, 3] = 1
invrm = np.linalg.inv(rm)
p = invrm.dot([0, 0, 0, 1])
# add offset
rp = array(p[:3] - offsets[j]).tolist()
# print("p ", p)
# print("rp ")
if (rp[2] < MIN_HEIGHT_COM):
addCom = False
if addCom:
if j == 1:
if rp[1] < MARGIN_FEET_SIDE:
compoints[j].append(rp)
else:
if rp[1] > -MARGIN_FEET_SIDE:
compoints[j].append(rp)
# compoints[j].append(p[:3])
else:
global fails
fails += 1
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 SE3FromConfig(q):
placement = SE3.Identity()
placement.translation = q[0:3]
r = Quaternion(q[6, 0], q[3, 0], q[4, 0], q[5, 0])
placement.rotation = r.matrix()
return placement
