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 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):
# 初始化抓取姿态对象
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 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 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 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 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 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, 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 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 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(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 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,
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 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 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 compute_grasp(joints_open_position,
open_hand_time,
close_hand_time,
pre_grasp_aproach_direction,
pre_grasp_desired_dis,
pre_gras_min_dis,
post_grasp_aproach_direction,
post_grasp_desired_dis,
post_gras_min_dis,
world_file,
package_name,
package_folder,
world_object_pose,
aditional_rotation=None,
aditional_translation=None,
max_contact_force=-1,
allowed_touch_objects=[],
reference_frame="world"):
"""
Compute the grasp of an object
inputs:
--joints_open_position: dictionary of the joints and angle in open position (pre-grasp)
--open_hand_time: time desired to open hand
--close_hand_time: time to reach grasp position with the fingers.
--pre_grasp_aproach_direction: (x,y,z) direction end effector will aproach grasp_poses in world frame
--pre_grasp_desired_dis: distance will travel when aproaching
--pre_gras_min_dis: the min distance that must be considered feasible before the grasp is even attempted
--post_grasp_aproach_direction: (x,y,z) direction end effector will retreat after closing the hand (from grasp_pose). in world frame
--post_grasp_desired_dis: distance that will retreat
--post_gras_min_dis: min distance to consider is enough far away?
--max_contact_force: the maximum contact force to use while grasping (<=0 to disable)
--allowed_touch_objects: objects that can be touched/pushed/moved in the course of grasping. list of strings.
--world_file: world file of the hand holding the object. For example 'mpl_checker_v3.xml'
--package_name: name of the package where the world file is located
--package_folder: location of the folders "moldels" and "worlds" from graspit, inside the package
--world_object_pose: object Pose in world frame (gazebo/moveit)
--aditional rotation and translation: can be include in case of unconsidered transformations between the object in gazebo and graspit (not needed of them if frame if the same when object in graspit and gazebo when no rotation and translation is the same)
--reference_frame: directions and object position reference frame
"""
filename = "worlds/" + world_file
#read world file
robot_joints, T_robot_graspit, T_object_graspit = utils.read_world_file(
filename, package_name, package_folder)
#create moveit Grasp
grasp = Grasp()
#grasp id (optional)
#define pre-grasp joints posture. basically open hand.
grasp.pre_grasp_posture = utils.get_posture(joints_open_position,
open_hand_time)
#define hand finger posture during grasping
grasp.grasp_posture = utils.get_posture(robot_joints, close_hand_time)
#define grasp pose: position of the end effector during grasp
grasp.grasp_pose.header.frame_id = reference_frame #pose in this reference frame
grasp.grasp_pose.pose = utils.get_robot_pose(T_robot_graspit,
T_object_graspit,
world_object_pose,
aditional_rotation,
aditional_translation)
#grasp quality (no needed/used. can be obtained from graspit)
#pre_grasp_approach.The approach direction the robot will move when aproaching the object
grasp.pre_grasp_approach = utils.get_gripper_translation(
pre_grasp_aproach_direction, pre_grasp_desired_dis, pre_gras_min_dis,
reference_frame)
#post_grasp_retreat. The retreat direction to take after a grasp has been completed (object is attached)
grasp.post_grasp_retreat = utils.get_gripper_translation(
post_grasp_aproach_direction, post_grasp_desired_dis,
post_gras_min_dis, reference_frame)
#max contact force
grasp.max_contact_force = max_contact_force
#allowd_tocuh_force
grasp.allowed_touch_objects = allowed_touch_objects
return grasp
# (aim_x, aim_y, aim_z, aim_yaw) = onine_arm.get_valid_pose(item_translation[0], item_translation[1], item_translation[2], - 0.080)
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "left_gripper_link"
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.position.x = aim_x
grasp_pose.pose.position.y = aim_y
grasp_pose.pose.position.z = aim_z
grasp_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0.0, 0, aim_yaw))
g = Grasp()
g.pre_grasp_posture = onine_arm.make_gripper_posture(0.09)
g.grasp_posture = onine_arm.make_gripper_posture(0.01)
g.pre_grasp_approach = onine_arm.make_gripper_translation(0.08, 0.10)
g.post_grasp_retreat = onine_arm.make_gripper_translation(0.08, 0.10, -1.0)
g.grasp_pose = grasp_pose
#2 degrees resolution
pitch_vals = [
-0.10472, -0.0698132, -0.0349066, 0, 0.0349066, 0.0698132, 0.10472
]
height_vals = [
-0.005, -0.004, -0.003, -0.002, -0.001, 0, 0.001, 0.002, 0.003, 0.004,
0.005
]
# generate list of grasps
grasps = []
for h in height_vals:
for p in pitch_vals:
q = quaternion_from_euler(0.0, -p, aim_yaw)
def __init__(self, robot_name="panda_arm", frame="panda_link0"):
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(name="pick_place_test")
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# region Robot initial
self.robot = MoveGroupCommander(robot_name)
self.robot.set_goal_joint_tolerance(0.00001)
self.robot.set_goal_position_tolerance(0.00001)
self.robot.set_goal_orientation_tolerance(0.01)
self.robot.set_goal_tolerance(0.00001)
self.robot.allow_replanning(True)
self.robot.set_pose_reference_frame(frame)
self.robot.set_planning_time(3)
# endregion
self.gripper = MoveGroupCommander("hand")
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_OPEN)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
# Robot go home
self.robot.set_named_target("home")
self.robot.go()
# clear all object in world
self.clear_all_object()
table_pose = un.Pose(0, 0, -10, 0, 0, 0)
table_color = un.Color(255, 255, 0, 100)
self.add_object_box("table", table_pose, table_color, frame,
(2000, 2000, 10))
bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
bearing_color = un.Color(255, 0, 255, 255)
bearing_file_name = "../stl/bearing.stl"
self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
bearing_file_name)
obpose = self.scene.get_object_poses(["bearing"])
# self.robot.set_support_surface_name("table")
g = Grasp()
# Create gripper position open or close
g.pre_grasp_posture = self.open_gripper()
g.grasp_posture = self.close_gripper()
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [0, 1.0, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.9, [0, 1.0, 0])
p = PoseStamped()
p.header.frame_id = "panda_link0"
p.pose.orientation = obpose["bearing"].orientation
p.pose.position = obpose["bearing"].position
g.grasp_pose = p
g.allowed_touch_objects = ["bearing"]
a = []
a.append(g)
result = self.robot.pick(object_name="bearing", grasp=a)
print(result)
except Exception as ex:
print(ex)
moveit_commander.roscpp_shutdown()
moveit_commander.roscpp_shutdown()
# scene.add_box("table", p, (0.5, 1.5, 0.7))
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_footprint"
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.position.x = aim_x
grasp_pose.pose.position.y = aim_y
grasp_pose.pose.position.z = aim_z
grasp_pose.pose.orientation = Quaternion(
*quaternion_from_euler(0.0, 0, aim_yaw))
g = Grasp()
g.pre_grasp_posture = onine_arm.make_gripper_posture(0.08)
g.grasp_posture = onine_arm.make_gripper_posture(0.0075)
g.pre_grasp_approach = onine_arm.make_gripper_translation(0.13, 0.15)
g.post_grasp_retreat = onine_arm.make_gripper_translation(
0.13, 0.15, -0.15)
g.grasp_pose = grasp_pose
#2 degrees resolution
pitch_vals = [
-0.10472, -0.0698132, -0.0349066, 0, 0.0349066, 0.0698132, 0.10472
]
height_vals = [
-0.005, -0.004, -0.003, -0.002, -0.001, 0, 0.001, 0.002, 0.003, 0.004,
0.005
]
pos_vals = [
-0.005, -0.004, -0.003, -0.002, -0.001, 0, 0.001, 0.002, 0.003, 0.004,
0.005
]
# generate list of grasps
def compute_checkers_grasp(checker_row, checker_col, open_hand_time,
close_hand_time, pre_grasp_aproach_direction,
pre_grasp_desired_dis, pre_gras_min_dis,
post_grasp_aproach_direction,
post_grasp_desired_dis, post_gras_min_dis,
max_contact_force, allowed_touch_objects,
robot_base_frame, grasp_file, pregrasp_file):
def cell_info_client(from_row, from_col):
"""
function to get checker information from gazebo
"""
rospy.wait_for_service('checkers/cell_info/')
try:
cell_info = rospy.ServiceProxy('checkers/cell_info/', CellInfo)
resp = cell_info(from_row, from_col)
return resp
except rospy.ServiceException as e:
rospy.loginfo("Service call checker info failed: %s" % e)
#define some parameters
# grasp_file="mpl_checker_v4.xml" #grasping position
# pregrasp_file="mpl_checker_pregrasp.xml"
package_folder = 'resources'
package_name = 'mpl_graspit'
#create moveit Grasp
grasp = Grasp()
#find object information
cell_info = cell_info_client(checker_row, checker_col)
if not cell_info.available:
#object pose in world frame
objects_pose_w = cell_info.pose_checker
# print("object pose")
# print(str(objects_pose_w))
#compute angle between shoulder and object (set a starting angle to start looking for for the grip of the checker. Later check. there is an angle that alwyas work?)
angle = int(
compute_angle_object_frame("mpl_right_arm__humerus",
objects_pose_w))
print("start looking IK from angle " + str(angle))
# if robot_base_frame !="world":
# objects_pose_r=utils.transform_pose_between_frames(objects_pose_w, "world", robot_base_frame)#object pose in robot frame
# else:
# objects_pose_r=objects_pose_w
#read world file grasp
filename = "worlds/" + grasp_file
robot_joints_grasp, T_robot_graspit, T_object_graspit = utils.read_world_file(
filename, package_name, package_folder)
#read world file pregrasp
filename_pre = "worlds/" + pregrasp_file
robot_joints_pregrasp, _, _ = utils.read_world_file(
filename_pre, package_name, package_folder)
#check robot pose
aditional_translation = None
#find hand rotation that have inverse kinematic solution
step = 1
# for deg in range(angle,90,step):#check all circle. Rotation in Z axis
for deg in range(angle, 360, step):
rospy.loginfo("IK solver trying pose: %i deg rotation" % deg)
aditional_rotation = np.array([
math.cos(math.radians(deg / 2)), 0, 0,
math.sin(math.radians(deg / 2))
])
robot_pose_w = utils.get_robot_pose(T_robot_graspit,
T_object_graspit,
objects_pose_w,
aditional_rotation,
aditional_translation)
if robot_base_frame != "world":
robot_pose_r = utils.transform_pose_between_frames(
robot_pose_w, "world",
robot_base_frame) #object pose in robot frame
else:
robot_pose_r = robot_pose_w
# robot_pose_r=utils.get_robot_pose(T_robot_graspit ,T_object_graspit, objects_pose_r,aditional_rotation, aditional_translation)
#tranform pose to robot frame
#check is pose is reachabe using inverse kinematics (service must be available)
is_valid = (IK_client(robot_pose_r)).is_solution
if is_valid:
rospy.loginfo("Pose can be achived: %i" % deg)
#----complete grasp information---------------
#define pre-grasp joints posture. basically open hand.
grasp.pre_grasp_posture = utils.get_posture(
robot_joints_pregrasp, open_hand_time)
#define hand finger posture during grasping
grasp.grasp_posture = utils.get_posture(
robot_joints_grasp, close_hand_time)
#define grasp pose: position of the end effector during grasp
grasp.grasp_pose.header.frame_id = robot_base_frame #pose in this reference frame
grasp.grasp_pose.pose = robot_pose_r
#pre_grasp_approach.The approach direction the robot will move when aproaching the object
grasp.pre_grasp_approach = utils.get_gripper_translation(
pre_grasp_aproach_direction, pre_grasp_desired_dis,
pre_gras_min_dis, robot_base_frame)
#post_grasp_retreat. The retreat direction to take after a grasp has been completed (object is attached)
grasp.post_grasp_retreat = utils.get_gripper_translation(
post_grasp_aproach_direction, post_grasp_desired_dis,
post_gras_min_dis, robot_base_frame)
#max contact force
grasp.max_contact_force = max_contact_force
#allowd_tocuh_force
grasp.allowed_touch_objects = allowed_touch_objects
return grasp
# rospy.loginfo("Pose CAN NOT be achived")
# return None
# grasp_result=grasps.compute_grasp(
# joints_open_position,open_hand_time,
# close_hand_time,
# pre_grasp_aproach_direction, pre_grasp_desired_dis, pre_gras_min_dis,
# post_grasp_aproach_direction, post_grasp_desired_dis, post_gras_min_dis,
# grasp_file,
# package_name,
# package_folder,
# world_object_pose,
# aditional_rotation,
# aditional_translation,
# max_contact_force,
# allowed_touch_objects,
# reference_frame)
# return grasp_result
else:
rospy.loginfo("Cell " + cell_info.cell_name +
" doesn't have a checker piece")
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.1, 0.1, [0, 1, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.1, 0.15, [1, 0, 0])
# Set the first grasp pose to the input pose
g.grasp_pose = initial_pose_stamped
ideal_roll = 0
ideal_pitch = 0
ideal_yaw = 0
step_size = 0.1
idx = 0.1
idx_roll = ideal_roll + idx
idx_pitch = ideal_pitch + idx
idx_yaw = ideal_yaw + idx
roll_vals = []
pitch_vals = []
yaw_vals = []
while idx >= -0.1:
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 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
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
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