def create_goal_pose(self, x, y, z, gripper_pose): point = [x, y, z] point = self.transform_in_frame(point, gripper_pose, -self.GRIPPER_POINT).tolist() pose = point + gripper_pose.tolist() goal_pose = cf.create_pose_stamped(pose, "torso_lift_link") goal_pose.header.stamp = rospy.Time.now() return goal_pose
def _move_cartesian_ik(self, position, orientation, \ stop_funcs=[], timeout = 30., settling_time = 0.25, \ frame='base_link', vel=.15): #pdb.set_trace() #self.arm_obj.set_cart_pose_ik(cart_pose, total_time=motion_length, frame=frame, block=False) #cart_pose = tfu.tf_as_matrix((position.A1.tolist(), orientation.A1.tolist())) self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame, rospy.Time(), rospy.Duration(10)) toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame, self.tf_listener) cart_pose = tfu.tf_as_matrix((position.A1.tolist(), orientation.A1.tolist())) cart_pose = cart_pose * toolframe_T_ikframe position, orientation = tfu.matrix_as_tf(cart_pose) #goal_pose_ps = create_pose_stamped(position.A1.tolist() + orientation.A1.tolist(), frame) goal_pose_ps = cf.create_pose_stamped(position.tolist() + orientation.tolist(), frame) r = self.reactive_gr.cm.move_cartesian_ik(goal_pose_ps, blocking=0, step_size=.005, \ pos_thres=.02, rot_thres=.1, timeout=rospy.Duration(timeout), settling_time=rospy.Duration(settling_time), vel=vel) if not (r == 'sent goal' or r == 'success'): return r #move_cartesian_ik(self, goal_pose, collision_aware = 0, blocking = 1, # step_size = .005, pos_thres = .02, rot_thres = .1, # timeout = rospy.Duration(30.), # settling_time = rospy.Duration(0.25), vel = .15): stop_trigger = None done_time = None start_time = rospy.get_rostime() while stop_trigger == None: for f, name in stop_funcs: if f(): self.arm_obj.stop_trajectory_execution() stop_trigger = name rospy.loginfo('"%s" requested that motion should be stopped.' % (name)) break if timeout != 0. and rospy.get_rostime() - start_time > rospy.Duration(timeout): rospy.loginfo("_move_cartesian_ik: motion timed out") break if (not done_time) and (not self.arm_obj.has_active_goal()): #rospy.loginfo("check_cartesian_done returned 1") done_time = rospy.get_rostime() if done_time and rospy.get_rostime() - done_time > rospy.Duration(settling_time): rospy.loginfo("_move_cartesian_ik: done settling") break if stop_trigger == 'pressure_safety': print 'ROBOT SAFETY ERROR' #raise RobotSafetyError(stop_trigger) return stop_trigger
def _move_cartesian_ik(self, position, orientation, \ stop_funcs=[], timeout = 30., settling_time = 0.25, \ frame='base_link', vel=.15): #pdb.set_trace() #self.arm_obj.set_cart_pose_ik(cart_pose, total_time=motion_length, frame=frame, block=False) #cart_pose = tfu.tf_as_matrix((position.A1.tolist(), orientation.A1.tolist())) self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame, rospy.Time(), rospy.Duration(10)) toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame, self.tf_listener) cart_pose = tfu.tf_as_matrix( (position.A1.tolist(), orientation.A1.tolist())) cart_pose = cart_pose * toolframe_T_ikframe position, orientation = tfu.matrix_as_tf(cart_pose) #goal_pose_ps = create_pose_stamped(position.A1.tolist() + orientation.A1.tolist(), frame) goal_pose_ps = cf.create_pose_stamped( position.tolist() + orientation.tolist(), frame) r = self.reactive_gr.cm.move_cartesian_ik(goal_pose_ps, blocking=0, step_size=.005, \ pos_thres=.02, rot_thres=.1, timeout=rospy.Duration(timeout), settling_time=rospy.Duration(settling_time), vel=vel) if not (r == 'sent goal' or r == 'success'): return r #move_cartesian_ik(self, goal_pose, collision_aware = 0, blocking = 1, # step_size = .005, pos_thres = .02, rot_thres = .1, # timeout = rospy.Duration(30.), # settling_time = rospy.Duration(0.25), vel = .15): stop_trigger = None done_time = None start_time = rospy.get_rostime() while stop_trigger == None: for f, name in stop_funcs: if f(): self.arm_obj.stop_trajectory_execution() stop_trigger = name rospy.loginfo( '"%s" requested that motion should be stopped.' % (name)) break if timeout != 0. and rospy.get_rostime( ) - start_time > rospy.Duration(timeout): rospy.loginfo("_move_cartesian_ik: motion timed out") break if (not done_time) and (not self.arm_obj.has_active_goal()): #rospy.loginfo("check_cartesian_done returned 1") done_time = rospy.get_rostime() if done_time and rospy.get_rostime() - done_time > rospy.Duration( settling_time): rospy.loginfo("_move_cartesian_ik: done settling") break if stop_trigger == 'pressure_safety': print 'ROBOT SAFETY ERROR' #raise RobotSafetyError(stop_trigger) return stop_trigger
def _move_cartesian_cart(self, position, orientation, \ stop_funcs=[], timeout = 3.0, settling_time = 0.5, frame='base_link'): # TODO: Test this function... # Transform the pose from 'frame' to 'base_link' self.tf_listener.waitForTransform('base_link', frame, rospy.Time(), rospy.Duration(10)) frame_T_base = tfu.transform('base_link', frame, self.tf_listener) init_cart_pose = tfu.tf_as_matrix( (position.A1.tolist(), orientation.A1.tolist())) base_cart_pose = frame_T_base * init_cart_pose # Get IK from tool frame to wrist frame for control input self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame, rospy.Time(), rospy.Duration(10)) toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame, self.tf_listener) #base_cart_pose = tfu.tf_as_matrix((base_position.A1.tolist(), base_orientation.A1.tolist())) base_cart_pose = base_cart_pose * toolframe_T_ikframe base_position, base_orientation = tfu.matrix_as_tf(base_cart_pose) pose_stamped = cf.create_pose_stamped(base_position.tolist() + base_orientation.tolist()) rg = self.reactive_gr rg.check_preempt() #send the goal to the Cartesian controllers #rospy.loginfo("sending goal to Cartesian controllers") (pos, rot) = cf.pose_stamped_to_lists(rg.cm.tf_listener, pose_stamped, 'base_link') rg.move_cartesian_step(pos + rot, timeout, settling_time) #watch the fingertip/palm sensors until the controllers are done and then some start_time = rospy.get_rostime() done_time = None #stopped = False stop_trigger = None #print 'enterning loop' stop_detector = ArmStoppedDetector() while (1): rg.check_preempt() if len(stop_funcs) > 0: for f, name in stop_funcs: if f(): rg.cm.switch_to_joint_mode() rg.cm.freeze_arm() #stopped = True stop_trigger = name rospy.loginfo( '"%s" requested that motion should be stopped.' % (name)) break if stop_trigger != None: break #if stop_func != None and stop_func(): # rg.cm.switch_to_joint_mode() # rg.cm.freeze_arm() # stopped = True # break #check if we're actually there if rg.cm.check_cartesian_near_pose(pose_stamped, .0025, .05): rospy.loginfo("_move_cartesian_cart: reached pose") #stop_trigger = 'at_pose' break stop_detector.record_diff(self.arm_obj.pose_cartesian()) if stop_detector.is_stopped(): rospy.loginfo("_move_cartesian_cart: arm stopped") #stop_trigger = 'stopped' break # if rg.cm.check_cartesian_really_done(pose_stamped, .0025, .05): # #rospy.loginfo("actually got there") # break # # #check if the controllers think we're done # if not done_time and rg.cm.check_cartesian_done(): # #rospy.loginfo("check_cartesian_done returned 1") # done_time = rospy.get_rostime() # #done settling # if done_time and rospy.get_rostime() - done_time > rospy.Duration(settling_time): # rospy.loginfo("done settling") # break #timed out #if timeout != 0. and rospy.get_rostime() - start_time > rospy.Duration(timeout): # rospy.loginfo("_move_cartesian_cart: timed out") # break #if stop_trigger == 'pressure_safety' or stop_trigger == 'self_collision': if stop_trigger == 'pressure_safety': print 'ROBOT SAFETY ERROR' #raise RobotSafetyError(stop_trigger) #name = ut.formatted_time() + '_stop_detector.pkl' #print 'saved', name #ut.save_pickle(stop_detector, name) return stop_trigger
def _move_cartesian_cart(self, position, orientation, \ stop_funcs=[], timeout = 3.0, settling_time = 0.5, frame='base_link'): # TODO: Test this function... # Transform the pose from 'frame' to 'base_link' self.tf_listener.waitForTransform('base_link', frame, rospy.Time(), rospy.Duration(10)) frame_T_base = tfu.transform('base_link', frame, self.tf_listener) init_cart_pose = tfu.tf_as_matrix((position.A1.tolist(), orientation.A1.tolist())) base_cart_pose = frame_T_base*init_cart_pose # Get IK from tool frame to wrist frame for control input self.tf_listener.waitForTransform(self.ik_frame, self.tool_frame, rospy.Time(), rospy.Duration(10)) toolframe_T_ikframe = tfu.transform(self.tool_frame, self.ik_frame, self.tf_listener) #base_cart_pose = tfu.tf_as_matrix((base_position.A1.tolist(), base_orientation.A1.tolist())) base_cart_pose = base_cart_pose * toolframe_T_ikframe base_position, base_orientation = tfu.matrix_as_tf(base_cart_pose) pose_stamped = cf.create_pose_stamped(base_position.tolist() + base_orientation.tolist()) rg = self.reactive_gr rg.check_preempt() #send the goal to the Cartesian controllers #rospy.loginfo("sending goal to Cartesian controllers") (pos, rot) = cf.pose_stamped_to_lists(rg.cm.tf_listener, pose_stamped, 'base_link') rg.move_cartesian_step(pos+rot, timeout, settling_time) #watch the fingertip/palm sensors until the controllers are done and then some start_time = rospy.get_rostime() done_time = None #stopped = False stop_trigger = None #print 'enterning loop' stop_detector = ArmStoppedDetector() while(1): rg.check_preempt() if len(stop_funcs) > 0: for f, name in stop_funcs: if f(): rg.cm.switch_to_joint_mode() rg.cm.freeze_arm() #stopped = True stop_trigger = name rospy.loginfo('"%s" requested that motion should be stopped.' % (name)) break if stop_trigger != None: break #if stop_func != None and stop_func(): # rg.cm.switch_to_joint_mode() # rg.cm.freeze_arm() # stopped = True # break #check if we're actually there if rg.cm.check_cartesian_near_pose(pose_stamped, .0025, .05): rospy.loginfo("_move_cartesian_cart: reached pose") #stop_trigger = 'at_pose' break stop_detector.record_diff(self.arm_obj.pose_cartesian()) if stop_detector.is_stopped(): rospy.loginfo("_move_cartesian_cart: arm stopped") #stop_trigger = 'stopped' break # if rg.cm.check_cartesian_really_done(pose_stamped, .0025, .05): # #rospy.loginfo("actually got there") # break # # #check if the controllers think we're done # if not done_time and rg.cm.check_cartesian_done(): # #rospy.loginfo("check_cartesian_done returned 1") # done_time = rospy.get_rostime() # #done settling # if done_time and rospy.get_rostime() - done_time > rospy.Duration(settling_time): # rospy.loginfo("done settling") # break #timed out #if timeout != 0. and rospy.get_rostime() - start_time > rospy.Duration(timeout): # rospy.loginfo("_move_cartesian_cart: timed out") # break #if stop_trigger == 'pressure_safety' or stop_trigger == 'self_collision': if stop_trigger == 'pressure_safety': print 'ROBOT SAFETY ERROR' #raise RobotSafetyError(stop_trigger) #name = ut.formatted_time() + '_stop_detector.pkl' #print 'saved', name #ut.save_pickle(stop_detector, name) return stop_trigger