def make_gripper_translation(self, min_dist, desired, axis=1.0): g = GripperTranslation() g.direction.vector.x = axis g.direction.header.frame_id = GRIPPER_FRAME g.min_distance = min_dist g.desired_distance = desired return g
def make_gripper_translation(self, min_dist, desired, axis=-1.0): g = GripperTranslation() g.direction.vector.z = axis g.direction.header.frame_id = 'right_l6' g.min_distance = min_dist g.desired_distance = desired return g
def make_gripper_translation(self, min_dist, desired, vector): g = GripperTranslation() g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] g.direction.header.frame_id = "panda_link0" g.min_distance = min_dist g.desired_distance = desired return g
def create_gripper_translation(direction_vector, desired_distance=0.15, min_distance=0.01): g_trans = GripperTranslation() g_trans.direction.header.frame_id = REFERENCE_LINK g_trans.direction.header.stamp = rospy.Time.now() g_trans.direction.vector.x = direction_vector.x g_trans.direction.vector.y = direction_vector.y g_trans.direction.vector.z = direction_vector.z g_trans.desired_distance = desired_distance g_trans.min_distance = min_distance return g_trans
def create_gripper_translation(direction_vector, desired_distance=0.10, min_distance=0.05): gt = GripperTranslation() gt.direction.header.frame_id = REFERENCE_FRAME gt.direction.header.stamp = rospy.Time.now() gt.direction.vector.x = direction_vector.x gt.direction.vector.y = direction_vector.y gt.direction.vector.z = direction_vector.z gt.desired_distance = desired_distance gt.min_distance = min_distance return gt
def make_grab_translation(self, min_dist, desired, vector): g = GripperTranslation() g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] g.direction.header.frame_id = 'lh_palm' g.min_distance = min_dist g.desired_distance = desired return g
def createGripperTranslation(direction_vector, desired_distance=0.15, min_distance=0.01): """Returns a GripperTranslation message with the direction_vector and desired_distance and min_distance in it. Intended to be used to fill the pre_grasp_approach and post_grasp_retreat field in the Grasp message.""" g_trans = GripperTranslation() g_trans.direction.header.frame_id = "base_link" g_trans.direction.header.stamp = rospy.Time.now() g_trans.direction.vector.x = direction_vector.x g_trans.direction.vector.y = direction_vector.y g_trans.direction.vector.z = direction_vector.z g_trans.desired_distance = desired_distance g_trans.min_distance = min_distance return g_trans
def create_gripper_translation(self, direction_vector, min_distance, desired_distance, frame_id='base_footprint'): # used to generate pre-grasp approach and post-grasp retreat for grasps g_trans = GripperTranslation() g_trans.direction.header.frame_id = frame_id g_trans.direction.vector = direction_vector g_trans.min_distance = min_distance g_trans.desired_distance = desired_distance return g_trans
def make_gripper_translation(min_dist, desired, vector): #Inicio el objeto grippert translation g = GripperTranslation() # Defino las componentes del vector g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] #EL vector es relativo al gripper_frame g.direction.header.frame_id = GRIPPER_FRAME #Defino la distancia minima y deseada al objetivo g.min_distance = min_dist g.desired_distance = desired return g
def get_gripper_translation(direction,desired_dis, min_dis,reference_frame): """ Return "pre_grasp_approach"/"post_grasp_retreat"/"post_place_retreat" for the moveit_msgs/Grasp input: direction of translation desired_dis: distance should translate in that direction min_dis: minimum distance to consider before changing grasp posture """ translation=GripperTranslation() translation.direction.header.frame_id=reference_frame #directions defined with respect of this reference frame translation.direction.vector.x=direction[0] translation.direction.vector.y=direction[1] translation.direction.vector.z=direction[2] translation.desired_distance=desired_dis translation.min_distance=min_dis return translation
def make_gripper_translation(self, min_dist, desired, vector): # Initialize the gripper translation object g = GripperTranslation() # Set the direction vector components to the input g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] # The vector is relative to the gripper frame g.direction.header.frame_id = GRIPPER_FRAME # Assign the min and desired distances from the input g.min_distance = min_dist g.desired_distance = desired return g
def make_gripper_translation(self, min_dist, desired, vector): # 初始化translation对象 g = GripperTranslation() # 设置方向向量 g.direction.vector.x = vector[0] g.direction.vector.y = vector[1] g.direction.vector.z = vector[2] # 设置参考坐标系 g.direction.header.frame_id = GRIPPER_FRAME # 设置最小和期望的距离 g.min_distance = min_dist g.desired_distance = desired return g
def _make_gripper_translation(frame_id, direction, desired_distance, min_distance=None): return GripperTranslation(direction=_make_vector_stamped( frame_id, direction), desired_distance=desired_distance, min_distance=min_distance or desired_distance * 0.6)
def create_grasp_2(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 fixed_pose = copy.deepcopy(pose) 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 = 0.13 # 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
def plan_point_cluster_grasps(self, target, arm_name): rospy.loginfo("Probabilistic Version! ") error_code = GraspPlanningErrorCode() grasps = [] #get the hand joint names from the param server (loaded from yaml config file) #hand_description = rospy.get_param('hand_description') hand_description = rospy.get_param('~hand_description') pregrasp_joint_angles_dict = rospy.get_param('~pregrasp_joint_angles') grasp_joint_angles_dict = rospy.get_param('~grasp_joint_angles') pregrasp_joint_efforts_dict = rospy.get_param( '~pregrasp_joint_efforts') grasp_joint_efforts_dict = rospy.get_param('~grasp_joint_efforts') if not arm_name: arm_name = hand_description.keys()[0] rospy.logerr( "point cluster grasp planner: missing arm_name in request! Using " + arm_name) try: hand_joints = hand_description[arm_name]["hand_joints"] except KeyError: arm_name = hand_description.keys()[0] rospy.logerr("arm_name " + arm_name + " not found! Using joint names from " + arm_name) try: hand_joints = hand_description[arm_name]["hand_joints"] except KeyError: rospy.logerr("no hand joints found for %s!" % arm_name) return ([], error_code.OTHER_ERROR) try: hand_frame = hand_description[arm_name]["hand_frame"] hand_approach_direction = hand_description[arm_name][ "hand_approach_direction"] except KeyError: rospy.logerr( "couldn't find hand_frame or hand_approach_direction!") return ([], error_code.OTHER_ERROR) pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name] grasp_joint_angles = grasp_joint_angles_dict[arm_name] pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name] grasp_joint_efforts = grasp_joint_efforts_dict[arm_name] #hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints') rospy.loginfo("hand_joints:" + str(hand_joints)) #find the cluster bounding box and relevant frames, and transform the cluster init_start_time = time.time() if len(target.cluster.points) > 0: if (target.use_probability): #probabilistic if (len(target.probabilities) > 0): self.pcgp.init_cluster_grasper(target.cluster, target.probabilities, use_probability=True) else: #weighted probabilistic self.pcgp.init_cluster_grasper(target.cluster, probabilities=[], use_probability=True) else: #deterministic self.pcgp.init_cluster_grasper(target.cluster, probabilities=[], use_probability=False) cluster_frame = target.cluster.header.frame_id self.cluster_frame = cluster_frame else: self.pcgp.init_cluster_grasper(target.region.cloud) cluster_frame = target.region.cloud.header.frame_id if len(cluster_frame) == 0: rospy.logerr("region.cloud.header.frame_id was empty!") error_code.value = error_code.OTHER_ERROR return (grasps, error_code) init_end_time = time.time() #print "init time: %.3f"%(init_end_time - init_start_time) #plan grasps for the cluster (returned in the cluster frame) grasp_plan_start_time = time.time() (pregrasp_poses, grasp_poses, gripper_openings, qualities, pregrasp_dists) = self.pcgp.plan_point_cluster_grasps() grasp_plan_end_time = time.time() #print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time) #add error code to service error_code.value = error_code.SUCCESS grasp_list = [] if pregrasp_poses == None: error_code.value = error_code.OTHER_ERROR return (grasps, error_code) #fill in the list of grasps for (grasp_pose, quality, pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists): pre_grasp_joint_state = self.create_joint_trajectory( hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts) grasp_joint_state = self.create_joint_trajectory( hand_joints, grasp_joint_angles, grasp_joint_efforts) #if the cluster isn't in the same frame as the graspable object reference frame, #transform the grasps to be in the reference frame if cluster_frame == target.reference_frame_id: transformed_grasp_pose = stamp_pose(grasp_pose, cluster_frame) else: transformed_grasp_pose = change_pose_stamped_frame( self.pcgp.tf_listener, stamp_pose(grasp_pose, cluster_frame), target.reference_frame_id) if self.pcgp.pregrasp_just_outside_box: min_approach_distance = pregrasp_dist else: min_approach_distance = max(pregrasp_dist - .05, .05) approach = GripperTranslation( create_vector3_stamped(hand_approach_direction, hand_frame), pregrasp_dist, min_approach_distance) retreat = GripperTranslation( create_vector3_stamped( [-1. * x for x in hand_approach_direction], hand_frame), pregrasp_dist, min_approach_distance) # LT grasp_list.append( Grasp(id="id", pre_grasp_posture=pre_grasp_joint_state, grasp_posture=grasp_joint_state, grasp_pose=transformed_grasp_pose, grasp_quality=quality, pre_grasp_approach=approach, post_grasp_retreat=retreat, max_contact_force=-1)) #if requested, randomize the first few grasps if self.randomize_grasps: first_grasps = grasp_list[:30] random.shuffle(first_grasps) shuffled_grasp_list = first_grasps + grasp_list[30:] grasps = shuffled_grasp_list else: grasps = grasp_list return (grasps, error_code)
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