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.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(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 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(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(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(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(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
