def performNavigationPlanning(self, goal, execute=True, draw_marker=False, steplength=0.1): goal = np.array(goal) if goal.ndim == 2: # assume a 4x4 transformation matrix angle = openravepy.axisAngleFromRotationMatrix(goal)[-1] goal = np.array([goal[0, -1], goal[1, -1], angle]) # find the boundaries of the environment envmin, envmax = utils.get_environment_limits(self.env, self.robot) with self.env: self.robot.SetAffineTranslationLimits(envmin, envmax) self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3]) self.robot.SetAffineRotationAxisMaxVels(np.ones(4)) self.robot.SetActiveDOFs( [], openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis, [0, 0, 1] ) # draw the marker if draw_marker: center = np.r_[goal[0:2], 0.2] xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0)) yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0)) h = self.env.drawlinelist( np.transpose( np.c_[ center - xaxis, center + xaxis, center - yaxis, center + yaxis, center + xaxis, center + 0.5 * xaxis + 0.5 * yaxis, center + xaxis, center + 0.5 * xaxis - 0.5 * yaxis, ] ), linewidth=5.0, colors=np.array((0, 1, 0)), ) openravepy.RaveLogInfo("Planning to goal " + str(goal)) res = self.basemanip.MoveActiveJoints( goal=goal, maxiter=3000, steplength=steplength, execute=execute, outputtrajobj=True ) if res is None: raise ValueError("Could not find a trajectory") if execute: openravepy.RaveLogInfo("Waiting for controller to finish") self.robot.WaitForController(0) self.robot.GetController().Reset() return res
def performNavigationPlanning(self, goal, execute=True, draw_marker=False, steplength=0.1): goal = np.array(goal) if goal.ndim == 2: #assume a 4x4 transformation matrix angle = openravepy.axisAngleFromRotationMatrix(goal)[-1] goal = np.array([goal[0, -1], goal[1, -1], angle]) # find the boundaries of the environment envmin, envmax = utils.get_environment_limits(self.env, self.robot) with self.env: self.robot.SetAffineTranslationLimits(envmin, envmax) self.robot.SetAffineTranslationMaxVels([0.3, 0.3, 0.3]) self.robot.SetAffineRotationAxisMaxVels(np.ones(4)) self.robot.SetActiveDOFs( [], openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis, [0, 0, 1]) # draw the marker if draw_marker: center = np.r_[goal[0:2], 0.2] xaxis = 0.5 * np.array((np.cos(goal[2]), np.sin(goal[2]), 0)) yaxis = 0.25 * np.array((-np.sin(goal[2]), np.cos(goal[2]), 0)) h = self.env.drawlinelist(np.transpose( np.c_[center - xaxis, center + xaxis, center - yaxis, center + yaxis, center + xaxis, center + 0.5 * xaxis + 0.5 * yaxis, center + xaxis, center + 0.5 * xaxis - 0.5 * yaxis]), linewidth=5.0, colors=np.array((0, 1, 0))) openravepy.RaveLogInfo("Planning to goal " + str(goal)) res = self.basemanip.MoveActiveJoints(goal=goal, maxiter=3000, steplength=steplength, execute=execute, outputtrajobj=True) if res is None: raise ValueError("Could not find a trajectory") if execute: openravepy.RaveLogInfo("Waiting for controller to finish") self.robot.WaitForController(0) self.robot.GetController().Reset() return res
def generate_random_pos(robot, obj_to_grasp = None): """Generate a random position for the robot within the boundaries of the world. """ T = robot.GetTransform() return T envmin, envmax = utils.get_environment_limits(robot.GetEnv(), robot) if obj_to_grasp is None: max_x = envmax[0] max_y = envmax[1] max_th = np.pi min_x = envmin[0] min_y = envmin[1] min_th = np.pi else: if type(obj_to_grasp) is openravepy.KinBody: obj_pos = obj_to_grasp.GetTransform()[:3,-1] else: obj_pos = obj_to_grasp max_x = min(obj_pos[0] + 1.2, envmax[0]) max_y = min(obj_pos[1] + 1.2, envmax[1]) min_x = max(obj_pos[0] - 1.2, envmin[0]) min_y = max(obj_pos[1] - 1.2, envmin[1]) #print "X ", (min_x, max_x), " Y: ", (min_y, max_y) x = np.random.uniform(min_x, max_x) y = np.random.uniform(min_y, max_y) z = T[2,3] if obj_to_grasp is None: th = np.random.uniform(min_th, max_th) else: robot_pos = T[:3,-1] facing_angle = np.arctan2(obj_pos[1] -y , obj_pos[0] - x) th = np.random.uniform(facing_angle - np.pi/4., facing_angle + np.pi/4.) #rotation T = openravepy.matrixFromAxisAngle([0,0,th]) #translation T[:, -1] = [x, y, z, 1] return T