def perturbateContactNormal(fb, state_id, epsilon=1e-2): """ Add a small variation (+- epsilon) to the contact normals of the given state :param fb: :param state_id: :param epsilon: :return: the new state ID, -1 if fail """ state = State(fb, state_id) for name in state.getLimbsInContact(): p, n = state.getCenterOfContactForLimb(name) n[2] += uniform(-epsilon, epsilon) n = np.array(n) state, success = StateHelper.addNewContact(state, name, p, n.tolist()) if not success: return -1 return state.sId
# sphere = target from hpp.corbaserver.rbprm.tools.display_tools import * createSphere('target', v, size=0.05, color=v.color.red) v.client.gui.setVisibility('target', 'ON') moveSphere('target', v, [0, 0, 0.5]) # create contact : fullBody.setStartState(q_init, [fullBody.lLegId, fullBody.rLegId]) n = [0, 0, 1] s0 = State(fullBody, q=q_init, limbsIncontact=[fullBody.lLegId, fullBody.rLegId]) ### move left foot of 30cm in front pLLeg = s0.getCenterOfContactForLimb(fullBody.lLegId)[0] pLLeg[0] += 0.3 pLLeg[2] -= 0.001 # required because this delta is added in rbprm ... s1, success = StateHelper.addNewContact(s0, fullBody.lLegId, pLLeg, n, lockOtherJoints=False) assert success, "Unable to project contact position for left foot" ### move hand to the handle pHand = [0.1, 0.5, 0.75] createSphere('s', v) moveSphere('s', v, pHand) s2, success = StateHelper.addNewContact(s1,
def genStateWithOneStep(fullbody, limbs, num_samples=100, coplanar=True): q_0 = fullBody.getCurrentConfig() #~ fullBody.getSampleConfig() qs = [] states = [] assert (len(limbs) == 2 and "only implemented for 2 limbs in contact for now") it = 0 while len(states) < num_samples and it < 10000: i = len(states) if (coplanar): q0 = fullBody.generateGroundContact(limbs) else: q0 = _genbalance(fullBody, limbs) if len(q0) > 1: s0 = projectMidFeet(fullBody, q0, limbs) if s0 is not None: success = False iter = 0 # try to make a step while not success and iter < 100: if (coplanar): q2 = fullBody.generateGroundContact(limbs) else: q2 = _genbalance(fullBody, limbs) if len(q2) > 1: s2 = State(fullbody, q=q2, limbsIncontact=limbs) moving_limb = limbs[i % 2] [p, n] = s2.getCenterOfContactForLimb( moving_limb ) # get position of the moving limb in the next state s1, success = StateHelper.addNewContact( s0, moving_limb, p, n ) # try to move the limb for s0 to it's position in s2 if success and __loosely_z_aligned( limbs[0], s1.q()) and __loosely_z_aligned( limbs[1], s1.q()): fullBody.setCurrentConfig(s1.q()) com = np.array( fullBody.getJointPosition(rfoot)[0:3]) com += np.array( fullBody.getJointPosition(lfoot)[0:3]) com /= 2. com[2] = fullBody.getCenterOfMass()[2] successProj = s1.projectToCOM(com.tolist()) success = successProj and fullBody.isConfigValid( s1.q())[0] and fullBody.isConfigBalanced( s1.q(), limbs, 5) else: success = False iter += 1 if success: # recreate the states to assure the continuity of the index in fullBody : state0 = State(fullBody, q=s0.q(), limbsIncontact=limbs) states += [[s1, state0]] print "Step " + str(i) + " done." else: print "cannot make the step." else: print "cannot project between feet" else: print "unable to generate first configuration." it += 1 print "Done generating pairs of states, found : " + str( len(states)) + " pairs." return states