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, 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:
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 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 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 _pick_box(self):
# Construct the grasp
p = PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
# TODO: Clean this
p.pose.position.x = self.box_x
p.pose.position.y = self.box_y
p.pose.position.z = self.box_z # old value = 0.85
p.pose.orientation.w = 1.0
g = Grasp()
g.grasp_pose = p
# Construct pre-grasp
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.header.frame_id = "l_wrist_roll_link"
g.pre_grasp_approach.min_distance = 0.2
g.pre_grasp_approach.desired_distance = 0.4
g.pre_grasp_posture = self._open_gripper(g.pre_grasp_posture)
g.grasp_posture = self._closed_gripper(g.grasp_posture)
grasps = []
grasps.append(g)
self.group.set_support_surface_name("table")
self.group.set_planner_id("RRTkConfigDefault")
print("EXECUTING PICK")
self.group.pick("box", grasps)
rospy.sleep(3)
return True
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, 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 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 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 pick(robot):
# Construct the grasp
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.34
p.pose.position.y = -0.7
p.pose.position.z = 0.5
p.pose.orientation.w = 1.0
g = Grasp()
g.grasp_pose = p
# Construct pre-grasp
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.header.frame_id = "r_wrist_roll_link"
g.pre_grasp_approach.min_distance = 0.2
g.pre_grasp_approach.desired_distance = 0.4
# Construct post-grasp
g.post_grasp_retreat.direction.header.frame_id = robot.get_planning_frame()
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
g.pre_grasp_posture = open_gripper(g.pre_grasp_posture)
g.grasp_posture = closed_gripper(g.grasp_posture)
grasps = []
grasps.append(g)
group = robot.get_group("right_arm")
group.set_support_surface_name("table")
group.pick("part", 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 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 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 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(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):
# 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 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 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 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 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 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()
# 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 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
grasp_pose.pose.position.x = 0.35
grasp_pose.pose.position.y = -0
grasp_pose.pose.position.z = 0.76
grasp_pose.pose.orientation.x = -0.0209083116076
grasp_pose.pose.orientation.y = -0.00636455547831
grasp_pose.pose.orientation.z = 0.0170413352124
grasp_pose.pose.orientation.w = 0.999615890147
rospy.logwarn("moving to arm")
right_arm.set_pose_target(grasp_pose)
right_arm.go()
rospy.sleep(1)
# set the grasp pose
g.grasp_pose = grasp_pose
# define the pre-grasp approach
g.pre_grasp_approach.direction.header.frame_id = "base_footprint"
g.pre_grasp_approach.direction.vector.x = 0.4
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.001
g.pre_grasp_approach.desired_distance = 0.1
g.pre_grasp_posture.header.frame_id = "right_gripper_link"
g.pre_grasp_posture.joint_names = ["right_arm_gripper_joint"]
pos = JointTrajectoryPoint()
pos.positions.append(0.0)
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
##quit()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("left_arm")
group.set_start_state_to_current_state()
left = baxter_interface.Gripper('left')
left.calibrate()
# === RANDOM ===
'''
group.set_random_target()
group.set_planning_time(20)
plan = group.plan()
group.go()
rospy.sleep(3)
print "\n close"
left.close()
rospy.sleep(2)
print "\n open"
left.open()
quit()
'''
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
#print "============ Waiting for RVIZ..."
#rospy.sleep(10)
print "============ Starting tutorial "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
print robot.has_group("left_hand")
left.open()
#rospy.sleep(1)
## Now, we call the planner to compute the plan
## and visualize it if successful
## Note that we are just planning, not asking move_group
## to actually move the robot
p = PoseStamped()
p.header.frame_id = "/base"
p.pose.position.x = 0.85
p.pose.position.y = 0.3
p.pose.position.z = -0.1
#scene.add_box("cube", p, (0.05, 0.05, 0.05))
p.pose.position.y = 0.5
p.pose.position.z = -0.3
#scene.add_box("table", p, (0.5, 1.5, 0.35))
# pick an object
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "============ Generating plan 1"
# This works:
## top approach x y z w : 0.707, 0.707, 0, 0
pose_target = geometry_msgs.msg.PoseStamped()
pose_target.header.frame_id = "/base"
pose_target.pose.orientation.x = 0.
pose_target.pose.orientation.y = 0.707
pose_target.pose.orientation.z = 0
pose_target.pose.orientation.w = 0.707
pose_target.pose.position.x = 0.8
pose_target.pose.position.y = 0.3
pose_target.pose.position.z = 0.11
group.set_pose_target(pose_target)
group.plan()
print "\n here now 1"
#rospy.sleep(5)
group.go()
quit()
print "\n here now 2"
rospy.sleep(2)
pose_target = geometry_msgs.msg.PoseStamped()
pose_target.header.frame_id = "/base"
pose_target.pose.orientation.x = 0.
pose_target.pose.orientation.y = 0.707
pose_target.pose.orientation.z = 0
pose_target.pose.orientation.w = 0.707
pose_target.pose.position.x = 0.8
pose_target.pose.position.y = 0.3
pose_target.pose.position.z = -0.02
group.set_pose_target(pose_target)
group.plan()
print "\n here now 3"
rospy.sleep(2)
group.go()
left.close()
group.attach_object("cube")
rospy.sleep(2)
pose_target = geometry_msgs.msg.PoseStamped()
pose_target.header.frame_id = "/base"
pose_target.pose.orientation.x = 0.
pose_target.pose.orientation.y = 0.707
pose_target.pose.orientation.z = 0
pose_target.pose.orientation.w = 0.707
pose_target.pose.position.x = 0.8
pose_target.pose.position.y = 0.3
pose_target.pose.position.z = 0.2
group.set_pose_target(pose_target)
group.plan()
print "\n here now 4"
rospy.sleep(2)
group.go()
rospy.sleep(2)
pose_target = geometry_msgs.msg.PoseStamped()
pose_target.header.frame_id = "/base"
pose_target.pose.orientation.x = 0.
pose_target.pose.orientation.y = 0.707
pose_target.pose.orientation.z = 0
pose_target.pose.orientation.w = 0.707
pose_target.pose.position.x = 0.8
pose_target.pose.position.y = 0
pose_target.pose.position.z = -0.02
group.set_pose_target(pose_target)
group.plan()
print "\n here now 5"
rospy.sleep(5)
group.go()
left.open()
group.detach_object("cube")
pose_target = geometry_msgs.msg.PoseStamped()
pose_target.header.frame_id = "/base"
pose_target.pose.orientation.x = 0.
pose_target.pose.orientation.y = 0.707
pose_target.pose.orientation.z = 0
pose_target.pose.orientation.w = 0.707
pose_target.pose.position.x = 0.8
pose_target.pose.position.y = 0
pose_target.pose.position.z = 0.2
group.set_pose_target(pose_target)
group.plan()
print "\n here now 6"
rospy.sleep(5)
group.go()
pose_target = geometry_msgs.msg.PoseStamped()
pose_target.header.frame_id = "/base"
pose_target.pose.orientation.x = 0.
pose_target.pose.orientation.y = 0.707
pose_target.pose.orientation.z = 0
pose_target.pose.orientation.w = 0.707
pose_target.pose.position.x = 0.8
pose_target.pose.position.y = 0.5
pose_target.pose.position.z = 0.5
group.set_pose_target(pose_target)
group.plan()
print "\n here now 7"
rospy.sleep(5)
group.go()
# =============== QUITTING ================
quit()
pose_target = geometry_msgs.msg.PoseStamped()
pose_target.header.frame_id = "/world"
pose_target.pose.orientation.x = 0.707
pose_target.pose.orientation.y = 0
pose_target.pose.orientation.z = 0.707
pose_target.pose.orientation.w = 0
pose_target.pose.position.x = 0.6
pose_target.pose.position.y = 0.4
pose_target.pose.position.z = 0
group.set_pose_target(pose_target)
group.plan()
rospy.sleep(10)
print "\n here now 1"
group.go()
rospy.sleep(3)
print "\n here now 2"
left.close()
rospy.sleep(2)
left.open()
# =============== QUITTING ================
quit()
# Create grasp
grasp_pose = PoseStamped()
grasp_pose.header.frame_id="base"
grasp_pose.pose.position.x = 0.6
grasp_pose.pose.position.y = 0.4
grasp_pose.pose.position.z = 0
grasp_pose.pose.orientation.x = 0
grasp_pose.pose.orientation.y = 0.707
grasp_pose.pose.orientation.z = 0
grasp_pose.pose.orientation.w = .707
grasp = Grasp()
grasp.grasp_pose = grasp_pose
grasp.pre_grasp_approach.direction.vector.y = 0
grasp.pre_grasp_approach.direction.vector.x = 0
grasp.pre_grasp_approach.direction.vector.z = 1
grasp.pre_grasp_approach.direction.header.frame_id = "base"
grasp.pre_grasp_approach.min_distance = 0.01
grasp.pre_grasp_approach.desired_distance = 0.25
grasp.post_grasp_retreat.direction.header.frame_id = "base"
grasp.pre_grasp_approach.direction.vector.y = 0
grasp.pre_grasp_approach.direction.vector.x = 0
grasp.pre_grasp_approach.direction.vector.z = -1
grasp.post_grasp_retreat.min_distance = 0.01
grasp.post_grasp_retreat.desired_distance = 0.25
grasp.pre_grasp_posture.header.frame_id="base"
grasp.pre_grasp_posture.joint_names.append("left_gripper_base")
pre_point = JointTrajectoryPoint()
pre_point.positions.append(0.0095)
grasp.pre_grasp_posture.points.append(pre_point)
grasp.grasp_posture.header.frame_id="base"
grasp.grasp_posture.joint_names.append("left_gripper_base")
point = JointTrajectoryPoint()
point.positions.append(-0.0125)
grasp.grasp_posture.points.append(point)
grasp.allowed_touch_objects.append("cube")
grasps = []
grasps.append(grasp)
print "grasp:", grasp
#quit()
group.set_goal_position_tolerance(10)
group.set_planning_time(20)
robot.left_arm.pick("cube", grasps)
quit()
#print "solution:", plan1
print "============ Waiting while RVIZ displays plan1..."
#rospy.sleep(5)
group.go()
quit()
## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the
## group.plan() method does this automatically so this is not that useful
## here (it just displays the same trajectory again).
print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory);
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)
## Moving to a pose goal
## ^^^^^^^^^^^^^^^^^^^^^
##
## Moving to a pose goal is similar to the step above
## except we now use the go() function. Note that
## the pose goal we had set earlier is still active
## and so the robot will try to move to that goal. We will
## not use that function in this tutorial since it is
## a blocking function and requires a controller to be active
## and report success on execution of a trajectory.
# Uncomment below line when working with a real robot
# group.go(wait=True)
## Planning to a joint-space goal
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
##
## Let's set a joint space goal and move towards it.
## First, we will clear the pose target we had just set.
group.clear_pose_targets()
## Then, we will get the current set of joint values for the group
group_variable_values = group.get_current_joint_values()
print "============ Joint values: ", group_variable_values
## Now, let's modify one of the joints, plan to the new joint
## space goal and visualize the plan
group_variable_values[0] = 1.0
group.set_joint_value_target(group_variable_values)
plan2 = group.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a cartesian path directly by specifying a list of waypoints
## for the end-effector to go through.
waypoints = []
# start with the current pose
waypoints.append(group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))
# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))
# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
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()
#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