def make_grasps(self, pose_stamped, mega_angle=False): # setup defaults of grasp g = Grasp() g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) g.pre_grasp_approach = self.make_gripper_translation(0.1, 0.15) g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, -1.0) g.grasp_pose = pose_stamped pitch_vals = [0, 0.2, -0.2, 0.4, -0.4] if mega_angle: pitch_vals += [0.3, -0.3, 0.5, -0.5, 0.6, -0.6] # generate list of grasps grasps = [] for y in [-1.57, -0.78, 0, 0.78, 1.57]: for p in pitch_vals: q = quaternion_from_euler(0, 1.57 - p, y) g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] g.id = str(len(grasps)) g.grasp_quality = 1.0 - abs(p / 2.0) grasps.append(copy.deepcopy(g)) return grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # 初始化抓取姿态对象 g = Grasp() # 创建夹爪张开、闭合的姿态 g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) # 设置期望的夹爪靠近、撤离目标的参数 # g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.05, [1.0, 0.0, 0.0]) g.post_grasp_retreat = self.make_gripper_translation( 0.01, 0.06, [0.0, 0.0, 1.0]) g.pre_grasp_approach = self.make_gripper_translation( 0.02, 0.05, self.direction_z) # 设置抓取姿态 g.grasp_pose = initial_pose_stamped # 抓取姿态的列表 grasps = [] # 设置抓取的唯一id号 g.id = str(len(grasps)) # 设置允许接触的物体 g.allowed_touch_objects = allowed_touch_objects # 将本次规划的抓取放入抓取列表中 grasps.append(deepcopy(g)) # 返回抓取列表 return grasps
def make_grasps(self, pose_stamped, mega_angle=False): # setup defaults for the grasp g = Grasp() g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) g.pre_grasp_approach = self.make_gripper_translation(0.05, 0.1) g.post_grasp_retreat = self.make_gripper_translation(0.05, 0.1, -1.0) g.grasp_pose = pose_stamped pitch_vals = [0, 0.2, -0.2, 0.4, -0.4] #pitch_vals = [0] yaw_vals = [-0.2, -0.1, 0, 0.1, 0.2] #yaw_vals = [0] if mega_angle: pitch_vals += [0.78, -0.78, 0.3, -0.3, 0.5, -0.5, 0.6, -0.6] # generate list of grasps grasps = [] #for y in [-1.57, -0.78, 0, 0.78, 1.57]: for y in yaw_vals: for p in pitch_vals: q = quaternion_from_euler(0, 1.57-p, y) g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] g.id = str(len(grasps)) g.allowed_touch_objects = ["part"] g.max_contact_force = 0 #g.grasp_quality = 1.0 - abs(p/2.0) grasps.append(copy.deepcopy(g)) return grasps
def graspObj(self, x, y, z, phi, theta, psi): grasps = [] g = Grasp() g.id = "test" grasp_pose = PoseStamped() grasp_pose.header.frame_id = "base_footprint" orientation_tuple = tf.transformations.quaternion_from_euler( phi, theta, psi) grasp_pose.orientation.x = orientation_tuple[0] grasp_pose.orientation.y = orientation_tuple[1] grasp_pose.orientation.z = orientation_tuple[2] grasp_pose.orientation.w = orientation_tuple[3] grasp_pose.position.x = x grasp_pose.position.y = y grasp_pose.position.z = z self.group.set_pose_target(grasp_pose) self.group.go() rospy.sleep(2) # set the grasp pose g.grasp_pose = grasp_pose self.group.pick("unit_box", g)
def createGrasp(grasp_pose, allowed_touch_objects=[], pre_grasp_posture=None, grasp_posture=None, pre_grasp_approach=None, post_grasp_retreat=None, id_grasp="grasp_"): """Create a grasp for object in grasp_pose, allowing collisions with allowed_touch_objects list, with pre_grasp_posture and grasp_posture as positions of the hand. Also id name id_grasp.""" grasp = Grasp() grasp.id = id_grasp # header = Header() # header.frame_id = "base_link" # header.stamp = rospy.Time.now() # grasp_pose_msg = PoseStamped(header, grasp_pose) grasp_pose_with_offset = add_offset_reem_hand(grasp_pose) grasp.grasp_pose = grasp_pose_with_offset if pre_grasp_posture == None: grasp.pre_grasp_posture = getPreGraspPosture else: grasp.pre_grasp_posture = pre_grasp_posture if grasp_posture == None: grasp.grasp_posture = getGraspPosture else: grasp.grasp_posture = grasp_posture grasp.allowed_touch_objects = allowed_touch_objects # ["table", "part"] if pre_grasp_approach != None: grasp.pre_grasp_approach = pre_grasp_approach if post_grasp_retreat != None: grasp.post_grasp_retreat = post_grasp_retreat grasp.max_contact_force = 0 #grasp.grasp_quality = 0 return grasp
def make_grasps(self, initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately; # grasp_opening should be a bit smaller than target width g.pre_grasp_posture = self.make_gripper_posture(self.gripper_opened) g.grasp_posture = self.make_gripper_posture(grasp_opening) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0]) g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4] # Yaw angles to try; given the limited dofs of turtlebot_arm, we must calculate the heading # from arm base to the object to pick (first we must transform its pose to arm base frame) target_pose_arm_ref = self.tf_listener.transformPose(ARM_BASE_FRAME, initial_pose_stamped) x = target_pose_arm_ref.pose.position.x y = target_pose_arm_ref.pose.position.y self.pick_yaw = atan2(y, x) # check in make_places method why we store the calculated yaw yaw_vals = [self.pick_yaw] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle for yaw in yaw_vals: for pitch in pitch_vals: # Create a quaternion from the Euler angles q = quaternion_from_euler(0, pitch, yaw) # Set the grasp pose orientation accordinglytb_return_pose.launch g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(pitch) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps
def __get_grasp(self, name): try: open_state = self.__get_state(name + "_open", "").state closed_state = self.__get_state(name + "_closed", "").state except Exception: rospy.logfatal("Couldn't get grasp pose from db.") return Grasp() try: self.__group.set_start_state_to_current_state() pre_pose = self.__group.plan(open_state.joint_state) self.__group.set_start_state(open_state) pose = self.__group.plan(closed_state.joint_state) except Exception: rospy.logfatal("Couldn't plan grasp trajectories.") return Grasp() grasp = Grasp() grasp.id = name grasp.pre_grasp_posture = pre_pose.joint_trajectory grasp.grasp_posture = pose.joint_trajectory grasp.pre_grasp_approach.desired_distance = 0.2 grasp.pre_grasp_approach.min_distance = 0.1 grasp.pre_grasp_approach.direction.vector.x = 0 grasp.pre_grasp_approach.direction.vector.y = -1 grasp.pre_grasp_approach.direction.vector.z = 0 return grasp
def make_grasps(self, pose_stamped, mega_angle=False): # setup defaults for the grasp g = Grasp() g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) g.pre_grasp_approach = self.make_gripper_translation(0.05, 0.1) g.post_grasp_retreat = self.make_gripper_translation(0.05, 0.1, -1.0) g.grasp_pose = pose_stamped pitch_vals = [0,0.4, 0.9, 1.2, 1.57, 1.8, 2.2, 2.8, 3.14] #pitch_vals = [0] yaw_vals = [-0.2, -0.1, 0, 0.1, 0.2] #yaw_vals = [0] if mega_angle: pitch_vals += [1.57, -1.57, 0.3, -0.3, 0.5, -0.5, 0.6, -0.6] # generate list of grasps grasps = [] #for y in [-1.57, -0.78, 0, 0.78, 1.57]: for y in yaw_vals: for p in pitch_vals: q = quaternion_from_euler(0, 1.57-p, y) g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] g.id = str(len(grasps)) g.allowed_touch_objects = ["part"] g.max_contact_force = 0 #g.grasp_quality = 1.0 - abs(p/2.0) grasps.append(copy.deepcopy(g)) return grasps
def create_grasp(grasp_pose, allowed_touch_objects=[], pre_grasp_posture=None, grasp_posture=None, pre_grasp_approach=None, post_grasp_retreat=None, id_grasp="grasp_"): grasp = Grasp() grasp.id = id_grasp grasp.grasp_pose = grasp_pose if pre_grasp_posture is None: grasp.pre_grasp_posture = get_grasp_posture(True) else: grasp.pre_grasp_posture = pre_grasp_posture if grasp_posture is None: grasp.grasp_posture = get_grasp_posture(False) else: grasp.grasp_posture = grasp_posture grasp.allowed_touch_objects = allowed_touch_objects if pre_grasp_approach is not None: grasp.pre_grasp_approach = pre_grasp_approach if post_grasp_retreat is not None: grasp.post_grasp_retreat = post_grasp_retreat grasp.max_contact_force = 0 return grasp
def get_grasp(self, grasp_pose, id): grasp = Grasp() grasp.grasp_pose = grasp_pose grasp.grasp_pose.header.frame_id = self.frame_id joint_name = self.gripper + '_gripper_l_finger_joint' point = JointTrajectoryPoint() point.positions.append(0.095) grasp.pre_grasp_posture.joint_names.append(joint_name) grasp.pre_grasp_posture.points.append(point) point = JointTrajectoryPoint() point.positions.append(-0.0125) grasp.grasp_posture.joint_names.append(joint_name) grasp.grasp_posture.points.append(point) grasp.grasp_quality = 1.0 grasp.id = str(id) grasp.post_place_retreat.desired_distance = 0.3 grasp.post_place_retreat.min_distance = 0.05 grasp.post_place_retreat.direction.header.frame_id = 'world' grasp.post_place_retreat.direction.vector.z = 1.0 grasp.pre_grasp_approach.desired_distance = 0.3 grasp.pre_grasp_approach.min_distance = 0.05 grasp.pre_grasp_approach.direction.header.frame_id = self.gripper + "_gripper" grasp.pre_grasp_approach.direction.vector.y = 1.0 grasp.post_grasp_retreat = grasp.post_place_retreat return grasp
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.065, [1.0, 0.0, 0.0]) g.post_grasp_retreat = self.make_gripper_translation(0.01, 0.1, [0.0, 0.0, 1.0]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3] # Yaw angles to try yaw_vals = [-1.57] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle for y in yaw_vals: # for p in pitch_vals: for i in range(0,3): p = (float)(i / 1000.0) print("pitch", p) # Create a quaternion from the Euler angles q = quaternion_from_euler(-1.57, p, y) # q = quaternion_from_euler(-1.5708, p, y) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(p) # Append the grasp to the list grasps.append(deepcopy(g)) # print("grasp_pose_pose", g) # Return the list return grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.2, [0.0, 1.0, 0.0]) g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.2, [0.0, 0.0, 1.0]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3] # Yaw angles to try yaw_vals = [0] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle for y in yaw_vals: for p in pitch_vals: # Create a quaternion from the Euler angles # q = quaternion_from_euler(0, p, y) # # Set the grasp pose orientation accordingly # g.grasp_pose.pose.orientation.x = q[0] # g.grasp_pose.pose.orientation.y = q[1] # g.grasp_pose.pose.orientation.z = q[2] # g.grasp_pose.pose.orientation.w = q[3] q = quaternion_from_euler(0, 0, -1.57079633) g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(p) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps
def generate_grasp_msgs(self, grasps): formatted_grasps = [] for i in range(0, len(grasps)): #dimitris, take out self. ! g = Grasp() g.id = "dupa" gp = PoseStamped() gp.header.frame_id = "camera_color_optical_frame" quat = self.trans_matrix_to_quaternion(grasps[i]) # Move grasp back for given offset gp.pose.position.x = grasps[ i].surface.x + self.grasp_offset * grasps[i].approach.x gp.pose.position.y = grasps[ i].surface.y + self.grasp_offset * grasps[i].approach.y gp.pose.position.z = grasps[ i].surface.z + self.grasp_offset * grasps[i].approach.z gp.pose.orientation.x = float(quat.elements[1]) gp.pose.orientation.y = float(quat.elements[2]) gp.pose.orientation.z = float(quat.elements[3]) gp.pose.orientation.w = float(quat.elements[0]) g.grasp_pose = gp g.pre_grasp_approach.direction.header.frame_id = "hand_link" g.pre_grasp_approach.direction.vector.x = 0.0 g.pre_grasp_approach.direction.vector.y = 0.0 g.pre_grasp_approach.direction.vector.z = 1.0 g.pre_grasp_approach.min_distance = 0.05 g.pre_grasp_approach.desired_distance = 0.1 # g.pre_grasp_posture.joint_names = ["gripper_right_finger_joint", "gripper_left_finger_joint"] g.pre_grasp_posture.joint_names = ["joint_6"] g.pre_grasp_posture.header.frame_id = "hand_link" pos = JointTrajectoryPoint() pos.positions.append(0) # pos.positions.append(0.1337) g.pre_grasp_posture.points.append(pos) # g.grasp_posture.joint_names = ["gripper_right_finger_joint", "gripper_left_finger_joint"] # g.grasp_posture.joint_names = ["joint_6"] # pos = JointTrajectoryPoint() # pos.positions.append(0.0) # pos.positions.append(0.0) # pos.accelerations.append(0.0) # pos.accelerations.append(0.0) # g.grasp_posture.points.append(pos) # g.grasp_posture.header.frame_id = "hand_link" g.allowed_touch_objects = ["<octomap>", "obj"] g.max_contact_force = 0.0 #g.grasp_quality = grasps[0].score.data perche 0 e non i???? g.grasp_quality = grasps[i].score.data formatted_grasps.append(g) return formatted_grasps
def pick(self): ##-----------GRAPS---------------## #set all grasp poses #first create an array where all graps are going to be saved grasps = [] #graps 1, box #create object grasp g = Grasp() g.id="box_grasp" #GRASP POSE: position of object in end efector seen from world grasp_pose = PoseStamped() grasp_pose.header.frame_id = "panda_link0" grasp_pose.pose.position.x = 0.4 #(box position- (half box lenght- distance distance b/w panda_link8 and palm of eef (0.058) - some extra padding))) grasp_pose.pose.position.y = 0 grasp_pose.pose.position.z = 0.5 #orientation in quaternion #orientation in quaternion q = quaternion_from_euler(-pi/2,-pi/4,-pi/2) grasp_pose.pose.orientation.x = q[0] grasp_pose.pose.orientation.y = q[1] grasp_pose.pose.orientation.z = q[2] grasp_pose.pose.orientation.w = q[3] g.grasp_pose = grasp_pose #set grasp pose #pre-grasp approach g.pre_grasp_approach.direction.header.frame_id = "panda_link0" g.pre_grasp_approach.direction.vector.x = 1.0 g.pre_grasp_approach.min_distance = 0.095 g.pre_grasp_approach.desired_distance = 0.115 self.openGripperPose(g.pre_grasp_posture) #posture before grasp #posture during grasp self.closeGripperPose(g.grasp_posture,0.02) #post-grasp retreat g.post_grasp_retreat.direction.header.frame_id = "panda_link0" g.post_grasp_retreat.direction.vector.z = 1.0 g.post_grasp_retreat.min_distance = 0.1 g.post_grasp_retreat.desired_distance= 0.25 #surface in contact with object #g.allowed_touch_objects = ["table1","box","table2"] self.move_group_arm.set_support_surface_name("table1") # append the grasp to the list of grasps grasps.append(g) rospy.sleep(2) # pick the object self.move_group_arm.pick("box", grasps)
def make_grasps(initial_pose_stamped, allowed_touch_objects, grasp_opening=[0]): # INicio el grasp g = Grasp() # Asigno las posiciones de pre-grasp y grasp postures; g.pre_grasp_posture = make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = make_gripper_posture(grasp_opening) # Defino los parametros de aproximación y alejar deseados g.pre_grasp_approach = make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0]) g.post_grasp_retreat = make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0]) # Defino la primera grasp pose g.grasp_pose = initial_pose_stamped # Pitch angulos a probar: POR AHORA SOLO UNO pitch_vals = [0] # Yaw angles to try: POR AHORA SOLO UNO yaw_vals = [0] # A list to hold the grasps grasps = [] # Genero un grasp para cada angulo pitch y yaw for y in yaw_vals: for p in pitch_vals: # Creo un quaternion de Euler angles, con un roll de pi (cenital) q = quaternion_from_euler(pi, p, y) # asigno grasppose acorde al quaternio g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # EStablezco la id de este grasp g.id = str(len(grasps)) # Defino los objetos que se pueden tocar g.allowed_touch_objects = allowed_touch_objects # no elimino la fuerza de contacto g.max_contact_force = 0 g.grasp_quality = 1.0 - abs(p) # Adjunto la lista de grasp grasps.append(deepcopy(g)) #Me devuelve la lista return grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # 初始化抓取姿态对象 g = Grasp() # 创建夹爪张开、闭合的姿态 g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) # 设置期望的夹爪靠近、撤离目标的参数 g.pre_grasp_approach = self.make_gripper_translation( 0.05, 0.1, [0.0, 0.0, -1.0]) g.post_grasp_retreat = self.make_gripper_translation( 0.05, 0.1, [0.0, 0.0, 1.0]) # 设置抓取姿态 g.grasp_pose = initial_pose_stamped rot = (initial_pose_stamped.pose.orientation.x, initial_pose_stamped.pose.orientation.y, initial_pose_stamped.pose.orientation.z, initial_pose_stamped.pose.orientation.w) # 需要尝试改变姿态的数据列表 yaw_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3] p = euler_from_quaternion(rot) # 抓取姿态的列表 grasps = [] # 改变姿态,生成抓取动作 for Y in yaw_vals: g.grasp_pose.pose.position.z += 0.18 # 欧拉角到四元数的转换 q = quaternion_from_euler(pi, 0, p[2] + Y) # 设置抓取的姿态 g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # 设置抓取的唯一id号 g.id = str(len(grasps)) # 设置允许接触的物体 g.allowed_touch_objects = allowed_touch_objects # 将本次规划的抓取放入抓取列表中 grasps.append(deepcopy(g)) # 返回抓取列表 return grasps
def handle_graps(req): #this will create an element of type grasp object_name=req.object_name object_pose=Pose() object_pose=Query_object_Pose(object_name) object_dimensions=[0,0,0] #get object dimensions if object_name=="block": object_dimensions=[0.045, 0.045, 0.2] print object_dimensions #graps 1 #create object grasp g = Grasp() g.id=object_name #GRASP POSE: position of object in end efector seen from world grasp_pose = PoseStamped() grasp_pose.header.frame_id = "panda_link0" grasp_pose.pose.position.x = object_pose.position.x -(object_dimensions[0]/2) -0.058-0.02 #(box position- (half box lenght- distance distance b/w panda_link8 and palm of eef (0.058) - some extra padding))) grasp_pose.pose.position.y = object_pose.position.y grasp_pose.pose.position.z = object_pose.position.z #orientation in quaternion q = quaternion_from_euler(-pi/2,-pi/4,-pi/2) grasp_pose.pose.orientation.x = q[0] grasp_pose.pose.orientation.y = q[1] grasp_pose.pose.orientation.z = q[2] grasp_pose.pose.orientation.w = q[3] g.grasp_pose = grasp_pose #set grasp pose #pre-grasp approach g.pre_grasp_approach.direction.header.frame_id = "panda_link0" g.pre_grasp_approach.direction.vector.x = 1.0 g.pre_grasp_approach.min_distance = 0.095 g.pre_grasp_approach.desired_distance = 0.115 openGripperPose(g.pre_grasp_posture) #posture before grasp #posture during grasp closeGripperPose(g.grasp_posture,object_dimensions[0]) #post-grasp retreat g.post_grasp_retreat.direction.header.frame_id = "panda_link0" g.post_grasp_retreat.direction.vector.z = 1.0 g.post_grasp_retreat.min_distance = 0.1 g.post_grasp_retreat.desired_distance= 0.25 #print "Returning grasp [%s , %s , %s]"%(g.grasp_pose.pose.position.x, g.grasp_pose.pose.position.y, g.grasp_pose.pose.position.z) return g
def createGrasp(grasp_pose): grasp = Grasp() grasp.id = "test" grasp.grasp_pose = grasp_pose # grasp.pre_grasp_approach.direction.header.frame_id = "base_link" # grasp.pre_grasp_approach.direction.header.stamp = rospy.Time.now() # grasp.pre_grasp_approach.direction.vector.x = 1.0 # grasp.pre_grasp_approach.direction.vector.y = 0.0 # grasp.pre_grasp_approach.direction.vector.z = 0.0 # grasp.pre_grasp_approach.desired_distance = 0.05 # grasp.pre_grasp_approach.min_distance = 0.01 grasp.pre_grasp_posture.header.frame_id = "base_link" # what link do i need here? grasp.pre_grasp_posture.header.stamp = rospy.Time.now() grasp.pre_grasp_posture.joint_names = [ "hand_right_thumb_joint", "hand_right_index_joint", "hand_right_middle_joint" ] pos = JointTrajectoryPoint() # pre-grasp with thumb down and fingers open pos.positions.append(2.0) pos.positions.append(0.0) pos.positions.append(0.0) grasp.pre_grasp_posture.points.append(pos) grasp.grasp_posture.header.frame_id = "base_link" grasp.grasp_posture.header.stamp = rospy.Time.now() grasp.grasp_posture.joint_names = [ "hand_right_thumb_joint", "hand_right_index_joint", "hand_right_middle_joint" ] pos = JointTrajectoryPoint() # grasp with all closed pos.positions.append(2.0) pos.positions.append(2.0) pos.positions.append(2.0) grasp.grasp_posture.points.append(pos) # grasp.post_grasp_retreat.direction.header.frame_id = "base_link" # grasp.post_grasp_retreat.direction.header.stamp = rospy.Time.now() # grasp.post_grasp_retreat.direction.vector.x = 0.0 # grasp.post_grasp_retreat.direction.vector.y = 0.0 # grasp.post_grasp_retreat.direction.vector.z = 1.0 # grasp.post_grasp_retreat.desired_distance = 0.1 # grasp.post_grasp_retreat.min_distance = 0.01 # grasp.allowed_touch_objects = ["table", "part"] grasp.max_contact_force = 0 return grasp
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # 初始化抓取姿态对象 g = Grasp() # 创建夹爪张开、闭合的姿态 g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) # 设置期望的夹爪靠近、撤离目标的参数 g.pre_grasp_approach = self.make_gripper_translation( 0.05, 0.1, 'ar_marker_0', [0.0, 1.0, 0.0]) g.post_grasp_retreat = self.make_gripper_translation( 0.1, 0.15, 'base_link', [0.0, 0.0, 1.0]) # 设置抓取姿态 g.grasp_pose = initial_pose_stamped # 需要尝试改变姿态的数据列表 yaw_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3] y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] # 抓取姿态的列表 grasps = [] # 改变姿态,生成抓取动作 #for Y in yaw_vals: # 欧拉角到四元数的转换 q = quaternion_from_euler(3.1415, 0, 1.5707) # 设置抓取的姿态 g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # 设置抓取的唯一id号 g.id = str(len(grasps)) # 设置允许接触的物体 g.allowed_touch_objects = allowed_touch_objects # 将本次规划的抓取放入抓取列表中 grasps.append(deepcopy(g)) # 返回抓取列表 return grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_GRASP) # Set the approach and retreat parameters as desired #g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 1.0, -1.0]) #g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [-1.0, -1.0, 1.0]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped ideal_roll = 0 ideal_pitch = 0 ideal_yaw = -1.57 # A list to hold the grasps grasps = [] q = quaternion_from_euler(ideal_roll, ideal_pitch, ideal_yaw) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(ideal_pitch) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps
def get_know_successful_grasp(self): g = Grasp() g.id = "successful_predefined_grasp" gp = PoseStamped() gp.header.frame_id = "camera_color_optical_frame" gp.pose.position.x = 0.183518647951 gp.pose.position.y = -0.23707952283 gp.pose.position.z = 0.493978534979 gp.pose.orientation.w = -0.604815599864 gp.pose.orientation.x = -0.132654186819 gp.pose.orientation.y = 0.698958888788 gp.pose.orientation.z = -0.357851126398 g.grasp_pose = gp return g
def createGrasp(grasp_pose): grasp = Grasp() grasp.id = "test" grasp.grasp_pose = grasp_pose # grasp.pre_grasp_approach.direction.header.frame_id = "base_link" # grasp.pre_grasp_approach.direction.header.stamp = rospy.Time.now() # grasp.pre_grasp_approach.direction.vector.x = 1.0 # grasp.pre_grasp_approach.direction.vector.y = 0.0 # grasp.pre_grasp_approach.direction.vector.z = 0.0 # grasp.pre_grasp_approach.desired_distance = 0.05 # grasp.pre_grasp_approach.min_distance = 0.01 grasp.pre_grasp_posture.header.frame_id = "base_link" # what link do i need here? grasp.pre_grasp_posture.header.stamp = rospy.Time.now() grasp.pre_grasp_posture.joint_names = ["hand_right_thumb_joint", "hand_right_index_joint", "hand_right_middle_joint"] pos = JointTrajectoryPoint() # pre-grasp with thumb down and fingers open pos.positions.append(2.0) pos.positions.append(0.0) pos.positions.append(0.0) grasp.pre_grasp_posture.points.append(pos) grasp.grasp_posture.header.frame_id = "base_link" grasp.grasp_posture.header.stamp = rospy.Time.now() grasp.grasp_posture.joint_names = ["hand_right_thumb_joint", "hand_right_index_joint", "hand_right_middle_joint"] pos = JointTrajectoryPoint() # grasp with all closed pos.positions.append(2.0) pos.positions.append(2.0) pos.positions.append(2.0) grasp.grasp_posture.points.append(pos) # grasp.post_grasp_retreat.direction.header.frame_id = "base_link" # grasp.post_grasp_retreat.direction.header.stamp = rospy.Time.now() # grasp.post_grasp_retreat.direction.vector.x = 0.0 # grasp.post_grasp_retreat.direction.vector.y = 0.0 # grasp.post_grasp_retreat.direction.vector.z = 1.0 # grasp.post_grasp_retreat.desired_distance = 0.1 # grasp.post_grasp_retreat.min_distance = 0.01 # grasp.allowed_touch_objects = ["table", "part"] grasp.max_contact_force = 0 return grasp
def make_grasps(self, initial_pose_stamped, allowed_touch_objects, pre, post, set_rpy=0): g = Grasp() g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) g.pre_grasp_approach = self.make_gripper_translation( pre[0], pre[1], pre[2]) g.post_grasp_retreat = self.make_gripper_translation( post[0], post[1], post[2]) g.grasp_pose = initial_pose_stamped roll_vals = [1.57] yaw_vals = [0, 0.2, -0.2, 0.4, -0.4, 0.6, -0.6] pitch_vals = [-1.57, -1.47, -1.67, -1.37, -1.77] z_vals = [0] grasps = [] for y in yaw_vals: for p in pitch_vals: for z in z_vals: for r in roll_vals: if set_rpy: q = quaternion_from_euler(r, p, y) g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] g.grasp_pose.pose.position.z = initial_pose_stamped.pose.position.z + z g.id = str(len(grasps)) g.allowed_touch_objects = allowed_touch_objects g.max_contact_force = 0 g.grasp_quality = 1.0 - abs(p) grasps.append(deepcopy(g)) return grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation( 0.05, 0.15, [0.0, -1.0, 0.0]) g.post_grasp_retreat = self.make_gripper_translation( 0.05, 0.1, [0.0, 1.0, 0.0]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3] # Yaw angles to try yaw_vals = [0] # A list to hold the grasps g.grasp_pose.pose.orientation = Quaternion(0.606301648371, 0.599731279995, 0.381153346104, 0.356991358063) # Set and id for this grasp (simply needs to be unique) g.id = str(len(yaw_vals)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 grasps = [g] # Return the list return grasps
def make_grasps(self, initial_pose, allow_touch_objects): # initialise a grasp object g = Grasp() g.pre_grasp_posture = self.make_grab_posture(self.open_joint_values) g.grasp_posture = self.make_grab_posture(self.closed_joint_values) g.pre_grasp_approach = self.make_grab_translation(0.01, 0.1, [0.0, 0.0, 1.0]) g.post_grasp_retreat = self.make_grab_translation(0.1, 0.15, [0.0, 0.0, 1.0]) g.grasp_pose = initial_pose # pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.4, -0.4] # # target_pose_arm_ref = self.tf_buffer.transform(initial_pose,'hand_iiwa_link_0') # x = target_pose_arm_ref.pose.position.x # y = target_pose_arm_ref.pose.position.y # yaw_vals = [math.atan2(y,x) + inc for inc in [0, 0.1, -0.1]] # grasps = [] # # for yaw in yaw_vals: # for pitch in pitch_vals: # q = quaternion_from_euler(0,pitch,yaw) # # g.grasp_pose.pose.orientation.x = q[0] # g.grasp_pose.pose.orientation.y = q[1] # g.grasp_pose.pose.orientation.z = q[2] # g.grasp_pose.pose.orientation.w = q[3] g.id = str(len(grasps)) g.allowed_touch_objects = allow_touch_objects g.max_contact_force = 0 g.grasp_quality = 1.0 #- abs(pitch) grasps.append(g) return grasps
def createGrasp(grasp_pose): grasp = Grasp() grasp.id = "grasp_test" grasp.grasp_pose = grasp_pose grasp.pre_grasp_posture.header.frame_id = "base_link" grasp.pre_grasp_posture.header.stamp = rospy.Time.now() grasp.pre_grasp_posture.joint_names = ["right_gripper_joint"] pos = JointTrajectoryPoint() # pre-grasp with thumb down and fingers open pos.positions.append(0.0) grasp.pre_grasp_posture.points.append(pos) grasp.grasp_posture.header.frame_id = "base_link" grasp.grasp_posture.header.stamp = rospy.Time.now() grasp.grasp_posture.joint_names = ["right_gripper_joint"] pos = JointTrajectoryPoint() # grasp with all closed pos.positions.append(.008) grasp.grasp_posture.points.append(pos) grasp.max_contact_force = 0 return grasp
def create_grasp(self, pose, approach, retreat, grasp_width, grasp_id=''): g = Grasp() g.id = grasp_id g.grasp_pose.header.frame_id = 'base_footprint' g.grasp_pose.pose = pose # set direction of approach and retreat pre and post grasping, respectively g.pre_grasp_approach = self.create_gripper_translation( approach, 0.095, 0.9) g.post_grasp_retreat = self.create_gripper_translation( retreat, 0.1, 0.2) print('grasp width : ', grasp_width) gripper_postures = self.open_and_close_gripper(grasp_width) g.pre_grasp_posture = gripper_postures[ 'pre_grasp_posture'] # open gripper before grasp g.grasp_posture = gripper_postures[ 'grasp_posture'] # close gripper during grasp g.max_contact_force = self._max_contact_force # don't knock the object down whilst grasping return g
def make_grasps(self): # setup defaults for the grasp g = Grasp() # These two lines are for gripper open and close. g.pre_grasp_posture = self.make_gripper_posture(0) # They don't matter in current simulation. g.grasp_posture = self.make_gripper_posture(0) g.pre_grasp_approach = self.make_gripper_translation(0.05, 0.1) g.post_grasp_retreat = self.make_gripper_translation(0.05, 0.1, 1.0) g.grasp_pose = self.group.get_current_pose() # generate list of grasps grasps = [] g.grasp_pose.pose.orientation.x = self.q[0] g.grasp_pose.pose.orientation.y = self.q[1] g.grasp_pose.pose.orientation.z = self.q[2] g.grasp_pose.pose.orientation.w = self.q[3] g.id = str(len(grasps)) g.allowed_touch_objects = [self.req.model_name_1] g.max_contact_force = 0 grasps.append(copy.deepcopy(g)) print 'Successfully created grasps!' return grasps
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # Initialize the grasp object g = Grasp() #g.grasp_pose = initial_pose_stamped #q0 = g.grasp_pose.pose.orientation.w #q1 = g.grasp_pose.pose.orientation.x #q2 = g.grasp_pose.pose.orientation.y #q3 = g.grasp_pose.pose.orientation.z #eulerR = atan2(2*(q0*q1+q2*q3),1-2*(q1*q1+q2*q2)) #eulerP = asin(2*(q0*q2-q1*q3)) #eulerY = atan2(2*(q0*q3+q1*q2),1-2*(q2*q2+q3*q3)) #print(eulerR) #print(eulerY) #print(eulerP) # Set the pre-grasp and grasp postures appropriately g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) # Set the approach and retreat parameters as desired g.pre_grasp_approach = self.make_gripper_translation( 0.01, 0.1, [1.0, 0.0, 0.0]) g.post_grasp_retreat = self.make_gripper_translation( 0.1, 0.15, [0.0, 0, 1.7]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped # Pitch angles to try pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3, 0.5, 0.4, 0.6] # Yaw angles to try yaw_vals = [0] # Roll angles to try roll_vals = [-3.14, 0] # A list to hold the grasps grasps = [] # Generate a grasp for each pitch and yaw angle\ for r in roll_vals: for y in yaw_vals: for p in pitch_vals: # Create a quaternion from the Euler angles q = quaternion_from_euler(r, p, y) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(p) # Append the grasp to the list grasps.append(deepcopy(g)) # Return the list return grasps
#right_arm.set_named_target("r_start") #right_arm.go() #rospy.sleep(1) #right_arm.set_random_target() #right_arm.go() #rospy.sleep(1) #right_arm.set_position_target([.75,-0.3, 1]) #right_arm.go() #rospy.sleep(1) grasps = [] g = Grasp() g.id = "test" grasp_pose = PoseStamped() grasp_pose.header.frame_id = "base_link" grasp_pose.pose.position.x = 0.47636 grasp_pose.pose.position.y = -0.21886 grasp_pose.pose.position.z = 0.7164 grasp_pose.pose.orientation.x = 0.00080331 grasp_pose.pose.orientation.y = 0.001589 grasp_pose.pose.orientation.z = -2.4165e-06 grasp_pose.pose.orientation.w = 1 rospy.logwarn("moving to arm") right_arm.set_pose_target(grasp_pose) right_arm.go() rospy.sleep(3)
def create_grasp(self, pose, grasp_id): """ :type pose: Pose pose of the gripper for the grasp :type grasp_id: str name for the grasp :rtype: Grasp """ g = Grasp() g.id = grasp_id pre_grasp_posture = JointTrajectory() pre_grasp_posture.header.frame_id = self._grasp_postures_frame_id pre_grasp_posture.joint_names = [ name for name in self._gripper_joint_names.split()] jtpoint = JointTrajectoryPoint() jtpoint.positions = [ float(pos) for pos in self._gripper_pre_grasp_positions.split()] jtpoint.time_from_start = rospy.Duration(self._time_pre_grasp_posture) pre_grasp_posture.points.append(jtpoint) grasp_posture = copy.deepcopy(pre_grasp_posture) grasp_posture.points[0].time_from_start = rospy.Duration( self._time_pre_grasp_posture + self._time_grasp_posture) jtpoint2 = JointTrajectoryPoint() jtpoint2.positions = [ float(pos) for pos in self._gripper_grasp_positions.split()] jtpoint2.time_from_start = rospy.Duration( self._time_pre_grasp_posture + self._time_grasp_posture + self._time_grasp_posture_final) grasp_posture.points.append(jtpoint2) g.pre_grasp_posture = pre_grasp_posture g.grasp_posture = grasp_posture header = Header() header.frame_id = self._grasp_pose_frame_id # base_footprint q = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] # Fix orientation from gripper_link to parent_link (tool_link) fix_tool_to_gripper_rotation_q = quaternion_from_euler( math.radians(self._fix_tool_frame_to_grasping_frame_roll), math.radians(self._fix_tool_frame_to_grasping_frame_pitch), math.radians(self._fix_tool_frame_to_grasping_frame_yaw) ) q = quaternion_multiply(q, fix_tool_to_gripper_rotation_q) fixed_pose = copy.deepcopy(pose) fixed_pose.orientation = Quaternion(*q) g.grasp_pose = PoseStamped(header, fixed_pose) g.grasp_quality = self._grasp_quality g.pre_grasp_approach = GripperTranslation() g.pre_grasp_approach.direction.vector.x = self._pre_grasp_direction_x # NOQA g.pre_grasp_approach.direction.vector.y = self._pre_grasp_direction_y # NOQA g.pre_grasp_approach.direction.vector.z = self._pre_grasp_direction_z # NOQA g.pre_grasp_approach.direction.header.frame_id = self._grasp_postures_frame_id # NOQA g.pre_grasp_approach.desired_distance = self._grasp_desired_distance # NOQA g.pre_grasp_approach.min_distance = self._grasp_min_distance g.post_grasp_retreat = GripperTranslation() g.post_grasp_retreat.direction.vector.x = self._post_grasp_direction_x # NOQA g.post_grasp_retreat.direction.vector.y = self._post_grasp_direction_y # NOQA g.post_grasp_retreat.direction.vector.z = self._post_grasp_direction_z # NOQA g.post_grasp_retreat.direction.header.frame_id = self._grasp_postures_frame_id # NOQA g.post_grasp_retreat.desired_distance = self._grasp_desired_distance # NOQA g.post_grasp_retreat.min_distance = self._grasp_min_distance g.max_contact_force = self._max_contact_force g.allowed_touch_objects = self._allowed_touch_objects return g
# right_arm.set_named_target("r_start") # right_arm.go() # rospy.sleep(1) # right_arm.set_random_target() # right_arm.go() # rospy.sleep(1) # right_arm.set_position_target([.75,-0.3, 1]) # right_arm.go() # rospy.sleep(1) grasps = [] g = Grasp() g.id = "test" grasp_pose = PoseStamped() grasp_pose.header.frame_id = "base_link" grasp_pose.pose.position.x = 0.47636 grasp_pose.pose.position.y = -0.21886 grasp_pose.pose.position.z = 0.7164 grasp_pose.pose.orientation.x = 0.00080331 grasp_pose.pose.orientation.y = 0.001589 grasp_pose.pose.orientation.z = -2.4165e-06 grasp_pose.pose.orientation.w = 1 rospy.logwarn("moving to arm") right_arm.set_pose_target(grasp_pose) right_arm.go() rospy.sleep(3)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) #right_arm.set_planner_id("KPIECEkConfigDefault"); right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") right_arm.set_named_target("right_start") right_arm.go() right_gripper.set_named_target("right_gripper_open") right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table p.pose.position.x = 0.42 p.pose.position.y = -0.2 p.pose.position.z = 0.3 scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.40 p.pose.position.y = -0.0 p.pose.position.z = 0.85 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.37636 start_pose.pose.position.y = 0.21886 start_pose.pose.position.z = 0.9 start_pose.pose.orientation.x = 0.00080331 start_pose.pose.orientation.y = 0.001589 start_pose.pose.orientation.z = -2.4165e-06 start_pose.pose.orientation.w = 1 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps grasps = self.make_grasps(start_pose) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part", grasps) n_attempts += 1 print "Attempts: ", n_attempts rospy.sleep(0.3) rospy.spin() roscpp_shutdown()
def grasp_array(cls, msg, obj): # for me... for i in range(msg['_length_grasp']): gsp = Grasp() # direct descendants of grasp: gsp.id = msg["%s_id" % i] gsp.grasp_quality = msg["%s_grasp_quality" % i] gsp.max_contact_force = msg["%s_max_contact_force" % i] gsp.allowed_touch_objects = msg["%s_allowed_touch_objects" % i] # descendants of pre_grasp_posture: gsp.pre_grasp_posture.header = \ decode.header(msg, gsp.pre_grasp_posture.header, "%s_pre" % i) gsp.pre_grasp_posture.joint_names = msg['%s_prejoint_names' % i] for j in range(msg['%s_length_points1' % i]): jtp1 = JointTrajectoryPoint() jtp1 = decode.grasp_points(msg, jtp1, "%s_%s_pre" % (i, j)) gsp.pre_grasp_posture.points.append(jtp1) # descendants of grasp_posture: gsp.grasp_posture.header = \ decode.header(msg, gsp.grasp_posture.header, "%s_posture" % i) gsp.grasp_posture.joint_names = msg["%s_joint_names" % i] for j in range(msg['%s_length_points2' % i]): jtp2 = JointTrajectoryPoint() jtp2 = decode.grasp_points(msg, jtp2, "%s_%s_posture" % (i, j)) gsp.grasp_posture.points.append(jtp2) # descendants of grasp_pose: gsp.grasp_pose.header = \ decode.header(msg, gsp.grasp_pose.header, "%s_pose" % i) gsp.grasp_pose.pose.position = \ decode.position(msg, gsp.grasp_pose.pose.position, "%s_pose" % i) gsp.grasp_pose.pose.orientation = \ decode.orientation(msg, gsp.grasp_pose.pose.orientation, "%s_pose" % i) # descendants of pre_grasp_approach: gsp.pre_grasp_approach.direction.header = \ decode.header(msg, gsp.pre_grasp_approach.direction.header, "%s_app" % i) gsp.pre_grasp_approach.direction.vector = \ decode.vector(msg, gsp.pre_grasp_approach.direction.vector, "%s_app" % i) gsp.pre_grasp_approach.desired_distance = \ msg['%s_appdesired_distance' % i] gsp.pre_grasp_approach.min_distance = msg['%s_appmin_distance' % i] # descendants of post_grasp_retreat: gsp.post_grasp_retreat.direction.header = \ decode.header(msg, gsp.post_grasp_retreat.direction.header, "%s_ret" % i) gsp.post_grasp_retreat.direction.vector = \ decode.vector(msg, gsp.post_grasp_retreat.direction.vector, "%s_ret" % i) gsp.post_grasp_retreat.desired_distance = \ msg['%s_retdesired_distance' % i] gsp.post_grasp_retreat.min_distance = msg['%s_retmin_distance' % i] # descendants of post_place_retreat gsp.post_place_retreat.direction.header = \ decode.header(msg, gsp.post_place_retreat.direction.header, '%s_place' % i) gsp.post_place_retreat.direction.vector = \ decode.vector(msg, gsp.post_place_retreat.direction.vector, '%s_place' % i) gsp.post_place_retreat.desired_distance = \ msg['%s_pldesired_distance' % i] gsp.post_place_retreat.min_distance = msg['%s_plmin_distance' % i] # append Grasp to GraspArray: obj.grasps.append(gsp) return(obj)
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander(GROUP_NAME_ARM) #right_arm.set_planner_id("KPIECEkConfigDefault"); right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object(GRIPPER_FRAME, "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") #right_arm.set_named_target("r_start") #right_arm.go() #right_gripper.set_named_target("right_gripper_open") #right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.7 p.pose.position.y = -0.2 p.pose.position.z = 0.85 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.47636 start_pose.pose.position.y = -0.21886 start_pose.pose.position.z = 0.9 start_pose.pose.orientation.x = 0.00080331 start_pose.pose.orientation.y = 0.001589 start_pose.pose.orientation.z = -2.4165e-06 start_pose.pose.orientation.w = 1 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps grasps = self.make_grasps(start_pose) result = False n_attempts = 0 # repeat until will succeed while result == False: result = robot.right_arm.pick("part", grasps) n_attempts += 1 print "Attempts: ", n_attempts rospy.sleep(0.3) rospy.spin() roscpp_shutdown()
def generate_grasp_msgs(self, selected_grasps): self.grasps = [] formatted_grasps = [] self.grasps_cartesian = [] formatted_grasps_cartesian = [] tot_grasps = len(selected_grasps) cont = 0 filtered_orientation = 0 for i in range(0, len(selected_grasps)): z_axis_unit = (0, 0, -1) ap_axis = (selected_grasps[i].approach.x, selected_grasps[i].approach.y, selected_grasps[i].approach.z) angle = numpy.dot(z_axis_unit, ap_axis) if (angle >= 0): # filter it out, because grasp coming from below the ground filtered_orientation += 1 perror( repr(filtered_orientation) + " Grasp filtered because coming from underneath the ground" ) continue tf_listener_.waitForTransform('/arm_camera_depth_optical_frame', '/summit_xl_base_footprint', rospy.Time(), rospy.Duration(2.0)) g = Grasp() g.id = "dupa" gp = PoseStamped() gp.header.frame_id = "arm_camera_depth_optical_frame" org_q = self.trans_matrix_to_quaternion(selected_grasps[i]) quat = org_q gp.pose.position.x = selected_grasps[ i].surface.x + self.grasp_offset * selected_grasps[i].approach.x gp.pose.position.y = selected_grasps[ i].surface.y + self.grasp_offset * selected_grasps[i].approach.y gp.pose.position.z = selected_grasps[ i].surface.z + self.grasp_offset * selected_grasps[i].approach.z gp.pose.orientation.x = float(quat.elements[1]) gp.pose.orientation.y = float(quat.elements[2]) gp.pose.orientation.z = float(quat.elements[3]) gp.pose.orientation.w = float(quat.elements[0]) translated_pose = tf_listener_.transformPose( "summit_xl_base_footprint", gp) g.grasp_pose = translated_pose #gp g.pre_grasp_approach.direction.header.frame_id = "arm_ee_link" g.pre_grasp_approach.direction.vector.x = 1.0 g.pre_grasp_approach.min_distance = 0.06 g.pre_grasp_approach.desired_distance = 0.1 #g.allowed_touch_objects = ["<octomap>", "obj"] g.allowed_touch_objects = ["obj"] #g.max_contact_force = 0.0 g.grasp_quality = selected_grasps[0].score.data formatted_grasps.append(g) # Add config for cartesian pick gp_cartesian = PoseStamped() gp_cartesian.header.frame_id = "arm_camera_depth_optical_frame" gp_cartesian.pose.position.x = selected_grasps[ i].surface.x + self.grasp_offset_cartesian * selected_grasps[ i].approach.x gp_cartesian.pose.position.y = selected_grasps[ i].surface.y + self.grasp_offset_cartesian * selected_grasps[ i].approach.y gp_cartesian.pose.position.z = selected_grasps[ i].surface.z + self.grasp_offset_cartesian * selected_grasps[ i].approach.z gp_cartesian.pose.orientation.x = float(quat.elements[1]) gp_cartesian.pose.orientation.y = float(quat.elements[2]) gp_cartesian.pose.orientation.z = float(quat.elements[3]) gp_cartesian.pose.orientation.w = float(quat.elements[0]) translated_pose = tf_listener_.transformPose( "summit_xl_base_footprint", gp_cartesian) g_cartesian = Grasp() g_cartesian.id = "cart" g_cartesian.grasp_pose = translated_pose g_cartesian.allowed_touch_objects = ["obj"] formatted_grasps_cartesian.append(g_cartesian) # Sort grasps using z (get higher grasps first) formatted_grasps.sort( key=lambda grasp: grasp.grasp_pose.pose.position.z, reverse=True) formatted_grasps_cartesian.sort( key=lambda grasp: grasp.grasp_pose.pose.position.z, reverse=True) return formatted_grasps, formatted_grasps_cartesian
def make_grasps(self, initial_pose_stamped, allowed_touch_objects): # Initialize the grasp object g = Grasp() # Set the pre-grasp and grasp postures appropriately g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) g.grasp_posture = self.make_gripper_posture(GRIPPER_GRASP) # Set the approach and retreat parameters as desired #g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 1.0, -1.0]) #g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [-1.0, -1.0, 1.0]) # Set the first grasp pose to the input pose g.grasp_pose = initial_pose_stamped ideal_roll = 3 #-1.57 #pi/2 ideal_pitch = 0 ideal_yaw = 1.57 step_size = 0.05 idx = 0.3 idx_roll = ideal_roll + idx idx_pitch = ideal_pitch + idx idx_yaw = ideal_yaw + idx roll_vals = [] pitch_vals = [] yaw_vals = [] while idx >= -0.3: roll_vals.append(idx_roll) pitch_vals.append(idx_pitch) yaw_vals.append(idx_yaw) idx -= step_size idx_roll -= step_size idx_pitch -= step_size idx_yaw -= step_size # A list to hold the grasps grasps = [] print "Generating Poses" # Generate a grasp for each roll pitch and yaw angle for r in roll_vals: for y in yaw_vals: for p in pitch_vals: # Create a quaternion from the Euler angles q = quaternion_from_euler(r, p, y) # Set the grasp pose orientation accordingly g.grasp_pose.pose.orientation.x = q[0] g.grasp_pose.pose.orientation.y = q[1] g.grasp_pose.pose.orientation.z = q[2] g.grasp_pose.pose.orientation.w = q[3] # Set and id for this grasp (simply needs to be unique) g.id = str(len(grasps)) # Set the allowed touch objects to the input list g.allowed_touch_objects = allowed_touch_objects # Don't restrict contact force g.max_contact_force = 0 # Degrade grasp quality for increasing pitch angles g.grasp_quality = 1.0 - abs(p) # Append the grasp to the list grasps.append(deepcopy(g)) print "Generated " + g.id + " poses" # Return the list return grasps
def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('moveit_py_demo', anonymous=True) scene = PlanningSceneInterface() robot = RobotCommander() right_arm = MoveGroupCommander("right_arm") right_gripper = MoveGroupCommander("right_gripper") eef = right_arm.get_end_effector_link() rospy.sleep(2) scene.remove_attached_object("right_gripper_link", "part") # clean the scene scene.remove_world_object("table") scene.remove_world_object("part") right_arm.set_named_target("start1") right_arm.go() right_gripper.set_named_target("open") right_gripper.go() rospy.sleep(1) # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() # add a table #p.pose.position.x = 0.42 #p.pose.position.y = -0.2 #p.pose.position.z = 0.3 #scene.add_box("table", p, (0.5, 1.5, 0.6)) # add an object to be grasped p.pose.position.x = 0.15 p.pose.position.y = -0.12 p.pose.position.z = 0.7 scene.add_box("part", p, (0.07, 0.01, 0.2)) rospy.sleep(1) g = Grasp() g.id = "test" start_pose = PoseStamped() start_pose.header.frame_id = FIXED_FRAME # start the gripper in a neutral pose part way to the target start_pose.pose.position.x = 0.0130178 start_pose.pose.position.y = -0.125155 start_pose.pose.position.z = 0.597653 start_pose.pose.orientation.x = 0.0 start_pose.pose.orientation.y = 0.388109 start_pose.pose.orientation.z = 0.0 start_pose.pose.orientation.w = 0.921613 right_arm.set_pose_target(start_pose) right_arm.go() rospy.sleep(2) # generate a list of grasps #grasps = self.make_grasps(start_pose) #result = False #n_attempts = 0 # repeat until will succeed #while result == False: #result = robot.right_arm.pick("part", grasps) #n_attempts += 1 #print "Attempts: ", n_attempts #rospy.sleep(0.2) rospy.spin() roscpp_shutdown()