Esempio n. 1
0
    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)
Esempio n. 2
0
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()
Esempio n. 3
0
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)
Esempio n. 5
0
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()
Esempio n. 6
0
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"]
Esempio n. 7
0
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.")
Esempio n. 8
0
    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")
Esempio n. 9
0
    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)
Esempio n. 10
0
    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()
Esempio n. 11
0
    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
Esempio n. 12
0
    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()
Esempio n. 13
0
    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)
Esempio n. 15
0
 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)
Esempio n. 16
0
 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
Esempio n. 17
0
    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
Esempio n. 18
0
    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'
Esempio n. 19
0
    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'
Esempio n. 20
0
 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
Esempio n. 21
0
    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')
Esempio n. 22
0
	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)
Esempio n. 23
0
    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])
Esempio n. 24
0
    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'
Esempio n. 25
0
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)
Esempio n. 26
0
    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'
Esempio n. 27
0
    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
Esempio n. 28
0
    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)
Esempio n. 29
0
    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')
Esempio n. 30
0
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)
Esempio n. 31
0
    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'
Esempio n. 32
0
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)
Esempio n. 33
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):
    """
    足し算ぐらいできます。
Esempio n. 34
0
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
Esempio n. 35
0
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
Esempio n. 36
0
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()