def solve_ik(self, target, grasp_type, fl_vec=[0, 0, 0], frame="base_link"): self.whole_body.linear_weight = 100.0 self.whole_body.angular_weight = 100.0 if grasp_type == "front_horizontal": self.whole_body.move_end_effector_pose(geometry.pose( x=(target.x + fl_vec[0]), y=(target.y + fl_vec[1]), z=(target.z + fl_vec[2]), ei=-math.pi / 2, ek=-math.pi / 2), ref_frame_id=frame) elif grasp_type == "front_vertical": self.whole_body.move_end_effector_pose(geometry.pose( x=(target.x + fl_vec[0]), y=(target.y + fl_vec[1]), z=(target.z + fl_vec[2]), ej=math.pi / 2), ref_frame_id=frame) elif grasp_type == "top": self.whole_body.move_end_effector_pose(geometry.pose( x=(target.x + fl_vec[0]), y=(target.y + fl_vec[1]), z=(target.z + fl_vec[2]), ei=-math.pi * 5 / 6, ek=-math.pi / 2), ref_frame_id=frame)
def manip(obj): # go to kitchen whole_body.move_to_go() omni_base.go(5.90, 2.0, -3.14, 120, relative=False) rospy.sleep(5) # for i in [1, 2, 3]: # try: # whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4') # break # except: # rospy.sleep(3) # tts.say("うんち") # rospy.loginfo("manip mistate no tf") # rospy.sleep(3) # omni_base.go(0.10, 0, 0, 10, relative=True) whole_body.move_end_effector_pose( geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4') whole_body.move_end_effector_pose(geometry.pose(z=0.05), 'hand_palm_link') gripper.grasp(-0.10) # -0.01 is too weak # go to table whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'hand_palm_link') omni_base.go(5.90, 4.25, -3.14, 120, relative=False) omni_base.go(5.25, 4.25, -3.14, 120, relative=False) # approach to table # omni_base.go(5.90, 4.4, -3.14, 120, relative=False) # omni_base.go(5.25, 4.4, -3.14, 120, relative=False) # approach to table # IK and release whole_body.move_end_effector_pose(geometry.pose(z=0.090), 'hand_palm_link') gripper.command(1.25) whole_body.move_end_effector_pose( geometry.pose(z=-0.20), 'hand_palm_link') # move arm far from table whole_body.move_to_go()
def manip_d(): # IK and release whole_body.move_end_effector_pose(geometry.pose(z=0.090), 'hand_palm_link') gripper.command(1.25) whole_body.move_end_effector_pose( geometry.pose(z=-0.20), 'hand_palm_link') # move arm far from table whole_body.move_to_go()
def execute_grasp(self, grasp_name, class_num=None): """ Picks up lego at target grasp Delivers lego to target bin by color To avoid collision errors, moves base in front of bin before moving gripper forward to deposit lego """ if not (class_num is None) and class_num not in range(len(cfg.labels)): raise ValueError("currently ony supports classes 0 to 7") self.gripper.open_gripper() self.whole_body.end_effector_frame = 'hand_palm_link' #before lowering gripper, go directly above grasp position self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1), grasp_name) self.whole_body.move_end_effector_pose(geometry.pose(z=0.015), grasp_name) self.gripper.close_gripper() self.whole_body.move_end_effector_pose(geometry.pose(z=-0.3), grasp_name) if class_num is None: self.temp_bin_pose() self.gripper.open_gripper() if not (class_num is None): color_name = class_num_to_name(class_num) print("CLASS IS " + cfg.labels[class_num]) #AR numbers from 8 to 11 self.place_in_bin(class_num + 8)
def manip(obj): # go to kitchen whole_body.move_to_go() omni_base.go(5.90, 2.0, -3.14, 120, relative=False) rospy.sleep(5) # for i in [1, 2, 3]: # try: # whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4') # break # except: # rospy.sleep(3) # tts.say("うんち") # rospy.loginfo("manip mistate no tf") # rospy.sleep(3) # omni_base.go(0.10, 0, 0, 10, relative=True) whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4') whole_body.move_end_effector_pose(geometry.pose(z=0.05), 'hand_palm_link') gripper.grasp(-0.10) # -0.01 is too weak # go to table whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'hand_palm_link') omni_base.go(5.90, 4.25, -3.14, 120, relative=False) omni_base.go(5.25, 4.25, -3.14, 120, relative=False) # approach to table # omni_base.go(5.90, 4.4, -3.14, 120, relative=False) # omni_base.go(5.25, 4.4, -3.14, 120, relative=False) # approach to table # IK and release whole_body.move_end_effector_pose(geometry.pose(z=0.090), 'hand_palm_link') gripper.command(1.25) whole_body.move_end_effector_pose(geometry.pose(z=-0.20), 'hand_palm_link') # move arm far from table whole_body.move_to_go()
def opendoor(req): # main(whole_body, gripper,wrist_wrench) frame = req.handle_pose.header.frame_id hanlde_pos = req.handle_pose.pose # hanlde_pos=geometry_msgs.msg.PoseStamped() res = OpendoorResponse() cli = actionlib.SimpleActionClient('/hsrb/omni_base_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction) vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10) armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1) robot = hsrb_interface.Robot() whole_body = robot.get('whole_body') gripper = robot.get('gripper') wrist_wrench = robot.get('wrist_wrench') base=robot.get('omni_base') start_theta=base.pose[2] # with hsrb_interface.Robot() as robot: # whole_body = robot.get('whole_body') # gripper = robot.get('gripper') # wrist_wrench = robot.get('wrist_wrench') try: # Open the gripper whole_body.move_to_neutral() grasp_point_client() global recog_pos global is_found print recog_pos.pose.position print("Opening the gripper") whole_body.move_to_neutral() rospy.sleep(2.5) switch = ImpedanceControlSwitch() gripper.command(1.0) # Approach to the door print("Approach to the door") grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos.pose.position.x-HANDLE_TO_HAND_POS_X, y=recog_pos.pose.position.y-HANDLE_TO_HAND_POS_Y, z=recog_pos.pose.position.z, ej=math.pi/2), geometry.pose(ek=math.pi/2)) whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF) # Close the gripper wrist_wrench.reset() switch.activate("grasping") gripper.grasp(-0.01) rospy.sleep(1.0) switch.inactivate() # Rotate the handle whole_body.impedance_config = 'grasping' traj = JointTrajectory() # This controller requires that all joints have values traj.joint_names = ["arm_lift_joint", "arm_flex_joint", "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
def grasp(): """ Grasping action. """ # Initial grasping setup manipulation_setup() # Command to open the gripper print("Grasp: opening gripper.") gripper.command(1.2) # Grasp: phase 1 (approach) print("Grasp: phase 1.") whole_body.move_end_effector_pose(geometry.pose(x=0.13, z=-0.1), OBJECT_TF_GRASP) # Grasp: phase 2 (grasp) print("Grasp: phase 2.") whole_body.move_end_effector_pose(geometry.pose(x=0.13, z=-0.02), OBJECT_TF_GRASP) # Specify the force to grasp print("Grasp: closing gripper now.") gripper.apply_force(GRASP_FORCE) # Wait time for simulator's grasp hack. Not needed on actual robot print("Grasp: simulator hack.") # Uncomment if working in simulation # rospy.sleep(2.0) # Transit to neutral position print("Grasp: neutral pose transition.") whole_body.move_to_neutral() # Rotate 180 degrees print("Grasp: 180 degree rotation.") omni_base.go_rel(0.0, 0.0, 3.14) # Release object print("Grasp: object drop.") gripper.command(1.2) # Uncomment if working in simulation # rospy.sleep(2.0) # Rotate to initial pose print("Grasp: 180 degree rotation.") omni_base.go_rel(0.0, 0.0, 3.14) # Close gripper print("Grasp: closing gripper.") gripper.command(0.0) print("Grasp: done.")
def yakan_solve_ik(self): yakan_point = self.get_bounding_box("yellow") self.solve_ik(yakan_point, "front_horizontal", fl_vec=[-0.1, 0.0, 0.2]) self.whole_body.move_end_effector_pose(geometry.pose(y=0.02), ref_frame_id="hand_palm_link") self.whole_body.move_end_effector_pose(geometry.pose(z=0.1), ref_frame_id="hand_palm_link") self.whole_body.move_end_effector_pose(geometry.pose(x=-0.02), ref_frame_id="hand_palm_link") self.gripper.apply_force(1.7) self.whole_body.move_end_effector_pose(geometry.pose(y=-0.05), ref_frame_id="hand_palm_link")
def execute_grasp(self, cards, whole_body, direction): whole_body.end_effector_frame = 'hand_palm_link' nothing = True #self.whole_body.move_to_neutral() #whole_body.linear_weight = 99.0 whole_body.move_end_effector_pose(geometry.pose(), cards[0]) self.com.grip_squeeze(self.gripper) whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'head_down') self.com.grip_open(self.gripper)
def execute_grasp(self, grasp_name): self.gripper.open_gripper() self.whole_body.end_effector_frame = 'hand_palm_link' self.whole_body.move_end_effector_pose(geometry.pose(), grasp_name) self.gripper.close_gripper() self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1), grasp_name) self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'head_down') self.gripper.open_gripper()
def get_hand_poses(self, tf_buffer, offset=geometry.pose()): ref_to_hands = [] for object_to_hand in self._object_to_hands: ref_to_hands.append( geometry.multiply_tuples(self._ref_to_object, object_to_hand)) if self._approach_frame == _HAND_FRAME: ref_to_hand_offsets = [] for ref_to_hand in ref_to_hands: ref_to_hand_offsets.append( geometry.multiply_tuples(ref_to_hand, offset)) return ref_to_hand_offsets else: hand_to_approach_tf = tf_buffer.lookup_transform( _HAND_FRAME, self._approach_frame, # rospy.get_rostime(), rospy.Time(0), rospy.Duration(1.0)) hand_to_approach = geometry.transform_to_tuples( hand_to_approach_tf.transform) ref_to_hand_offsets = [] for ref_to_hand in ref_to_hands: ref_to_approach = geometry.multiply_tuples( ref_to_hand, hand_to_approach) ref_to_approach_offset = geometry.multiply_tuples( ref_to_approach, offset) ref_to_hand_offset = geometry.multiply_tuples( ref_to_approach_offset, _inverse_tuples(hand_to_approach)) ref_to_hand_offsets.append(ref_to_hand_offset) return ref_to_hand_offsets
def place_in_bin(self, class_id): self.move_to_home() time.sleep(1) nothing = True i = 0 while nothing and i < 10: try: ar_name = 'ar_marker/' + str(class_id) self.whole_body.move_end_effector_pose( geometry.pose(y=0.08, z=-0.3), ar_name) nothing = False except: rospy.logerr('continuing to search for AR marker') i += 1 curr_tilt = -1 + (i * 1.0) / 5.0 self.whole_body.move_to_joint_positions( {'head_pan_joint': curr_tilt}) if nothing: print( "Could not find AR marker- depositing object in default position." ) self.temp_bin_pose() self.gripper.open_gripper()
def force_pull(self, whole_body, direction): """Pull to the target `direction`, hopefully with the sheet! I think this splits it into several motions, at most `cfg.MAX_PULLS`, so this explains the occasional pauses with the HSR's motion. This is *per* grasp+pull attempt, so it's different from the max attempts per side, which I empirically set at 4. Actually he set his max pulls to 3, but because of the delta <= 1.0 (including equality) this is 4 also. Move the end-effector, then quickly read the torque data and look at the wrench. The end-effector moves according to a _fraction_ that is specified by `s = 1-delta`, so I _hope_ if there's too much tension after that tiny bit of motion, that we'll exit. But maybe the discretization into four smaller pulls is not fine enough? """ count = 0 is_pulling = False t_o = self.get_translation(direction) delta = 0.0 while delta <= 1.0: s = 1.0 - delta whole_body.move_end_effector_pose( geometry.pose(x=t_o[0]*s, y=t_o[1]*s, z=t_o[2]*s), direction ) wrench = self.torque.read_data() norm = np.abs(self.compute_bed_tension(wrench,direction)) print("FORCE NORM: {}".format(norm)) if norm > cfg.HIGH_FORCE: is_pulling = True if is_pulling and norm < cfg.LOW_FORCE: break if norm > cfg.FORCE_LIMT: break delta += 1.0/float(cfg.MAX_PULLS)
def handing(): try: D = 2 #d = sub_face.center #print('{},{}'.format(D,d)) pan = np.round(whole_body.joint_positions['head_pan_joint'], 2) tilt = np.round(whole_body.joint_positions['head_tilt_joint'], 2) print('pan, tilt = {}, {}'.format(pan, tilt)) #-----------head_pan_link からface への座標変換--------------# i = np.array([[1, 0, 0], [0, math.cos(0), math.sin(0)], [0, -math.sin(0), math.cos(0)]]) j = np.array([[math.cos(tilt), 0, -math.sin(tilt)], [0, 1, 0], [math.sin(tilt), 0, math.cos(tilt)]]) k = np.array([[math.cos(pan), math.sin(pan), 0], [-math.sin(pan), math.cos(pan), 0], [0, 0, 1]]) pan2tilt = np.array([0.02, 0, 0]) tilt2camera = np.dot(j, np.array([-0.0798, 0.022, 0.2152])) pan2camera = pan2tilt + tilt2camera #base2pan = np.dot(k,np.array([0,0,0.752])) #base2tilt = #base2camera = base2pan + pan2camera print('pan2camera = {}'.format(pan2camera)) #print('base2camera = {}'.format(base2camera)) if tilt >= 0.52: tilt = math.acos(d * math.cos(0.52) / D) #x = D*math.cos(tilt)*math.cos(pan) x = D * math.cos(tilt) #y = D*math.cos(tilt)*math.sin(pan) y = 0 z = D * math.sin(tilt) camera2face = np.array([x, y, z]) print('cam2face = {}'.format(camera2face)) pan2face = pan2camera + camera2face face2hand = np.array([-0.5, 0, -0.5]) #print(pan2face) pan2hand = pan2face + face2hand Hand_TF = np.round(pan2hand, 2) #print(Hand_TF) handing_point = geometry.pose(Hand_TF[0], Hand_TF[1], Hand_TF[2], ei=1.57 * 2, ej=-1.57) #------------------------------------------------------# #rospy.sleep(0.001) #手渡し #whole_body.move_end_effector_pose(handing_point, ref_frame_id='head_pan_link') handing_END = True except rospy.ROSException as wait_for_msg_exception: rospy.logerr(wait_for_msg_exception)
def go_to_point(self, point, rot, c_img, d_img): y, x = point z_box = d_img[y - 20:y + 20, x - 20:x + 20] z = self.gp.find_mean_depth(z_box) print "singulation pose:", x, y, z pose_name = self.gripper.get_grasp_pose(x, y, z, rot, c_img=c_img) raw_input("Click enter to move to " + pose_name) self.whole_body.move_end_effector_pose(geometry.pose(), pose_name)
def kitchen_collision(self): collision_world.add_box(x=0.5, y=1.0, z=0.8, pose=geometry.pose(x=3.8, y=0.0, z=0.4), frame_id="map") #collision_world.remove_all() self.whole_body.collision_world = collision_world
def get_grasp_for_pose(object, distance_from_object_side=0.04): MAX_GRASPABLE_WIDTH = 0.12 MAX_GRASPABLE_LENGTH = 0.12 MIN_SIDE_GRASPABLE_HEIGHT = 0.04 GRIPPER_TO_PALM_LENGTH = 0.08 MIN_TOP_GRASPABLE_HEIGHT= 0.03 TOP_GRASP_DIST_TO_OBJECT_ADJUST = -0.04 possible_styles = {"top", "side"} if object.width > MAX_GRASPABLE_WIDTH: rospy.loginfo("Too wide to top grasp") possible_styles.discard("top") if object.length > MAX_GRASPABLE_LENGTH: rospy.loginfo("Too long to side grasp") possible_styles.discard("side") if object.height < MIN_SIDE_GRASPABLE_HEIGHT: rospy.loginfo("Too high to side grasp") possible_styles.discard("side") if object.height < MIN_TOP_GRASPABLE_HEIGHT: rospy.loginfo("Too low to top grasp") possible_styles.discard("top") if "side" in possible_styles: style = "side" # The robot approaches from the negative x direction delta_x = -(object.width / 2 + distance_from_object_side) delta_y = 0 target_pose = geometry.pose(x=delta_x, y=delta_y, z=object.height/4, ei=3.14, ej=-1.57, ek=0) gripper_width = BasicGraspPlanner.gripper_width_for_object_dimension(object.width) # Z axis extends from the palm elif "top" in possible_styles: style = "top" delta_x = 0 delta_y = 0 delta_z = object.height / 2 + distance_from_object_side + TOP_GRASP_DIST_TO_OBJECT_ADJUST target_pose = geometry.pose(x=delta_x, y=delta_y, z=delta_z, ei=3.14, ej=0., ek=1.57) gripper_width = BasicGraspPlanner.gripper_width_for_object_dimension(object.length) else: return None, None, None return style, target_pose, gripper_width
def execute(self, userdata): rospy.loginfo('State : GRASP') gripper.set_distance(.8, 1) whole_body.move_end_effector_pose( geometry.pose(x=-.51, y=-.51, z=.2, ei=3.1416, ek=1.57), 'my_frame_hole') whole_body.move_end_effector_pose( geometry.pose(x=-.51, y=-.51, z=-.2, ei=3.1416, ek=1.57), 'my_frame_hole') whole_body.move_end_effector_pose( geometry.pose(x=-.51, y=-.51, z=.2, ei=3.1416, ek=1.57), 'my_frame_hole') whole_body.move_to_neutral() base.go_rel(x=-.40, timeout=10) return 'outcome1'
def execute(self, userdata): rospy.loginfo('State : PLACE_TRASH_GRASPED_ABOVE') base.go_rel(-0.35, 0, 0, 10, "pumas") whole_body.move_to_go() whole_body.move_end_effector_pose( geometry.pose(ei=3.1416, ej=-1.57, ek=-1.57), 'trash') whole_body.move_end_effector_pose( geometry.pose(ei=3.1416, ej=-1.57, ek=-1.57), 'trash') whole_body.move_end_effector_by_line((-1, 0, 0), .1) gripper.set_distance(.8, .1) whole_body.move_end_effector_by_line((1, 0, -1), .08) gripper.set_distance(.01, .1) whole_body.move_to_go() #whole_body.move_end_effector_pose(geometry.pose(y=-.08,z=.19,ei=3.1416,ej=-1.57,ek=1.57), 'hole_reest') return 'outcome1'
def stop_yakan_solve_ik(self): self.whole_body.linear_weight = 100.0 self.whole_body.angular_weight = 100.0 #self.gripper.apply_force(1.0) self.whole_body.move_end_effector_pose(geometry.pose(z=0.1, ei=math.pi), ref_frame_id="button2") self.stop_yakan_tweak_flag = True
def go_to_centroid(self, whole_body): whole_body.end_effector_frame = 'hand_l_finger_vacuum_frame' nothing = True #self.whole_body.move_to_neutral() whole_body.move_end_effector_pose(geometry.pose(z=-0.02, ei=-0.785), 'ar_marker/11')
def execute_grasp(self): self.com.grip_open(self.gripper) self.whole_body.end_effector_frame = 'hand_palm_link' nothing = True try: self.whole_body.move_end_effector_pose(geometry.pose(y=0.15,z=-0.09, ek=-1.57),'ar_marker/9') except: rospy.logerr('mustard bottle found') self.com.grip_squeeze(self.gripper) self.whole_body.move_end_effector_pose(geometry.pose(z=-0.9),'hand_palm_link') self.com.grip_open(self.gripper) self.com.grip_squeeze(self.gripper)
def singulate(self, waypoints, rot, c_img, d_img, expand=False): self.gripper.close_gripper() pose_names = [ self.get_pose(waypoint, rot, c_img, d_img) for waypoint in waypoints ] self.whole_body.move_end_effector_pose(geometry.pose(z=-0.05), pose_names[0]) for pose_name in pose_names: print "singulating", pose_name self.whole_body.move_end_effector_pose(geometry.pose(z=0), pose_name) self.whole_body.move_end_effector_pose(geometry.pose(z=-0.05), pose_names[-1])
def execute(self, userdata): rospy.loginfo('State : go to table') whole_body.move_end_effector_pose( geometry.pose(x=-.05, z=.2, ei=3.1416, ek=1.57), 'my_frame_hole') gripper.set_distance(.8, .5) if self.visited: return 'outcome2' else: self.visited = True return 'outcome1'
def manip_e(obj): for i in [1, 2, 3]: try: whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4') break except: rospy.sleep(5) tts.say("うんち") rospy.loginfo("manip mistate no tf") rospy.sleep(5) omni_base.go(0.10, 0, 0, 10, relative=True)
def execute(self, userdata): rospy.loginfo('State : reestimate_hole') whole_body.move_end_effector_pose( geometry.pose(z=.2, ei=3.1416, ek=1.57), 'chido') if self.visited: return 'outcome2' else: self.visited = True return 'outcome1'
def get_grasp(object, style='side'): # Planning wrt /base_link frame thres = 0.05 if style == 'top': target_pose = geometry.pose(x=object.bbox_pose.pose.position.x, y=object.bbox_pose.pose.position.y, z=object.bbox_pose.pose.position.z + object.bbox_height / 2 + thres, ei=3.14, ej=0., ek=0.) else: target_pose = geometry.pose(x=object.bbox_pose.pose.position.x - object.bbox_width / 2 - thres, y=object.bbox_pose.pose.position.y, z=object.bbox_pose.pose.position.z, ei=3.14, ej=-1.57, ek=0.) return target_pose
def execute_suction(self, grasp_name, class_num): self.whole_body.end_effector_frame = "hand_l_finger_vacuum_frame" self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1), grasp_name) self.whole_body.move_end_effector_pose(geometry.pose(z=0.1), grasp_name) self.suction.start() self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1), grasp_name) color_name = class_num_to_name(class_num) print("Identified lego: " + color_name) lego_class_num = cfg.HUES_TO_BINS.index(color_name) above_pose = "lego" + str(lego_class_num) + "above" below_pose = "lego" + str(lego_class_num) + "below" IPython.embed() self.whole_body.end_effector_frame = 'hand_palm_link' self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1), above_pose) self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1), below_pose) self.suction.stop() self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1), above_pose)
def go_to_pose(self, results, c_img, d_img): whole_body.end_effector_frame = 'hand_l_finger_vacuum_frame' nothing = True #self.whole_body.move_to_neutral() #whole_body.linear_weight = 99.0 transforms = self.tl.getFrameStrings() cards = [] for transform in transforms: if 'belief' in transform: f_p = self.tl.lookupTransform('head_rgbd_sensor_rgb_frame', transform, rospy.Time(0)) cards.append(transform) return True, cards whole_body.move_end_effector_pose(geometry.pose(), cards[0]) self.base.move(geometry.pose(), 100.0, ref_frame_id='belief')
def manip_e(obj): for i in [1, 2, 3]: try: whole_body.move_end_effector_pose( geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4') break except: rospy.sleep(5) tts.say("うんち") rospy.loginfo("manip mistate no tf") rospy.sleep(5) omni_base.go(0.10, 0, 0, 10, relative=True)
def execute(self, userdata): rospy.loginfo('STATE : initial state') whole_body.linear_weight = 20 whole_body.move_to_go() collision.remove_all() collision.add_cylinder(radius=0.2, length=.40, pose=geometry.pose(x=1.0, z=.30)) whole_body.collision_world = collision #base.go_rel(x=-.40, timeout=10) #base.go_abs(x=0,y=0.3, timeout=10) return 'outcome1'
def callback(data): whole_body.move_to_neutral() omni_base.go_abs(0, 0, 0) omni_base.go_rel(x=-0.1, y=-0.08) data = data.data.reshape(-1, 2) dis = [(x - 0)**2 + (y - 240)**2 for (x, y) in data] ob_to = data[np.argmax(dis)] ob_from = data[np.argmin(dis)] print "from:", ob_from, ", to", ob_to omni_base.go_rel(y=-(ob_from[0] - 320.) / 306.) gripper.set_distance(1.3) omni_base.go_rel(x=0.1) whole_body.move_end_effector_pose(geometry.pose(x=-0.18, z=0.16, ej=math.radians(-90.0)), ref_frame_id='hand_palm_link') gripper.apply_force(1.0) whole_body.move_to_neutral() omni_base.go_abs(0, 0, 0) omni_base.go_rel(x=-0.1, y=-0.08) omni_base.go_rel(y=-(ob_to[0] - 320.) / 306.) omni_base.go_rel(x=0.1) whole_body.move_end_effector_pose(geometry.pose(x=-0.10, z=0.16, ej=math.radians(-90.0)), ref_frame_id='hand_palm_link') gripper.set_distance(1.3) whole_body.move_to_neutral() omni_base.go_abs(0, 0, 0)
import rospy import sys from hsrb_interface import geometry # 移動のタイムアウト[s] _MOVE_TIMEOUT=60.0 # ロボット機能を使うための準備 robot = hsrb_interface.Robot() omni_base = robot.get('omni_base') whole_body = robot.get('whole_body') gripper = robot.get('gripper') tts = robot.get('default', robot.Items.TEXT_TO_SPEECH) # handを0.1[m]上に移動させる姿勢 hand_up = geometry.pose(x=0.1) # handを0.5[m]手前に移動させる姿勢 hand_back = geometry.pose(z=-0.5) def hello(msg): """ こんにちわ。 :param string msg: 出力するメッセージ """ print msg def add(a, b): """ 足し算ぐらいできます。
def manip_c(): # go to table whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'hand_palm_link') omni_base.go(5.90, 4.4, -3.14, 120, relative=False) omni_base.go(5.25, 4.4, -3.14, 120, relative=False) # approach to table
def manip_b(): # IK and grasp whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4') whole_body.move_end_effector_pose(geometry.pose(z=0.05), 'hand_palm_link') gripper.grasp(-0.10) # -0.01 is too weak
def manip_d(): # IK and release whole_body.move_end_effector_pose(geometry.pose(z=0.090), 'hand_palm_link') gripper.command(1.25) whole_body.move_end_effector_pose(geometry.pose(z=-0.20), 'hand_palm_link') # move arm far from table whole_body.move_to_go()