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