def continuousSolve(self, itrs=1000, auto=False, extra=[], show=True, quitonsolve=False): """ Continously solve in realtime, allowing a user to reposition the robot, to quickly get an intuitive idea of what works and doesn't. Note that TSR's based on object poses are not recalculated if an object is moved.""" report = _rave.CollisionReport() env = self.robot.GetEnv() pose = Pose(self.robot) for k in xrange(itrs): pose.send() with self.robot: self.run(auto, extra) colcheck = env.CheckCollision( self.robot, report=report) and self.robot.CheckSelfCollision( ) if self.bcollisions else False if show: self.goto() _time.sleep(.1) if self.solved and not colcheck and quitonsolve: break return self.solved()
def do_traj_ik_graph_search(pr2, lr, gripper_poses): manip = get_manipulator(pr2, lr) hmats = [juc.pose_to_hmat(pose) for pose in gripper_poses] def ikfunc(hmat): return traj_ik_graph_search.ik_for_link(hmat, manip, "%s_gripper_tool_frame"%lr, return_all_solns=True) pr2.update_rave() start_joints = pr2.robot.GetDOFValues(manip.GetArmJoints()) robot = manip.GetRobot() env = robot.GetEnv() report = rave.CollisionReport() link_info = [] def nodecost(joints): robot.SetDOFValues(joints, manip.GetArmJoints()) return 100*env.CheckCollision(robot) paths, costs, timesteps = traj_ik_graph_search.traj_cart2joint(hmats, ikfunc, start_joints=start_joints, nodecost=nodecost) i_best = np.argmin(costs) print "lowest cost of initial trajs:", costs[i_best] best_path = paths[i_best] return best_path
def move_out_of_collision(env, body, max_displacement=0.005): """ Moves an OpenRAVE body out of collision in the opposite direction to the penetration direction. @type env: orpy.Environment @param env: The OpenRAVE environment. @type body: orpy.KinBody @param body: The OpenRAVE body. @type max_displacement: float @param max_displacement: The maximum displacement we can apply to the body. """ if not env.CheckCollision(body): # Not in collision return True # Need to use pqp collision checker previous_cc = env.GetCollisionChecker() checker = orpy.RaveCreateCollisionChecker(env, 'pqp') checker.SetCollisionOptions(orpy.CollisionOptions.Distance | orpy.CollisionOptions.Contacts) env.SetCollisionChecker(checker) # Collision report report = orpy.CollisionReport() env.CheckCollision(body, report) # Restore previous collision checker env.SetCollisionChecker(previous_cc) # Get the direction we should push the object positions = [] normals = [] occurrences = [] for c in report.contacts: positions.append(c.pos) if len(normals) == 0: normals.append(c.norm) occurrences.append(1) continue found = False for i, normal in enumerate(normals): if np.allclose(c.norm, normal): occurrences[i] += 1 found = True break if not found: normals.append(c.norm) occurrences.append(1) push_direction = tr.unit_vector(normals[np.argmax(occurrences)]) # Get the distance we should push the object Tbody = body.GetTransform() Tnew = np.array(Tbody) push_distance = 0 while env.CheckCollision(body): push_distance += 0.001 Tnew[:3, 3] = Tbody[:3, 3] + push_distance * push_direction body.SetTransform(Tnew) if push_distance > max_displacement: print 'push_distance: {0}'.format(push_distance) body.SetTransform(Tbody) return False return True
def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args): """ Attempt to plan a straight line trajectory from the robot's current configuration to a desired end-effector pose. This happens by finding the closest IK solution to the robot's current configuration and attempts to snap there (using PlanToConfiguration) if possible. In the case of a redundant manipulator, no attempt is made to check other IK solutions. @param robot @param goal_pose desired end-effector pose @return traj """ from prpy.planning.exceptions import CollisionPlanningError from prpy.planning.exceptions import SelfCollisionPlanningError ikp = openravepy.IkParameterizationType ikfo = openravepy.IkFilterOptions # Find an IK solution. OpenRAVE tries to return a solution that is # close to the configuration of the arm, so we don't need to do any # custom IK ranking. manipulator = robot.GetActiveManipulator() ik_param = openravepy.IkParameterization(goal_pose, ikp.Transform6D) ik_solution = manipulator.FindIKSolution( ik_param, ikfo.CheckEnvCollisions, ikreturn=False, releasegil=True ) if ik_solution is None: # FindIKSolutions is slower than FindIKSolution, # so call this only to identify and raise error when # there is no solution ik_solutions = manipulator.FindIKSolutions( ik_param, ikfo.IgnoreSelfCollisions, ikreturn=False, releasegil=True) for q in ik_solutions: robot.SetActiveDOFValues(q) report = openravepy.CollisionReport() if self.env.CheckCollision(robot, report=report): raise CollisionPlanningError.FromReport(report) elif robot.CheckSelfCollision(report=report): raise SelfCollisionPlanningError.FromReport(report) raise PlanningError('There is no IK solution at the goal pose.') return self._Snap(robot, ik_solution, **kw_args)
def Lift(robot, obj, distance=0.05, manip=None, render=True, **kw_args): """ @param robot The robot performing the push grasp @param obj The object to lift @param distance The distance to lift the cup @param manip The manipulator to perform the grasp with (if None active manipulator is used) @param render Render tsr samples and push direction vectors during planning """ if manip is None: with robot.GetEnv(): manip = robot.GetActiveManipulator() # Check for collision and disable anything in collision creport = openravepy.CollisionReport() disabled_objects = [] # Resolve inconsistencies in grabbed objects if robot.CheckSelfCollision(): grabbed_objs = robot.GetGrabbed() for obj in grabbed_objs: robot.Release(obj) for obj in grabbed_objs: robot.Grab(obj) # Create list of any current collisions so those can be disabled while robot.GetEnv().CheckCollision(robot, creport): collision_obj = creport.plink2.GetParent() disabled_objects.append(collision_obj) collision_obj.Enable(False) for obj in disabled_objects: obj.Enable(True) # Perform the lift with prpy.rave.AllDisabled(robot.GetEnv(), disabled_objects): lift_direction = [0., 0., 1.] lift_distance = distance ee_in_world = manip.GetEndEffectorTransform() with prpy.viz.RenderVector(ee_in_world[:3, 3], lift_direction, distance, robot.GetEnv(), render=render): manip.PlanToEndEffectorOffset(direction=lift_direction, distance=lift_distance, execute=True, **kw_args)
def run(self, auto=False, extra=[]): if auto: self.activate(extra) response = self.problem.SendCommand(self.Serialize()) if len(response) > 0: collisions = _rave.CollisionReport() if self.robot.CheckSelfCollision(collisions): print "Self-collision between links {} and {}!".format( collisions.plink1, collisions.plink2) return False if self.robot.GetEnv().CheckCollision(self.robot, collisions): print "Environment collision between links {} and {}!".format( collisions.plink1, collisions.plink2) return False self.soln = [float(x) for x in response[:-1].split(' ')] return True
def get_collision_free_surface_pose(good_bodies, robot, obj, max_trials = 500, use_general_grasps = True): env = robot.GetEnv() robot_pose = robot.GetTransform() torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0] manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() grasping_poses = generate_manip_above_surface(obj) env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts) collision = env.CheckCollision(robot) sol, _ = check_reachable(good_bodies, env, obj, manip, grasping_poses) isreachable = sol is not None min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 with robot: while ((collision) or (not isreachable)) and (num_trial < max_trials): num_trial +=1 torso_angle = move_random_torso(robot, min_torso, max_torso) robot_pose = generate_random_pos(robot, obj) robot.SetTransform(robot_pose) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: grasping_poses = generate_manip_above_surface(obj) sol, _ = check_reachable(good_bodies, env, obj, manip, grasping_poses) isreachable = sol is not None else: continue if (sol is None) or collision: raise GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials) else: return (robot_pose, sol, torso_angle)
def get_real_contacts(self): collision_report = orpy.CollisionReport() real_contacts = [] # iterate over all fingertip links and determine the contacts for eel in self._robot.get_fingertip_links(): link = self._robot.GetLink(eel) self._orEnv.CheckCollision(self._obj, link, report=collision_report) # self._orEnv.CheckCollision(link, self._obj, report=collision_report) if len(collision_report.contacts) == 0: raise ValueError('[HFTSSampler::get_real_contacts] No contacts found') # TODO the normals reported by the collision check are wrong, so instead we use a nearest # TODO neighbor lookup. Should see what's wrong with OpenRAVE here... position = collision_report.contacts[0].pos normal = self._object_points[self._object_kd_tree.query(position), 3:][1] # normal = collision_report.contacts[0].norm real_contacts.append(np.concatenate((position, normal))) real_contacts = np.asarray(real_contacts) return real_contacts
def checkCollisionIgnoreHand(self, config): '''TODO''' self.robot.SetDOFValues(config, self.manip.GetArmIndices()) self.env.UpdatePublishedBodies() report = openravepy.CollisionReport() inCollision = self.env.CheckCollision(self.robot, report) if not inCollision: return False inCollision = False for linkPair in report.vLinkColliding: if not (linkPair[0].GetName() in self.handLinkNames or linkPair[1].GetName() in self.handLinkNames): inCollision = True break return inCollision
def plot_contacts(robot, scale=.1): env = robot.GetEnv() with env: # setup the collision checker to return contacts checker = env.GetCollisionChecker() checker.SetCollisionOptions(_rave.CollisionOptions.Contacts) # get first collision report = _rave.CollisionReport() env.CheckCollision(robot, report=report) #_rave.raveLogInfo('{} contacts'.format(len(report.contacts))) positions = _np.array([c.pos for c in report.contacts]) normals = _np.array([c.norm for c in report.contacts]) if len(positions): p_handles = env.plot3(positions, 5, [.7, .3, .3]) plist = _np.hstack((positions, positions + normals * scale)) n_handles = env.drawlinelist(_np.reshape(plist, (1, -1)), .5) handles = (p_handles, n_handles) else: handles = None return handles
def main(): """Loads an environment and generates random positions until the robot can reach an oject from a collision-free pose. """ env = openravepy.Environment() env.Load('data/pr2test1.env.xml') robot = env.GetRobots()[0] mug = env.GetKinBody('mug1') manip = robot.SetActiveManipulator('leftarm_torso') ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot, iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() num_contacts = 1 #just cheating isreachable = False while (num_contacts > 0) or (not isreachable): T = generate_random_pos(robot, mug) robot.SetTransform(T) env.GetCollisionChecker().SetCollisionOptions( openravepy.CollisionOptions.Contacts) # get first collision report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) num_contacts = len(report.contacts) openravepy.raveLogInfo("Number of contacts: " + str(num_contacts)) sol = check_reachable(robot, manip, mug) isreachable = sol is not None #time.sleep(1) robot.SetDOFValues(sol, manip.GetArmIndices()) openravepy.raveLogInfo("Done!!") env.SetViewer('qtcoin')
def get_torso_grasping_pose(good_bodies, robot, object_to_grasp, max_trials = 100, use_general_grasps = True, ): """Returns the torso height from where the robot can grasp an object. Parameters: robot: an OpenRave robot instance object_to_grasp: a KinBody that the robot should grasp max_trials: how many attempts before giving up use_general_grasps: f True, don't calculate actual grasp points, but use a pre-generated list. It is much faster if a grasping model has not been generated. Returns: (sol, torso_angle): , the active manipulator angles and the torso joint angle from where the robot can grasp an object. Raises GraspingPoseError if no valid solution is found. """ env = robot.GetEnv() robot_pose = robot.GetTransform() torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0] manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() grasping_poses = generate_grasping_pose(robot, object_to_grasp, use_general_grasps) openravepy.raveLogInfo("I've got %d grasping poses" % len(grasping_poses)) env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts) collision = env.CheckCollision(robot) sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses) isreachable = sol is not None min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 with robot: while ((collision) or (not isreachable)) and (num_trial < max_trials): num_trial +=1 torso_angle = move_random_torso(robot, min_torso, max_torso) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: grasping_poses = generate_grasping_pose(robot, object_to_grasp, use_general_grasps) sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses) isreachable = sol is not None else: continue if (sol is None) or collision: e = GraspingPoseError("No collision free grasping pose found within %d steps" % max_trials) raise e else: return (sol, torso_angle)
def get_collision_free_ik_pose(good_bodies, robot, object_to_grasp, ik_pose, max_trials = 100, only_reachable=False ): """Returns the position from where the robot can reach a position (in cartesian coordinates). The active manipulator is used. Parameters: robot: an OpenRave robot instance ik_pose: a 4x4 matrix with the desired 6D pose max_trials: how many attempts before giving up use_general_grasps: f True, don't calculate actual grasp points, but use a pre-generated list. It is much faster if a grasping model has not been generated. Returns: (robot_pose, sol, torso_angle): the robot position (as a transformation matrix), the active manipulator angles and the torso joint angle from where the robot can grasp an object. Raises GraspingPoseError if no valid solution is found. """ env = robot.GetEnv() robot_pose = robot.GetTransform() torso_angle = robot.GetJoint("torso_lift_joint").GetValues()[0] manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() env.GetCollisionChecker().SetCollisionOptions(openravepy.CollisionOptions.Contacts) collision = env.CheckCollision(robot) sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, [ik_pose], only_reachable) isreachable = sol is not None min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 xyz = ik_pose[:3, 3] with robot: while ((collision) or (not isreachable)) and (num_trial < max_trials): num_trial +=1 torso_angle = move_random_torso(robot, min_torso, max_torso) robot_pose = generate_random_pos(robot, xyz) robot.SetTransform(robot_pose) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: sol, _ = check_reachable(good_bodies, env, object_to_grasp, manip, [ik_pose], only_reachable) isreachable = sol is not None else: continue if (sol is None) or collision: e = GraspingPoseError("No collision free IK pose found within %d steps" % max_trials) raise e else: return (robot_pose, sol, torso_angle)
def get_occluding_objects(good_bodies, robot, object_to_grasp, max_trials = 0, just_one_attempt = False, return_pose = False, body_filter = None ): """Generates a list of all the objects that prevent the robot from reaching a target object. Several (up to max_trials) attempts are performed to grasp Parameters: robot: a openravepy.Robot instance object_to_grasp: a openravepy.KinBody instance representing the object to grasp Returns: a list of sets of objects """ if just_one_attempt != return_pose: raise ValueError("If asking for a return poes then set just_one_attempt to True") env = robot.GetEnv() robot_pose = robot.GetTransform() manip = robot.GetActiveManipulator() ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel( robot,iktype=openravepy.IkParameterization.Type.Transform6D) if not ikmodel.load(): ikmodel.autogenerate() min_torso, max_torso = utils.get_pr2_torso_limit(robot) num_trial = 0 collisions_list = [] with robot: while num_trial < max_trials: num_trial +=1 # sample a base pose torso_angle = generate_reaching_poses.move_random_torso(robot, min_torso, max_torso) robot_pose = generate_reaching_poses.generate_random_pos(robot, object_to_grasp) robot.SetTransform(robot_pose) report = openravepy.CollisionReport() collision = env.CheckCollision(robot, report=report) if not collision: openravepy.raveLogInfo("Got a position not in collision") #sample a gripper pose grasping_poses = generate_reaching_poses.generate_grasping_pose(robot, object_to_grasp, use_general_grasps = True, checkik=False) openravepy.raveLogInfo("Got %d grasping poses" % len(grasping_poses)) #check if gripper pose is reachable from base pose #use robot's base pose to transform precomputed #gripper poses into the robot's frame of reference sol, collisions = generate_reaching_poses.check_reachable(good_bodies, env, object_to_grasp, manip, grasping_poses, only_reachable = True) if sol is None: print "Finding collisions: trial {0} No sol from base pose to gripper pose".\ format(num_trial) goodCollisions = False if body_filter is not None: goodCollisions = filter(body_filter, collisions) print "good collisions: " + repr(goodCollisions) print "all collisions: " + repr(collisions) if sol is not None and goodCollisions: print "Sol from base pose to gripper pose found in trial {0}".\ format(num_trial) openravepy.raveLogInfo("Getting the list of collisions") with robot: #robot.SetDOFValues(sol, robot.GetActiveManipulator().GetArmIndices()); #collisions_list.append(utils.get_all_collisions(robot, env)) collisions_list.append(collisions) if just_one_attempt: if return_pose: return (robot_pose, sol, torso_angle, collisions_list) else: return collisions_list if num_trial == max_trials: print "Getting obj names: No gripper pose reachable from collision free base pose found", print "after {0} trials".format(num_trial) if return_pose: return (robot_pose, None, torso_angle, collisions_list) else: return collisions_list
def showPossibleBasePoses(self, Tgrasp, N=1, Timeout=5): """visualizes possible base poses for a grasp specified by Tgrasp and gripper_angle :param Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame. equals manip.GetTransform() in the goal state :param gripper_angle: float, the gripper angle :param N: int, the number of sample poses we want to get """ # find the robot base distribution for the grasp specified by Tgrasp # Input for computeBaseDistribution(): # Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame # equals manip.GetTransform() in the goal state # Output for computeBaseDistribution(): # densityfn: gaussian kernel density function taking poses of openrave quaternion type, returns probabilities # samplerfn: gaussian kernel sampler function taking number of sample and weight, returns robot base poses and joint states # bounds: 2x3 array, bounds of samples, [[min rotation, min x, min y],[max rotation, max x, max y]] self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel( robot=self.robot) print 'loading irmodel' if not self.irmodel.load(): self.irmodel.autogenerate() self.irmodel.load() densityfn, samplerfn, bounds = self.irmodel.computeBaseDistribution( Tgrasp) if densityfn == None: print 'the specified grasp is not reachable!' return # Code fragmenmanip = self.robot.SetActiveManipulator('left_wam')t from `examples.mobilemanipulation` # initialize sampling parameters goals = [] numfailures = 0 starttime = time.time() timeout = Timeout self.manip = self.robot.SetActiveManipulator('left_wam') # self.manip = self.robot.GetActiveManipulator() self.env = self.robot.GetEnv() with self.robot: while len(goals) < N: if time.time() - starttime > timeout: break poses, jointstate = samplerfn(N - len(goals)) for pose in poses: self.robot.SetTransform(pose) self.robot.SetDOFValues(*jointstate) # Check if base is in collision if not self.manip.CheckIndependentCollision( openravepy.CollisionReport()): q = self.manip.FindIKSolution( Tgrasp, filteroptions=openravepy.IkFilterOptions. CheckEnvCollisions) if q is not None: pose = self.robot.GetTransform() xy_pose = [pose[0][3], pose[1][3]] goals.append((Tgrasp, xy_pose, q, pose)) elif self.manip.FindIKSolution(Tgrasp, 0) is None: numfailures += 1 print "base pose is in collision" return goals
def GetBasePoseForObjectGrasp(self, obj): # Load grasp database self.gmodel = openravepy.databases.grasping.GraspingModel( self.robot, obj) if not self.gmodel.load(): self.gmodel.autogenerate() base_pose = None grasp_config = None ################################################################### # TODO: Here you will fill in the function to compute # a base pose and associated grasp config for the # grasping the bottle ################################################################### #get the ordered valid grasp from homework1 print "robot start transformation -----------------" print self.robot.GetTransform() self.graspindices = self.gmodel.graspindices self.grasps = self.gmodel.grasps self.order_grasps() # get the grasp transform Tgrasp = self.gmodel.getGlobalGraspTransform(self.grasps_ordered[10], collisionfree=True) # load inverserechability database irmodel = openravepy.databases.inversereachability.InverseReachabilityModel( robot=self.robot) starttime = time.time() print 'loading irmodel' if not irmodel.load(): irmodel.autogenerate() loaded = irmodel.load() print "irmodel loaded? {}".format(loaded) densityfn, samplerfn, bounds = irmodel.computeBaseDistribution(Tgrasp) #find the valid pose and joint states # initialize sampling parameters goals = [] numfailures = 0 N = 3 with self.robot.GetEnv(): while len(goals) < N: poses, jointstate = samplerfn(N - len(goals)) for pose in poses: self.robot.SetTransform(pose) self.robot.SetDOFValues(*jointstate) # validate that base is not in collision if not self.manip.CheckIndependentCollision( openravepy.CollisionReport()): q = self.manip.FindIKSolution( Tgrasp, filteroptions=IkFilterOptions.CheckEnvCollisions) if q is not None: values = self.robot.GetDOFValues() values[self.manip.GetArmIndices()] = q goals.append((Tgrasp, pose, values)) elif self.manip.FindIKSolution(Tgrasp, 0) is None: numfailures += 1 # To do still #base_pose = goals[0][1] #grasp_config = goals[0][2] for i, goal in enumerate(goals): grasp_with_pose, pose, values = goal self.robot.SetTransform(pose) self.robot.SetJointValues(values) trans_pose = self.robot.GetTransform() angle_pose = openravepy.axisAngleFromRotationMatrix(trans_pose) pose = [trans_pose[0, 3], trans_pose[1, 3], angle_pose[2]] base_pose = numpy.array(pose) grasp_config = q #import IPython #IPython.embed() print "grasping result" print base_pose print grasp_config return base_pose, grasp_config
def handle_select_base(self, req): #, task): #choose_task(req.task) print 'I have received inputs!' #print 'My given inputs were: \n' #print 'goal is: \n',req.goal #print 'head is: \n', req.head # The head location is received as a posestamped message and is converted and used as the head location. pos_temp = [ req.head.pose.position.x, req.head.pose.position.y, req.head.pose.position.z ] ori_temp = [ req.head.pose.orientation.x, req.head.pose.orientation.y, req.head.pose.orientation.z, req.head.pose.orientation.w ] head = createBMatrix(pos_temp, ori_temp) #print 'head from input: \n', head # This lets the service use TF to get the head location instead of requiring it as an input. #(trans,rot) = self.listener.lookupTransform('/base_link', '/head_frame', rospy.Time(0)) #pos_temp = trans #ori_temp = rot #head = createBMatrix(pos_temp,ori_temp) #print 'head from tf: \n',head # The goal location is received as a posestamped message and is converted and used as the goal location. pos_temp = [ req.goal.pose.position.x, req.goal.pose.position.y, req.goal.pose.position.z ] ori_temp = [ req.goal.pose.orientation.x, req.goal.pose.orientation.y, req.goal.pose.orientation.z, req.goal.pose.orientation.w ] goal = createBMatrix(pos_temp, ori_temp) #print 'goal: \n',goal print 'I will move to be able to reach the goal.' # Sets the wheelchair location based on the location of the head using a few homogeneous transforms. pr2_B_wc = np.matrix([[head[0, 0], head[0, 1], 0., head[0, 3]], [head[1, 0], head[1, 1], 0., head[1, 3]], [0., 0., 1., 0.], [0., 0., 0., 1]]) # Transform from the coordinate frame of the wc model in the back right bottom corner, to the head location corner_B_head = np.matrix([ [m.cos(0.), -m.sin(0.), 0., .442603], [m.sin(0.), m.cos(0.), 0., .384275], #0.34 [0., 0., 1., 0.], [0., 0., 0., 1.] ]) wheelchair_location = pr2_B_wc * corner_B_head.I self.wheelchair.SetTransform(np.array(wheelchair_location)) # Published the wheelchair location to create a marker in rviz for visualization to compare where the service believes the wheelchair is to # where the person is (seen via kinect). pos_goal = wheelchair_location[0:3, 3] ori_goal = tr.matrix_to_quaternion(wheelchair_location[0:3, 0:3]) self.publish_wc_marker(pos_goal, ori_goal) #Setup for inside loop angle = m.pi # Transforms from end of wrist of PR2 to the end of its gripper. Openrave has a weird coordinate system for the gripper so I use a transform to correct. goal_B_gripper = np.matrix([[0, 0, 1., .1], [0, 1, 0., 0.], [-1., 0., 0., 0.], [0., 0., 0., 1.]]) #pr2_B_goal = head * head_B_goal pr2_B_goal = goal * goal_B_gripper angle_base = m.pi / 2 #print 'The goal gripper pose is: \n' , np.array(pr2_B_goal) # This is to publish WC position w.r.t. PR2 after the PR2 reaches goal location. # Only necessary for testing in simulation to set the wheelchair in reach of PR2. #goalpr2_B_wc = wc_B_goalpr2.I #print 'pr2_B_wc is: \n',goalpr2_B_wc #pos_goal = goalpr2_B_wc[:3,3] #ori_goal = tr.matrix_to_quaternion(goalpr2_B_wc[0:3,0:3]) #psm_wc = PoseStamped() #psm_wc.header.frame_id = '/odom_combined' #psm_wc.pose.position.x=pos_goal[0] #psm_wc.pose.position.y=pos_goal[1] #psm_wc.pose.position.z=pos_goal[2] #psm_wc.pose.orientation.x=ori_goal[0] #psm_wc.pose.orientation.y=ori_goal[1] #psm_wc.pose.orientation.z=ori_goal[2] #psm_wc.pose.orientation.w=ori_goal[3] #self.wc_position.publish(psm_wc) # Find a base location by testing various base locations online for IK solution for i in [ 0., .05, .1, .15, .2, .25, .3, .35, .4, .45, .5, -.05, -.1, -.15, -.2, -.25, -.3 ]: #[.1]:#[0.,.1,.3,.5,.8,1,-.1,-.2,-.3]: for j in [ 0., .03, .05, .08, -.03, -.05, -.08, -.1, -.12, -.2, -.3 ]: #[.2]:#[0.,.1,.3,.5,.8,-.1,-.2,-.3]: for k in [ 0., m.pi / 4, m.pi / 2, -m.pi / 4, -m.pi / 2, m.pi, 3 * m.pi / 2 ]: #goal_pose = req.goal # transform from head frame in wheelchair to desired base goal wc_B_goalpr2 = np.matrix([[ m.cos(angle_base + k), -m.sin(angle_base + k), 0., .4 + i ], [ m.sin(angle_base + k), m.cos(angle_base + k), 0., -.9 + j ], [0., 0., 1, 0.], [0., 0., 0., 1]]) base_position = pr2_B_wc * wc_B_goalpr2 #print 'base position: \n',base_position self.robot.SetTransform(np.array(base_position)) #res = self.manipprob.MoveToHandPosition(matrices=[np.array(pr2_B_goal)],seedik=10) # call motion planner with goal joint angles #self.robot.WaitForController(0) # wait #print 'res: \n',res v = self.robot.GetActiveDOFValues() for name in self.joint_names: v[self.robot.GetJoint(name).GetDOFIndex( )] = self.joint_angles[self.joint_names.index(name)] self.robot.SetActiveDOFValues(v) with self.env: #print 'checking goal base location: \n' , np.array(base_position) if not self.manip.CheckIndependentCollision( op.CollisionReport()): sol = self.manip.FindIKSolution( np.array(pr2_B_goal), op.IkFilterOptions.CheckEnvCollisions) if sol is not None: now = rospy.Time.now() + rospy.Duration(1.0) self.listener.waitForTransform( '/odom_combined', '/base_link', now, rospy.Duration(10)) (trans, rot) = self.listener.lookupTransform( '/odom_combined', '/base_link', now) odom_goal = createBMatrix(trans, rot) * base_position pos_goal = odom_goal[:3, 3] ori_goal = tr.matrix_to_quaternion( odom_goal[0:3, 0:3]) #print 'Got an iksolution! \n', sol psm = PoseStamped() psm.header.frame_id = '/odom_combined' psm.pose.position.x = pos_goal[0] psm.pose.position.y = pos_goal[1] psm.pose.position.z = pos_goal[2] psm.pose.orientation.x = ori_goal[0] psm.pose.orientation.y = ori_goal[1] psm.pose.orientation.z = ori_goal[2] psm.pose.orientation.w = ori_goal[3] # This is to publish WC position w.r.t. PR2 after the PR2 reaches goal location. # Only necessary for testing in simulation to set the wheelchair in reach of PR2. #goalpr2_B_wc = wc_B_goalpr2.I #print 'pr2_B_wc is: \n',goalpr2_B_wc #pos_goal = goalpr2_B_wc[:3,3] #ori_goal = tr.matrix_to_quaternion(goalpr2_B_wc[0:3,0:3]) #psm_wc = PoseStamped() #psm_wc.header.frame_id = '/odom_combined' #psm_wc.pose.position.x=pos_goal[0] #psm_wc.pose.position.y=pos_goal[1] #psm_wc.pose.position.z=pos_goal[2] #psm_wc.pose.orientation.x=ori_goal[0] #psm_wc.pose.orientation.y=ori_goal[1] #psm_wc.pose.orientation.z=ori_goal[2] #psm_wc.pose.orientation.w=ori_goal[3] #self.wc_position.publish(psm_wc) print 'I found a goal location! It is at B transform: \n', base_position print 'The quaternion to the goal location is: \n', psm return psm #self.robot.SetDOFValues(sol,self.manip.GetArmIndices()) # set the current solution #Tee = self.manip.GetEndEffectorTransform() #self.env.UpdatePublishedBodies() # allow viewer to update new robot # traj = None # try: # #res = self.manipprob.MoveToHandPosition(matrices=[np.array(pr2_B_goal)],seedik=10) # call motion planner with goal joint angles # traj = self.manipprob.MoveManipulator(goal=sol, outputtrajobj=True) # print 'Got a trajectory! \n'#,traj # except: # #print 'traj = \n',traj # traj = None # print 'traj failed \n' # pass # #traj =1 #This gets rid of traj # if traj is not None: else: print 'I found a bad goal location. Trying again!' #rospy.sleep(.1) print 'I found nothing! My given inputs were: \n', req.goal, req.head print 'I found a goal location! It is at B transform: \n', base_location print 'The quaternion to the goal location is: \n', psm return psm
def GetBasePoseForObjectGrasp(self, obj): # Load grasp database graspModel = GraspModel(self.robot, obj) graspModel.order_grasps() for i in range(6): print 'Showing grasp ', i graspModel.show_grasp(graspModel.grasps_ordered[i], delay=self.delay) print "using grasp 0 to generate Transform " Tgrasp = graspModel.gmodel.getGlobalGraspTransform(graspModel.grasps_ordered[i], collisionfree=True) # get the grasp transform if Tgrasp != None: break base_pose = None grasp_config = None ################################################################### # TODO: Here you will fill in the function to compute # a base pose and associated grasp config for the # grasping the bottle ################################################################### self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel(self.robot) loaded = self.irmodel.load() if loaded: densityfn,samplerfn,bounds = self.irmodel.computeBaseDistribution(Tgrasp) # densityfn: gaussian kernel density function taking poses of openrave quaternion type, returns probabilities # samplerfn: gaussian kernel sampler function taking number of sample and weight, returns robot base poses and joint states # bounds: 2x3 array, bounds of samples, [[min rotation, min x, min y],[max rotation, max x, max y]] goals = [] numfailures = 0 starttime = time.time() timeout = 5000 N = 5 with self.robot: while len(goals) < N: if time.time()-starttime > timeout: break poses,jointstate = samplerfn(N-len(goals)) for pose in poses: self.robot.SetTransform(pose) self.robot.SetDOFValues(*jointstate) # validate that base is not in collision if not self.irmodel.manip.CheckIndependentCollision(openravepy.CollisionReport()): q = self.irmodel.manip.FindIKSolution(Tgrasp,filteroptions=openravepy.IkFilterOptions.CheckEnvCollisions) if q is not None: values = self.robot.GetDOFValues() values[self.irmodel.manip.GetArmIndices()] = q goals.append((Tgrasp,pose,values)) elif self.irmodel.manip.FindIKSolution(Tgrasp,0) is None: numfailures += 1 self.armmanip = self.robot.GetManipulator('right_wam') self.robot.SetActiveManipulator('right_wam') print 'showing %d results'%N for ind,goal in enumerate(goals): raw_input('press ENTER to show goal %d'%ind) Tgrasp,base_pose, all_config = goal self.robot.SetTransform(base_pose) self.robot.SetDOFValues(all_config) # IPython.embed() idx = raw_input("Choose from goal index 0, 1, 2, 3, 4: ") goal_chosen = goals[int(idx)] Tgrasp, pose, all_config = goal_chosen matrix = openravepy.matrixFromPose(pose) aa = openravepy.axisAngleFromRotationMatrix(matrix) base_pose = [matrix[0,3], matrix[1,3], aa[2]] # x, y , theta return base_pose, all_config[11:18] # base_pose [x, y] grasp_config [7 dof values]
def GetBasePoseForObjectGrasp(self, obj): # Load grasp database self.gmodel = openravepy.databases.grasping.GraspingModel( self.robot, obj) if not self.gmodel.load(): self.gmodel.autogenerate() base_pose = None grasp_config = None ################################################################### # TODO: Here you will fill in the function to compute # a base pose and associated grasp config for the # grasping the bottle ################################################################### self.graspindices = self.gmodel.graspindices self.grasps = self.gmodel.grasps grasp_config = self.order_grasps_noisy() # Store the initial robot pose init_pose = self.robot.GetTransform() # Format grasp transform in global frame into 4x4 numpy.array Tgrasp = self.gmodel.getGlobalGraspTransform(grasp_config, collisionfree=True) print 'Gripper transform', Tgrasp # load inverserechability database self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel( robot=self.robot) print 'loading irmodel' if not self.irmodel.load(): class IrmodelOption: self.irmodel.autogenerate() self.irmodel.load() densityfn, samplerfn, bounds = self.irmodel.computeBaseDistribution( Tgrasp) # initialize sampling parameters with self.robot: while base_pose is None: pose, jointstate = samplerfn(1) self.robot.SetTransform(pose[0]) pose_config = self.base_planner.planning_env.herb.GetCurrentConfiguration( ) pid = self.base_planner.planning_env.discrete_env.ConfigurationToNodeId( pose_config) pose_config = self.base_planner.planning_env.discrete_env.NodeIdToConfiguration( pid) self.base_planner.planning_env.herb.SetCurrentConfiguration( pose_config) # print self.robot.GetTransform() self.robot.SetDOFValues(*jointstate) # validate that base is not in collision if not self.robot.ikmodel.manip.CheckIndependentCollision( openravepy.CollisionReport()): q = self.robot.ikmodel.manip.FindIKSolution( Tgrasp, filteroptions=openravepy.IkFilterOptions. CheckEnvCollisions) if q is not None: print "Found valid pose" base_pose = self.base_planner.planning_env.herb.GetCurrentConfiguration( ) print 'base pose', base_pose grasp_config = q print "grasp_config:" print grasp_config # Once after finding out the final base pose, change the robot back to its initial pose self.robot.SetTransform(init_pose) return base_pose, grasp_config
def GetBasePoseForObjectGrasp(self, obj): # Load grasp database self.gmodel = openravepy.databases.grasping.GraspingModel( self.robot, obj) if not self.gmodel.load(): self.gmodel.autogenerate() #load model of base self.irmodel = openravepy.databases.inversereachability.InverseReachabilityModel( self.robot) if not self.irmodel.load(): self.irmodel.autogenerate() print "models are loaded" base_pose = None grasp_config = None ################################################################### # TODO: Here you will fill in the function to compute # a base pose and associated grasp config for the # grasping the bottle ################################################################### print "start calculate grasp and base!!!!!!" self.graspindices = self.gmodel.graspindices self.grasps = self.gmodel.grasps self.order_grasps() Tgrasp = self.gmodel.getGlobalGraspTransform(self.grasps_ordered[10], collisionfree=True) densityfn, samplerfn, bounds = self.irmodel.computeBaseDistribution( Tgrasp) # pickle.dump([ Tgrasp,densityfn,samplerfn,bounds ], open( "save.p", "wb" ) ) # Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame # equals manip.GetTransform() in the goal state print "got irmodel {}".format(Tgrasp) goals = [] numfailures = 0 starttime = time.time() timeout = 10000 N = 5 print "start time {}".format(starttime) with self.robot: while len(goals) < N: if time.time() - starttime > timeout: break poses, jointstate = samplerfn(N - len(goals)) for pose in poses: self.robot.SetTransform(pose) self.robot.SetDOFValues(*jointstate) # validate that base is not in collision if not self.manip.CheckIndependentCollision( openravepy.CollisionReport()): q = self.manip.FindIKSolution( Tgrasp, filteroptions=openravepy.IkFilterOptions. CheckEnvCollisions) if q is not None: values = self.robot.GetDOFValues() values[self.manip.GetArmIndices()] = q goals.append((Tgrasp, pose, values)) print "q: {}".format(q) elif self.manip.FindIKSolution(Tgrasp, 0) is None: numfailures += 1 print 'showing {} results'.format(N) for ind, goal in enumerate(goals): raw_input('press ENTER to show goal %d' % ind) Tgrasp, pose, values = goal self.robot.SetTransform(pose) self.robot.SetDOFValues(values) base_pose = np.array( self.base_planner.planning_env.herb.GetCurrentConfiguration()) grasp_config = np.array( self.arm_planner.planning_env.herb.GetCurrentConfiguration()) print "found goals!" IPython.embed() theta = -numpy.pi / 4. start_pose = numpy.array( [[numpy.cos(theta), -numpy.sin(theta), 0, -1.25], [numpy.sin(theta), numpy.cos(theta), 0, 0.82], [0., 0., 1, 0.], [0., 0., 0, 1.]]) right_relaxed = [5.65, -1.76, -0.26, 1.96, -1.15, 0.87, -1.43] left_relaxed = [0.64, -1.76, 0.26, 1.96, 1.16, 0.87, 1.43] self.robot.SetTransform(start_pose) self.robot.SetActiveDOFValues(left_relaxed) return base_pose, grasp_config
def _Snap(self, robot, goal, **kw_args): from prpy.util import CheckJointLimits from prpy.util import GetLinearCollisionCheckPts from prpy.planning.exceptions import CollisionPlanningError from prpy.planning.exceptions import SelfCollisionPlanningError # Create a two-point trajectory between the # current configuration and the goal. # (a straight line in joint space) traj = openravepy.RaveCreateTrajectory(self.env, '') cspec = robot.GetActiveConfigurationSpecification('linear') active_indices = robot.GetActiveDOFIndices() # Check the start position is within joint limits, # this can throw a JointLimitError start = robot.GetActiveDOFValues() CheckJointLimits(robot, start) # Add the start waypoint start_waypoint = numpy.zeros(cspec.GetDOF()) cspec.InsertJointValues(start_waypoint, start, robot, active_indices, False) traj.Init(cspec) traj.Insert(0, start_waypoint.ravel()) # Make the trajectory end at the goal configuration, as # long as it is not in collision and is not identical to # the start configuration. CheckJointLimits(robot, goal) if not numpy.allclose(start, goal): goal_waypoint = numpy.zeros(cspec.GetDOF()) cspec.InsertJointValues(goal_waypoint, goal, robot, active_indices, False) traj.Insert(1, goal_waypoint.ravel()) # Get joint configurations to check # Note: this returns a python generator, and if the # trajectory only has one waypoint then only the # start configuration will be collisioned checked. # # Sampling function: # 'linear' # from prpy.util import SampleTimeGenerator # linear = SampleTimeGenerator # 'Van der Corput' from prpy.util import VanDerCorputSampleGenerator vdc = VanDerCorputSampleGenerator checks = GetLinearCollisionCheckPts(robot, traj, norm_order=2, sampling_func=vdc) # Run constraint checks at DOF resolution: for t, q in checks: # Set the joint positions # Note: the planner is using a cloned 'robot' object robot.SetActiveDOFValues(q) # Check for collisions report = openravepy.CollisionReport() if self.env.CheckCollision(robot, report=report): raise CollisionPlanningError.FromReport(report) elif robot.CheckSelfCollision(report=report): raise SelfCollisionPlanningError.FromReport(report) # Tag the return trajectory as smooth (in joint space). SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True) return traj