def rootOrientationFromFeetPlacement(phase, phase_next):
#FIXME : extract only the yaw rotation
qr = Quaternion(phase.RF_patch.placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
ql = Quaternion(phase.LF_patch.placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
q_rot = qr.slerp(0.5, ql)
placement_init = SE3.Identity()
placement_init.rotation = q_rot.matrix()
if phase_next:
if not isContactActive(phase, cfg.Robot.rfoot) and isContactActive(
phase_next, cfg.Robot.rfoot):
qr = Quaternion(phase_next.RF_patch.placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
if not isContactActive(phase, cfg.Robot.lfoot) and isContactActive(
phase_next, cfg.Robot.lfoot):
ql = Quaternion(phase_next.LF_patch.placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
q_rot = qr.slerp(0.5, ql)
placement_end = SE3.Identity()
placement_end.rotation = q_rot.matrix()
return placement_init, placement_end
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
class TrajectorySE3LinearInterp(RefTrajectory):
def __init__(self, placement_init, placement_final, time_interval):
RefTrajectory.__init__(self, "TrajectorySE3LinearInterp")
self.placement_init = placement_init
self.placement_final = placement_final
self.t0 = time_interval[0]
self.t1 = time_interval[1]
self.length = self.t1 - self.t0
self.quat0 = Quaternion(self.placement_init.rotation)
self.quat1 = Quaternion(self.placement_final.rotation)
self.M = SE3.Identity()
self.v = Motion.Zero()
self.a = Motion.Zero()
# constant velocity and null acceleration :
self.v.linear = (placement_final.translation -
placement_final.translation) / self.length
self.v.angular = pin.log3(placement_final.rotation.T *
placement_final.rotation) / self.length
def __call__(self, t):
return self.compute_for_normalized_time(t - self.t0)
def compute_for_normalized_time(self, t):
if t < 0:
print "Trajectory called with negative time."
return self.compute_for_normalized_time(0)
elif t > self.length:
print "Trajectory called after final time."
return self.compute_for_normalized_time(self.t_total)
u = t / self.length
self.M = SE3.Identity()
self.M.translation = u * self.placement_final.translation + (
1. - u) * self.placement_init.translation
self.M.rotation = (self.quat0.slerp(u, self.quat1)).matrix()
return self.M, self.v, self.a
def compute_for_normalized_time(self, t):
if t < 0:
print "Trajectory called with negative time."
return self.compute_for_normalized_time(0)
elif t > self.t_total:
print "Trajectory called after final time."
return self.compute_for_normalized_time(self.t_total)
self.M = SE3.Identity()
self.v = Motion.Zero()
self.a = Motion.Zero()
self.M.translation = self.curves(t)
self.v.linear = self.curves.d(t)
self.a.linear = self.curves.dd(t)
#rotation :
if self.curves.isInFirstNonZero(t):
self.M.rotation = self.placement_init.rotation.copy()
elif self.curves.isInLastNonZero(t):
self.M.rotation = self.placement_end.rotation.copy()
else:
# make a slerp between self.effector_placement[id][0] and [1] :
quat0 = Quaternion(self.placement_init.rotation)
quat1 = Quaternion(self.placement_end.rotation)
t_rot = t - self.t_mid_begin
"""
print "t : ",t
print "t_mid_begin : ",self.t_mid_begin
print "t_rot : ",t_rot
print "t mid : ",self.t_mid
"""
assert t_rot >= 0, "Error in the time intervals of the polybezier"
assert t_rot <= (self.t_mid + 1e-6
), "Error in the time intervals of the polybezier"
u = t_rot / self.t_mid
# normalized time without the pre defined takeoff/landing phases
self.M.rotation = (quat0.slerp(u, quat1)).matrix()
# angular velocity :
dt = 0.001
u_dt = dt / self.t_mid
r_plus_dt = (quat0.slerp(u + u_dt, quat1)).matrix()
self.v.angular = pin.log3(self.M.rotation.T * r_plus_dt) / dt
r_plus2_dt = (quat0.slerp(u + (2. * u_dt), quat1)).matrix()
next_angular_velocity = pin.log3(r_plus_dt.T * r_plus2_dt) / dt
self.a.angular = (next_angular_velocity - self.v.angular) / dt
#r_plus_dt = (quat0.slerp(u+u_dt,quat1)).matrix()
#next_angular_vel = (pin.log3(self.M.rotation.T * r_plus_dt)/dt)
#self.a.angular = (next_angular_vel - self.v.angular)/dt
return self.M, self.v, self.a
def gen_state(s, pId, num_max_sample = 0, first = False, normal = lp.Z_AXIS, newContact = True ,projectCOM = True):
#~ pId = 6
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fullBody.lLegId
if moving == lp.RF:
movingID = fullBody.rLegId
print("# gen state for phase Id = ",pId)
if False and pId < len(pb["phaseData"])-1:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
quat1 = Quaternion(pb["phaseData"][pId+1]["rootOrientation"])
qrot = (quat0.slerp(0.5,quat1)).matrix()
else:
qrot = Quaternion(phase["rootOrientation"]) # rotation of the root, from the guide
#q_n = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T)
#rot = quatToConfig(qrot * q_n)
if not isclose(normal,Z_AXIS).all():
qrot = Quaternion().FromTwoVectors(np.matrix(Z_AXIS).T,np.matrix(normal).T)
# ignore guide orientation when normal is not z ...
#rot = quatToConfig(qrot)
pos = allfeetpos[pId];
pos[2] += 0.002
pose = pos.tolist()+quatToConfig(qrot)
print("Try to add contact for "+movingID+" pos = "+str(pose))
disp.moveSphere('c',v,pose)
if newContact:
sres, succ = tryCreateContactAround(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample,rotation = qrot)
#sres, succ = StateHelper.removeContact(s,movingID)
#assert succ
#succ = sres.projectToRoot(s.q()[0:3]+rot)
#sres, succ = StateHelper.addNewContact(s, movingID, pos.tolist(), normal.tolist(), num_max_sample= num_max_sample)
else:
sres, succ = StateHelper.cloneState(s)
if not succ:
print("Cannot project config q = ",sres.q())
print("To new contact position for "+movingID+" = "+str(pose)+" ; n = "+str(normal.tolist()))
raise RuntimeError("Cannot project feet to new contact position") # try something else ??
if projectCOM :
#sfeet, _ = StateHelper.cloneState(sres)
#print "config before projecting to com q1=",sres.q()
successCOM = projectCoMInSupportPolygon(sres)
if not successCOM:
# is it really an issue ?
print("Unable to project CoM in the center of the support polygone")
v(sres.q())
return sres
def rootOrientationFromFeetPlacement(Robot, phase_prev, phase, phase_next):
"""
Compute an initial and final root orientation for the ContactPhase
The initial orientation is a mean between both feet contact position in the current (or previous) phase
the final orientation is considering the new contact position of the moving feet
:param Robot: a Robot configuration class (eg. the class defined in talos_rbprm.talos.Robot)
:param phase_prev: the previous contact phase
:param phase: the current contact phase
:param phase_next: the next contact phase
:return: a list of SE3 objects: [initial placement, final placement]
"""
#FIXME : extract only the yaw rotation
qr = None
ql = None
patchR = None
patchL = None
if phase.isEffectorInContact(Robot.rfoot):
patchR = phase.contactPatch(Robot.rfoot)
elif phase_prev and phase_prev.isEffectorInContact(Robot.rfoot):
patchR = phase_prev.contactPatch(Robot.rfoot)
if patchR:
qr = Quaternion(patchR.placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
if phase.isEffectorInContact(Robot.lfoot):
patchL = phase.contactPatch(Robot.lfoot)
elif phase_prev and phase_prev.isEffectorInContact(Robot.lfoot):
patchL = phase_prev.contactPatch(Robot.lfoot)
if patchL:
ql = Quaternion(patchL.placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
if ql is not None and qr is not None:
q_rot = qr.slerp(0.5, ql)
elif qr is not None:
q_rot = qr
elif ql is not None:
q_rot = ql
else:
raise RuntimeError("In rootOrientationFromFeetPlacement, cannot deduce feet initial contacts positions.")
placement_init = SE3.Identity()
placement_init.rotation = q_rot.matrix()
# compute the final orientation :
if phase_next:
if not phase.isEffectorInContact(Robot.rfoot) and phase_next.isEffectorInContact(Robot.rfoot):
qr = Quaternion(phase_next.contactPatch(Robot.rfoot).placement.rotation)
qr.x = 0
qr.y = 0
qr.normalize()
if not phase.isEffectorInContact(Robot.lfoot) and phase_next.isEffectorInContact(Robot.lfoot):
ql = Quaternion(phase_next.contactPatch(Robot.lfoot).placement.rotation)
ql.x = 0
ql.y = 0
ql.normalize()
if ql is not None and qr is not None:
q_rot = qr.slerp(0.5, ql)
elif qr is not None:
q_rot = qr
elif ql is not None:
q_rot = ql
else:
raise RuntimeError("In rootOrientationFromFeetPlacement, cannot deduce feet initial contacts positions.")
placement_end = SE3.Identity()
placement_end.rotation = q_rot.matrix()
return placement_init, placement_end
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
