def set_random_configs(self): """ randomly sample initial and goal configuration : """ self.q_init[:3] = [0, 0, 0.465] self.q_init[3:7] = [0, 0, 0, 1] self.q_init[-6] = self.v_init random.seed() alpha = random.uniform(0., 2. * np.pi) #alpha = 4. print("Test on a circle, alpha = ", alpha) self.q_goal = self.q_init[::] self.q_goal[:3] = [ self.radius * np.sin(alpha), -self.radius * np.cos(alpha), 0.465 ] # set final orientation to be along the circle : vx = np.matrix([1, 0, 0]).T v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T quat = Quaternion.FromTwoVectors(vx, v_goal) self.q_goal[3:7] = quat.coeffs().tolist() # set final velocity to fix the orientation : self.q_goal[-6] = self.v_goal * np.sin(alpha) self.q_goal[-5] = -self.v_goal * np.cos(alpha) self.v(self.q_init) print("initial root position : ", self.q_init) print("final root position : ", self.q_goal) self.ps.setInitialConfig(self.q_init) self.ps.addGoalConfig(self.q_goal) self.alpha = alpha # write problem in files : with open(self.status_filename, "w") as f: f.write("q_init= " + str(self.q_init) + "\n") f.write("q_goal= " + str(self.q_goal) + "\n")
def set_transform(origin, a, b): from pinocchio import rpy, Quaternion length = np.linalg.norm(b - a) z = (b - a) / length R = Quaternion.FromTwoVectors(np.array([0, 0, 1]), z).matrix() origin.attrib['xyz'] = " ".join([str(v) for v in ((a + b) / 2)]) origin.attrib['rpy'] = " ".join([str(v) for v in rpy.matrixToRpy(R)])
def createPhaseFromRBPRMState(fb, stateId, t_init=-1): """ Create a multicontact_api ContactPhase object from an rbprm state :param fb: an instance of rbprm.FullBody :param stateId: the Id of the state :param t_init: the initial time of the contact phase :return: a multicontact_api.ContactPhase with the same contacts as the rbprm state """ q = fb.getConfigAtState(stateId) limbs_contact = fb.getAllLimbsInContact(stateId) cp = createPhaseFromConfig(fb, q, limbs_contact, t_init) if fb.cType == "_3_DOF": # update the contact normal from the data in fullbody for limbId in limbs_contact: eeName = fb.dict_limb_joint[limbId] [p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb( stateId, False, limbId) normal = np.array(n) print("New normal : ", normal) quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal) placement = cp.contactPatch(eeName).placement placement.rotation = quat.matrix() cp.contactPatch(eeName).placement = placement print("New placement : ", normal) return cp
def rotationFromNormal(n): """ return a rotation matrix corresponding to the given contact normal :param n: the normal of the surface (as a numpy array) :return: a rotation matrix """ z_up = np.array([0., 0., 1.]) q = Quaternion.FromTwoVectors(z_up, n) return q.matrix()
def contactPlacementFromRBPRMState(fb, stateId, eeName): # get limbName from effector name : fb.setCurrentConfig(fb.getConfigAtState(stateId)) placement = SE3FromConfig(fb.getJointPosition(eeName)) if fb.cType == "_3_DOF": limbId = list(fb.dict_limb_joint.keys())[list(fb.dict_limb_joint.values()).index(eeName)] [p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(stateId, False, limbId) normal = np.array(n) quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal) placement.rotation = quat.matrix() return placement
def createPhaseFromRBPRMState(fb, stateId, t_init = -1): q = fb.getConfigAtState(stateId) limbs_contact = fb.getAllLimbsInContact(stateId) cp = createPhaseFromConfig(fb, q, limbs_contact, t_init) if fb.cType == "_3_DOF": # update the contact normal from the data in fullbody for limbId in limbs_contact: eeName = fb.dict_limb_joint[limbId] [p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb(stateId, False, limbId) normal = np.array(n) print("New normal : ", normal) quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal) placement = cp.contactPatch(eeName).placement placement.rotation = quat.matrix() cp.contactPatch(eeName).placement = placement print("New placement : ", normal) return cp
def set_random_configs(self): """ Generate random init and goal position. these position will be on the flat part of the environment, with an orientation such that they can be connected by a straight line, and an angle between +- 25 degree from the x axis """ # select randomly the initial and final plateform, they cannot be the same # end plateform is always after the init plateform on the x axis init_plateform_id = random.randint(0, 3) end_plateform_id = random.randint(init_plateform_id + 1, 4) #if end_plateform_id >= init_plateform_id: # end_plateform_id+=1 # set x position from the plateform choosen : x_init = 0.16 + 0.925 * init_plateform_id x_goal = 0.16 + 0.925 * end_plateform_id # uniformly sample y position y_init = random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]) self.q_init[:3] = [x_init, y_init, self.Z_VALUE] # y_goal must be random inside Y_BOUNDS but such that the line between q_init and q_goal is between +- MAX_ANGLE radian from the x axis y_bound_goal = self.Y_BOUNDS[::] y_angle_max = math.sin(self.MAX_ANGLE) * abs(x_init - x_goal) + y_init y_angle_min = math.sin(-self.MAX_ANGLE) * abs(x_init - x_goal) + y_init y_bound_goal[0] = max(y_angle_min, y_bound_goal[0]) y_bound_goal[1] = min(y_angle_max, y_bound_goal[1]) y_goal = random.uniform(y_bound_goal[0], y_bound_goal[1]) # compute the orientation such that q_init face q_goal : # set final orientation to be along the circle : vx = np.matrix([1, 0, 0]).T v_init = np.matrix([x_goal - x_init, y_goal - y_init, 0]).T quat = Quaternion.FromTwoVectors(vx, v_init) self.q_init[3:7] = quat.coeffs().tolist() self.q_goal = self.q_init[::] self.q_goal[:2] = [x_goal, y_goal] print("q_init= " + str(self.q_init)) print("q_goal= " + str(self.q_goal)) # write problem in files : with open(self.status_filename, "w") as f: f.write("q_init= " + str(self.q_init) + "\n") f.write("q_goal= " + str(self.q_goal) + "\n")
def setContactPlacementFromRBPRMState(phase, fb, stateId, limbs=None): if limbs == None: limbs = fb.limbs_names for limbId in limbs: eeName = fb.dict_limb_joint[limbId] if fb.isLimbInContact(limbId, stateId): [p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb( stateId, False, limbId) placement = SE3.Identity() placement.translation = np.matrix(p).T if fb.cType == "_3_DOF": normal = np.matrix(n).T quat = Quaternion.FromTwoVectors( np.matrix(fb.dict_normal[eeName]).T, normal) else: q_r = fb.getJointPosition(eeName) quat = quatFromConfig(q_r) placement.rotation = quat.matrix() patch = contactPatchForEffector(phase, eeName) patch.placement = placement
def set_random_configs(self): """ randomly sample initial and goal configuration : """ # init position at the origin, facing x axis self.q_init[:3] = [0, 0, 1.] self.q_init[-6] = self.v_init # sample random position on a circle of radius random in [MIN_ROOT_DIST; MAX_ROOT_DIST] random.seed() radius = random.uniform(self.MIN_ROOT_DIST, self.MAX_ROOT_DIST) alpha = random.uniform(0., 2. * np.pi) print("Test on a circle, alpha = ", alpha) print("Radius = ", radius) self.q_goal = self.q_init[::] self.q_goal[:3] = [radius * np.sin(alpha), -radius * np.cos(alpha), 1.] if self.FINAL_ORIENTATION_RANDOM: alpha = random.uniform( 0., 2. * np.pi) # sample new random yaw value for the orientation v_goal = np.matrix( [radius * np.sin(alpha), -radius * np.cos(alpha), 0]).T else: v_goal = np.matrix([self.q_goal[0], self.q_goal[1], 0]).T # direction root_init -> root_goal # set final orientation to be along the circle : vx = np.matrix([1, 0, 0]).T quat = Quaternion.FromTwoVectors(vx, v_goal) self.q_goal[3:7] = quat.coeffs().tolist() # set final velocity to fix the orientation : self.q_goal[-6] = self.v_goal * np.sin(alpha) self.q_goal[-5] = -self.v_goal * np.cos(alpha) self.ps.setInitialConfig(self.q_init) self.ps.addGoalConfig(self.q_goal) print("q_init= " + str(self.q_init)) print("q_goal= " + str(self.q_goal)) # write problem in files : with open(self.status_filename, "w") as f: f.write("q_init= " + str(self.q_init) + "\n") f.write("q_goal= " + str(self.q_goal) + "\n")
def contactPlacementFromRBPRMState(fb, stateId, eeName): """ Compute the placement of eeName for the given stateId :param fb: an instance of rbprm.FullBody :param stateId: the Id of the state :param eeName: the name of the effector :return: the placement represented as a pinocchio.SE3 object """ # get limbName from effector name : fb.setCurrentConfig(fb.getConfigAtState(stateId)) placement = SE3FromConfig(fb.getJointPosition(eeName)) if fb.cType == "_3_DOF": limbId = list(fb.dict_limb_joint.keys())[list( fb.dict_limb_joint.values()).index(eeName)] [p, n] = fb.clientRbprm.rbprm.computeCenterOfContactAtStateForLimb( stateId, False, limbId) normal = np.array(n) quat = Quaternion.FromTwoVectors(np.array(fb.dict_normal[eeName]), normal) placement.rotation = quat.matrix() return placement
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 set_random_configs(self): """ randomly sample initial and goal configuration : """ random.seed() self.q_init[:3] = [ random.uniform(self.X_BOUNDS[0], self.X_BOUNDS[1]), random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]), self.Z_VALUE ] self.q_goal = self.q_init[::] for i in range(random.randint(0, 1000)): random.uniform(0., 1.) self.q_goal[:3] = [ random.uniform(self.X_BOUNDS[0], self.X_BOUNDS[1]), random.uniform(self.Y_BOUNDS[0], self.Y_BOUNDS[1]), self.Z_VALUE ] # compute the orientation such that q_init face q_goal : # set final orientation to be along the circle : vx = np.matrix([1, 0, 0]).T v_init = np.matrix([ self.q_goal[0] - self.q_init[0], self.q_goal[1] - self.q_init[1], 0 ]).T quat = Quaternion.FromTwoVectors(vx, v_init) self.q_init[3:7] = quat.coeffs().tolist() self.q_goal[3:7] = self.q_init[3:7] self.v(self.q_init) print("initial root position : ", self.q_init[:3]) print("final root position : ", self.q_goal[:3]) self.ps.setInitialConfig(self.q_init) self.ps.addGoalConfig(self.q_goal) # write problem in files : with open(self.status_filename, "w") as f: f.write("q_init= " + str(self.q_init) + "\n") f.write("q_goal= " + str(self.q_goal) + "\n")
q_init[3:7] = [0, 0, 0, 1] q_init[-6] = vInit # sample random position on a circle of radius 2m radius = 1. import random random.seed() #alpha = random.uniform(0.,2.*np.pi) alpha = random.uniform(0., 2. * np.pi) print "Test on a circle, alpha = ", alpha q_goal = q_init[::] q_goal[0:3] = [radius * np.sin(alpha), -radius * np.cos(alpha), 1.] # set final orientation to be along the circle : vx = np.matrix([1, 0, 0]).T v_goal = np.matrix([q_goal[0], q_goal[1], 0]).T quat = Quaternion.FromTwoVectors(vx, v_goal) q_goal[3:7] = quat.coeffs().T.tolist()[0] # set final velocity to fix the orientation : q_goal[-6] = vGoal * np.sin(alpha) q_goal[-5] = -vGoal * np.cos(alpha) v(q_goal) print "initial root position : ", q_init print "final root position : ", q_goal ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) # write problem in files : f = open(statusFilename, "w") f.write("q_init= " + str(q_init) + "\n") f.write("q_goal= " + str(q_goal) + "\n") f.close()
# y_goal must be random inside Y_BOUNDS but such that the line between q_init and q_goal is between +- MAX_ANGLE radian from the x axis y_bound_goal = Y_BOUNDS[::] y_angle_max = math.sin(MAX_ANGLE)*abs(x_init-x_goal) + y_init y_angle_min = math.sin(-MAX_ANGLE)*abs(x_init-x_goal) + y_init y_bound_goal[0] = max(y_angle_min,y_bound_goal[0]) y_bound_goal[1] = min(y_angle_max,y_bound_goal[1]) y_goal = random.uniform(y_bound_goal[0],y_bound_goal[1]) # compute the orientation such that q_init face q_goal : # set final orientation to be along the circle : vx = np.matrix([1,0,0]).T v_init = np.matrix([x_goal-x_init,y_goal-y_init,0]).T quat = Quaternion.FromTwoVectors(vx,v_init) q_init[3:7] = quat.coeffs().T.tolist()[0] q_goal=q_init[::] q_goal[0:2] = [x_goal,y_goal] print "initial root position : ",q_init print "final root position : ",q_goal ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) # write problem in files : f = open(statusFilename,"w") f.write("q_init= "+str(q_init)+"\n") f.write("q_goal= "+str(q_goal)+"\n") f.close()
def quatFromAlpha(alpha): vx = np.matrix([1, 0, 0]).T v = Vector3FromAlpha(alpha) return Quaternion.FromTwoVectors(vx, v)