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