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, 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):
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, 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, 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 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 __init__(self):
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.arm = MoveGroupCommander("arm")
rospy.sleep(1)
#remove existing objects
self.scene.remove_world_object()
self.scene.remove_attached_object("endeff", "part")
rospy.sleep(5)
'''
# publish a demo scene
self.pos = PoseStamped()
self.pos.header.frame_id = "world"
# add wall in between
self.pos.pose.position.x = -0.14
self.pos.pose.position.y = 0.02
self.pos.pose.position.z = 0.09
self.scene.add_box("wall", pos, (0.06, 0.01, 0.18))
# add an object to be grasped
self.pos.pose.position.x = -0.14
self.pos.pose.position.y = -0.0434
self.pos.pose.position.z = 0.054
'''
self.g=Grasp()
self.g.pre_grasp_approach.direction.vector.z= 1
self.g.pre_grasp_approach.direction.header.frame_id = 'endeff'
self.g.pre_grasp_approach.min_distance = 0.04
self.g.pre_grasp_approach.desired_distance = 0.10
self.g.grasp_posture = self.make_gripper_posture(0)
self.g.post_grasp_retreat.direction.vector.z= -1
self.g.post_grasp_retreat.direction.header.frame_id = 'endeff'
self.g.post_grasp_retreat.min_distance = 0.04
self.g.post_grasp_retreat.desired_distance = 0.10
self.g.allowed_touch_objects = ["part"]
self.p=PlaceLocation()
self.p.place_pose.header.frame_id= 'world'
self.p.place_pose.pose.position.x= -0.13341
self.p.place_pose.pose.position.y= 0.12294
self.p.place_pose.pose.position.z= 0.099833
self.p.place_pose.pose.orientation.x= 0
self.p.place_pose.pose.orientation.y= 0
self.p.place_pose.pose.orientation.z= 0
self.p.place_pose.pose.orientation.w= 1
self.p.pre_place_approach.direction.vector.z= 1
self.p.pre_place_approach.direction.header.frame_id = 'endeff'
self.p.pre_place_approach.min_distance = 0.06
self.p.pre_place_approach.desired_distance = 0.12
self.p.post_place_posture= self.make_gripper_posture(-1.1158)
self.p.post_place_retreat.direction.vector.z= -1
self.p.post_place_retreat.direction.header.frame_id = 'endeff'
self.p.post_place_retreat.min_distance = 0.05
self.p.post_place_retreat.desired_distance = 0.06
self.p.allowed_touch_objects=["part"]
self._as = actionlib.SimpleActionServer("server_test", TwoIntsAction, execute_cb=self.execute_pick_place, auto_start = False)
self._as.start()
def load_grasps(filename): f = open(filename) args = yaml.load(f) grasp = Grasp() genpy.message.fill_message_args(grasp, args) return grasp
def make_grasps(self, initial_pose_stamped, allowed_touch_objects):
# Initialize the grasp object
g = Grasp()
# Set the pre-grasp and grasp postures appropriately
g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)
g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)
# Set the approach and retreat parameters as desired
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.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 main():
try:
print("creating basic interface")
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_and_pick', anonymous=True)
#creating a robot object
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
#Defining the group name
group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
move_group.set_planning_time(45.0)
print "============ Printing initial robot state"
print robot.get_current_state()
################################################### Creating the environment: Namely a cube and 2 tables to pick from and place on.
#clean the scene
scene.remove_world_object("table1")
scene.remove_world_object("table2")
scene.remove_world_object("cube")
print"################Creating the scene"
dem = moveit_commander.PoseStamped()
dem.header.frame_id = "panda_link0"
#add a pickup table
dem.pose.position.x = 0.5
dem.pose.position.y = 0
dem.pose.position.z = 0.2
scene.add_box("table1", dem, (0.02, 0.02, 0.58))
#add a drop off table
dem.pose.position.x = 0
dem.pose.position.y = 0.5
dem.pose.position.z = 0.2
scene.add_box("table2", dem, (0.02, 0.02, 0.58))
#add an object to be grasped
dem.pose.position.x = 0.5
dem.pose.position.y = 0
dem.pose.position.z = 0.5
scene.add_box("cube", dem, (0.02, 0.02, 0.02))
rospy.sleep(1)
########################Resetting the robot arm to prevent singularity
print"Resetting: moving joints to defined orientation to avoid singularity"
joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi/4
joint_goal[2] = 0
joint_goal[3] = -pi/2
joint_goal[4] = 0
joint_goal[5] = pi/3
joint_goal[6] = 0
move_group.go(joint_goal, wait=True)
move_group.stop()
rospy.sleep(1)
##################################################Approaching the cube and picking it up
print "################Moving to pick the cube"
grasps = Grasp()
#defining the grasp pose of the end effector
grasps.grasp_pose.header.frame_id = "panda_link0"
grasps.grasp_pose.pose.position.x = 0.415
grasps.grasp_pose.pose.position.y = 0
grasps.grasp_pose.pose.position.z = 0.5
quaternion = quaternion_from_euler(-pi/2, -pi/4, -pi/2)
grasps.grasp_pose.pose.orientation.x = quaternion[0]
grasps.grasp_pose.pose.orientation.y = quaternion[1]
grasps.grasp_pose.pose.orientation.z = quaternion[2]
grasps.grasp_pose.pose.orientation.w = quaternion[3]
#setting the pregrasp approach
grasps.pre_grasp_approach.direction.header.frame_id = "panda_link0"
grasps.pre_grasp_approach.direction.vector.x = 1.0
grasps.pre_grasp_approach.min_distance = 0.095
grasps.pre_grasp_approach.desired_distance = 0.115
#setting post grasp-retreat
grasps.post_grasp_retreat.direction.header.frame_id = "panda_link0"
grasps.post_grasp_retreat.direction.vector.z = 1.0
grasps.post_grasp_retreat.min_distance = 0.1
grasps.post_grasp_retreat.desired_distance = 0.25
#opening the hand
#add points
grasps.pre_grasp_posture.joint_names.append("panda_finger_joint1")
grasps.pre_grasp_posture.joint_names.append("panda_finger_joint2")
point1=JointTrajectoryPoint()
point1.positions.append(0.04)
point1.positions.append(0.04)
point1.time_from_start = rospy.Duration(0.5)
grasps.pre_grasp_posture.points.append(point1)
#closing the hand
#add points
grasps.grasp_posture.joint_names.append("panda_finger_joint1")
grasps.grasp_posture.joint_names.append("panda_finger_joint2")
point2=JointTrajectoryPoint()
point2.positions.append(0.00)
point2.positions.append(0.00)
point2.time_from_start = rospy.Duration(0.5)
grasps.grasp_posture.points.append(point2)
move_group.set_support_surface_name("table1")
move_group.pick("cube", grasps)
rospy.sleep(1.0)
print "============ Printing picked up robot state"
print robot.get_current_state()
##########################################################Moving the cube and placing it down
print"################Moving to place the cube"
placer = PlaceLocation()
#defining the grasp pose of the end effector
placer.place_pose.header.frame_id = "panda_link0"
placer.place_pose.pose.position.x = 0
placer.place_pose.pose.position.y = 0.5
placer.place_pose.pose.position.z = 0.5
quaternion = quaternion_from_euler(-pi, 0, pi/2)
placer.place_pose.pose.orientation.x = quaternion[0]
placer.place_pose.pose.orientation.y = quaternion[1]
placer.place_pose.pose.orientation.z = quaternion[2]
placer.place_pose.pose.orientation.w = quaternion[3]
#setting the pre-place approach
placer.pre_place_approach.direction.header.frame_id = "panda_link0"
placer.pre_place_approach.direction.vector.z = -1.0
placer.pre_place_approach.min_distance = 0.095
placer.pre_place_approach.desired_distance = 0.115
#setting post place-retreat
placer.post_place_retreat.direction.header.frame_id = "panda_link0"
placer.post_place_retreat.direction.vector.y = -1.0
placer.post_place_retreat.min_distance = 0.1
placer.post_place_retreat.desired_distance = 0.25
#opening the hand
#add points
placer.post_place_posture.joint_names.append("panda_finger_joint1")
placer.post_place_posture.joint_names.append("panda_finger_joint2")
point3 = JointTrajectoryPoint()
point3.positions.append(0.00)
point3.positions.append(0.00)
point3.time_from_start = rospy.Duration(0.5)
placer.post_place_posture.points.append(point3)
move_group.set_support_surface_name("table2")
move_group.place("cube", placer)
print "============ Printing placed/final robot state"
print robot.get_current_state()
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
def plan_point_cluster_grasps(self, target, arm_name):
rospy.loginfo("Probabilistic Version! ")
error_code = GraspPlanningErrorCode()
grasps = []
#get the hand joint names from the param server (loaded from yaml config file)
#hand_description = rospy.get_param('hand_description')
hand_description = rospy.get_param('~hand_description')
pregrasp_joint_angles_dict = rospy.get_param('~pregrasp_joint_angles')
grasp_joint_angles_dict = rospy.get_param('~grasp_joint_angles')
pregrasp_joint_efforts_dict = rospy.get_param(
'~pregrasp_joint_efforts')
grasp_joint_efforts_dict = rospy.get_param('~grasp_joint_efforts')
if not arm_name:
arm_name = hand_description.keys()[0]
rospy.logerr(
"point cluster grasp planner: missing arm_name in request! Using "
+ arm_name)
try:
hand_joints = hand_description[arm_name]["hand_joints"]
except KeyError:
arm_name = hand_description.keys()[0]
rospy.logerr("arm_name " + arm_name +
" not found! Using joint names from " + arm_name)
try:
hand_joints = hand_description[arm_name]["hand_joints"]
except KeyError:
rospy.logerr("no hand joints found for %s!" % arm_name)
return ([], error_code.OTHER_ERROR)
try:
hand_frame = hand_description[arm_name]["hand_frame"]
hand_approach_direction = hand_description[arm_name][
"hand_approach_direction"]
except KeyError:
rospy.logerr(
"couldn't find hand_frame or hand_approach_direction!")
return ([], error_code.OTHER_ERROR)
pregrasp_joint_angles = pregrasp_joint_angles_dict[arm_name]
grasp_joint_angles = grasp_joint_angles_dict[arm_name]
pregrasp_joint_efforts = pregrasp_joint_efforts_dict[arm_name]
grasp_joint_efforts = grasp_joint_efforts_dict[arm_name]
#hand_joints = rospy.get_param('/hand_description/'+arm_name+'/hand_joints')
rospy.loginfo("hand_joints:" + str(hand_joints))
#find the cluster bounding box and relevant frames, and transform the cluster
init_start_time = time.time()
if len(target.cluster.points) > 0:
if (target.use_probability): #probabilistic
if (len(target.probabilities) > 0):
self.pcgp.init_cluster_grasper(target.cluster,
target.probabilities,
use_probability=True)
else: #weighted probabilistic
self.pcgp.init_cluster_grasper(target.cluster,
probabilities=[],
use_probability=True)
else: #deterministic
self.pcgp.init_cluster_grasper(target.cluster,
probabilities=[],
use_probability=False)
cluster_frame = target.cluster.header.frame_id
self.cluster_frame = cluster_frame
else:
self.pcgp.init_cluster_grasper(target.region.cloud)
cluster_frame = target.region.cloud.header.frame_id
if len(cluster_frame) == 0:
rospy.logerr("region.cloud.header.frame_id was empty!")
error_code.value = error_code.OTHER_ERROR
return (grasps, error_code)
init_end_time = time.time()
#print "init time: %.3f"%(init_end_time - init_start_time)
#plan grasps for the cluster (returned in the cluster frame)
grasp_plan_start_time = time.time()
(pregrasp_poses, grasp_poses, gripper_openings, qualities,
pregrasp_dists) = self.pcgp.plan_point_cluster_grasps()
grasp_plan_end_time = time.time()
#print "total grasp planning time: %.3f"%(grasp_plan_end_time - grasp_plan_start_time)
#add error code to service
error_code.value = error_code.SUCCESS
grasp_list = []
if pregrasp_poses == None:
error_code.value = error_code.OTHER_ERROR
return (grasps, error_code)
#fill in the list of grasps
for (grasp_pose, quality,
pregrasp_dist) in zip(grasp_poses, qualities, pregrasp_dists):
pre_grasp_joint_state = self.create_joint_trajectory(
hand_joints, pregrasp_joint_angles, pregrasp_joint_efforts)
grasp_joint_state = self.create_joint_trajectory(
hand_joints, grasp_joint_angles, grasp_joint_efforts)
#if the cluster isn't in the same frame as the graspable object reference frame,
#transform the grasps to be in the reference frame
if cluster_frame == target.reference_frame_id:
transformed_grasp_pose = stamp_pose(grasp_pose, cluster_frame)
else:
transformed_grasp_pose = change_pose_stamped_frame(
self.pcgp.tf_listener, stamp_pose(grasp_pose,
cluster_frame),
target.reference_frame_id)
if self.pcgp.pregrasp_just_outside_box:
min_approach_distance = pregrasp_dist
else:
min_approach_distance = max(pregrasp_dist - .05, .05)
approach = GripperTranslation(
create_vector3_stamped(hand_approach_direction, hand_frame),
pregrasp_dist, min_approach_distance)
retreat = GripperTranslation(
create_vector3_stamped(
[-1. * x for x in hand_approach_direction], hand_frame),
pregrasp_dist, min_approach_distance)
# LT
grasp_list.append(
Grasp(id="id",
pre_grasp_posture=pre_grasp_joint_state,
grasp_posture=grasp_joint_state,
grasp_pose=transformed_grasp_pose,
grasp_quality=quality,
pre_grasp_approach=approach,
post_grasp_retreat=retreat,
max_contact_force=-1))
#if requested, randomize the first few grasps
if self.randomize_grasps:
first_grasps = grasp_list[:30]
random.shuffle(first_grasps)
shuffled_grasp_list = first_grasps + grasp_list[30:]
grasps = shuffled_grasp_list
else:
grasps = grasp_list
return (grasps, error_code)
#robot.pincher_arm.set_planner_id('RRTConnectkConfigDefault')
robot.pincher_arm.set_num_planning_attempts(10000)
robot.pincher_arm.set_planning_time(5)
robot.pincher_arm.set_goal_position_tolerance(0.01)
robot.pincher_arm.set_goal_orientation_tolerance(0.5)
robot.pincher_gripper.set_goal_position_tolerance(0.01)
robot.pincher_gripper.set_goal_orientation_tolerance(0.5)
robot.get_current_state()
robot.pincher_arm.set_start_state(RobotState())
#------------------------------
# The Grasp
#-----------------------------
the_grasp = Grasp()
the_grasp.id = "Por cima"
# Gripper Posture before the grasp (opened griper)
the_grasp.pre_grasp_posture.joint_names = robot.pincher_gripper.get_active_joints(
)
the_grasp.pre_grasp_posture.points.append(JointTrajectoryPoint())
the_grasp.pre_grasp_posture.points[0].positions = [0.030]
# Gripper posture while grapping (closed gripper)
the_grasp.grasp_posture.joint_names = robot.pincher_gripper.get_active_joints(
)
the_grasp.grasp_posture.points.append(JointTrajectoryPoint())
the_grasp.grasp_posture.points[0].positions = [0.015]
# Where the arm should go to grasp the object
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# add a position for placement
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 0.4
p1.pose.position.y = -0.3
p1.pose.position.z = 0.75
rospy.sleep(1)
#rospy.logwarn("moving to test")
grasps = []
0.67611; 0.0091003; 0.71731
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_footprint"
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()
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
#right_arm.set_planner_id("KPIECEkConfigDefault");
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("right_gripper_open")
#right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.7
p.pose.position.y = -0.2
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.47636
start_pose.pose.position.y = -0.21886
start_pose.pose.position.z = 0.9
start_pose.pose.orientation.x = 0.00080331
start_pose.pose.orientation.y = 0.001589
start_pose.pose.orientation.z = -2.4165e-06
start_pose.pose.orientation.w = 1
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.3)
rospy.spin()
roscpp_shutdown()
# move to a random target
# right_arm.set_named_target("r_start")
# right_arm.go()
# rospy.sleep(1)
# right_arm.set_random_target()
# right_arm.go()
# rospy.sleep(1)
# right_arm.set_position_target([.75,-0.3, 1])
# right_arm.go()
# rospy.sleep(1)
grasps = []
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_link"
grasp_pose.pose.position.x = 0.47636
grasp_pose.pose.position.y = -0.21886
grasp_pose.pose.position.z = 0.7164
grasp_pose.pose.orientation.x = 0.00080331
grasp_pose.pose.orientation.y = 0.001589
grasp_pose.pose.orientation.z = -2.4165e-06
grasp_pose.pose.orientation.w = 1
rospy.logwarn("moving to arm")
right_arm.set_pose_target(grasp_pose)
right_arm.go()
rospy.sleep(3)
def 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
def generate_grasp_width(self,
eef_orientation,
position,
width,
roll=0,
pitch=0,
yaw=0,
length=0):
now = rospy.Time.now()
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose.position = position
if eef_orientation == "horizontal":
q = quaternion_from_euler(0.0, numpy.deg2rad(pitch), 0.0)
elif eef_orientation == "vertical":
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0),
numpy.deg2rad(yaw))
elif eef_orientation == "user_defined":
q = quaternion_from_euler(numpy.deg2rad(roll),
numpy.deg2rad(pitch), numpy.deg2rad(yaw))
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# transform from gripper to TCP
grasp.grasp_pose.pose = self.gripper2TCP(grasp.grasp_pose.pose, length)
if not self.is_inside_workspace(grasp.grasp_pose.pose.position.x,
grasp.grasp_pose.pose.position.y,
grasp.grasp_pose.pose.position.z):
rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****')
#return False
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector = self.approach_direction
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector = self.retreat_direction
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
grasp.max_contact_force = 1000
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.0)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(width)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
return grasp
def generate_grasps(self, name, pose):
grasps = []
now = rospy.Time.now()
#for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(10.0)):
# Create place location:
angle = 0
grasp = Grasp()
grasp.grasp_pose.header.stamp = now
grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame()
grasp.grasp_pose.pose = copy.deepcopy(pose)
# Setting pre-grasp approach
grasp.pre_grasp_approach.direction.header.stamp = now
grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.pre_grasp_approach.direction.vector.z = -0.2
grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist
grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist
# Setting post-grasp retreat
grasp.post_grasp_retreat.direction.header.stamp = now
grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame(
)
grasp.post_grasp_retreat.direction.vector.z = 0.2
grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist
grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist
# if name != "cylinder":
q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle)
grasp.grasp_pose.pose.orientation = Quaternion(*q)
# else:
# #grasp.pre_grasp_approach.direction.vector.z = -0.05
# grasp.pre_grasp_approach.direction.vector.x = 0.1
# #grasp.post_grasp_retreat.direction.vector.z = 0.05
# grasp.post_grasp_retreat.direction.vector.x = -0.1
grasp.max_contact_force = 100
grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
traj.positions.append(0.03)
traj.time_from_start = rospy.Duration.from_sec(0.5)
grasp.pre_grasp_posture.points.append(traj)
grasp.grasp_posture.joint_names.append("gripper_finger1_joint")
traj = JointTrajectoryPoint()
if name == "box":
traj.positions.append(0.57)
elif name == "sphere":
traj.positions.append(0.3)
else:
traj.positions.append(0.3)
#traj.velocities.append(0.2)
traj.effort.append(800)
traj.time_from_start = rospy.Duration.from_sec(5.0)
grasp.grasp_posture.points.append(traj)
grasps.append(grasp)
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 grasp_array(cls, msg, obj):
# for me...
for i in range(msg['_length_grasp']):
gsp = Grasp()
# direct descendants of grasp:
gsp.id = msg["%s_id" % i]
gsp.grasp_quality = msg["%s_grasp_quality" % i]
gsp.max_contact_force = msg["%s_max_contact_force" % i]
gsp.allowed_touch_objects = msg["%s_allowed_touch_objects" % i]
# descendants of pre_grasp_posture:
gsp.pre_grasp_posture.header = \
decode.header(msg, gsp.pre_grasp_posture.header, "%s_pre" % i)
gsp.pre_grasp_posture.joint_names = msg['%s_prejoint_names' % i]
for j in range(msg['%s_length_points1' % i]):
jtp1 = JointTrajectoryPoint()
jtp1 = decode.grasp_points(msg, jtp1, "%s_%s_pre" % (i, j))
gsp.pre_grasp_posture.points.append(jtp1)
# descendants of grasp_posture:
gsp.grasp_posture.header = \
decode.header(msg, gsp.grasp_posture.header, "%s_posture" % i)
gsp.grasp_posture.joint_names = msg["%s_joint_names" % i]
for j in range(msg['%s_length_points2' % i]):
jtp2 = JointTrajectoryPoint()
jtp2 = decode.grasp_points(msg, jtp2, "%s_%s_posture" % (i, j))
gsp.grasp_posture.points.append(jtp2)
# descendants of grasp_pose:
gsp.grasp_pose.header = \
decode.header(msg, gsp.grasp_pose.header, "%s_pose" % i)
gsp.grasp_pose.pose.position = \
decode.position(msg, gsp.grasp_pose.pose.position,
"%s_pose" % i)
gsp.grasp_pose.pose.orientation = \
decode.orientation(msg, gsp.grasp_pose.pose.orientation,
"%s_pose" % i)
# descendants of pre_grasp_approach:
gsp.pre_grasp_approach.direction.header = \
decode.header(msg, gsp.pre_grasp_approach.direction.header,
"%s_app" % i)
gsp.pre_grasp_approach.direction.vector = \
decode.vector(msg, gsp.pre_grasp_approach.direction.vector,
"%s_app" % i)
gsp.pre_grasp_approach.desired_distance = \
msg['%s_appdesired_distance' % i]
gsp.pre_grasp_approach.min_distance = msg['%s_appmin_distance' % i]
# descendants of post_grasp_retreat:
gsp.post_grasp_retreat.direction.header = \
decode.header(msg, gsp.post_grasp_retreat.direction.header,
"%s_ret" % i)
gsp.post_grasp_retreat.direction.vector = \
decode.vector(msg, gsp.post_grasp_retreat.direction.vector,
"%s_ret" % i)
gsp.post_grasp_retreat.desired_distance = \
msg['%s_retdesired_distance' % i]
gsp.post_grasp_retreat.min_distance = msg['%s_retmin_distance' % i]
# descendants of post_place_retreat
gsp.post_place_retreat.direction.header = \
decode.header(msg, gsp.post_place_retreat.direction.header,
'%s_place' % i)
gsp.post_place_retreat.direction.vector = \
decode.vector(msg, gsp.post_place_retreat.direction.vector,
'%s_place' % i)
gsp.post_place_retreat.desired_distance = \
msg['%s_pldesired_distance' % i]
gsp.post_place_retreat.min_distance = msg['%s_plmin_distance' % i]
# append Grasp to GraspArray:
obj.grasps.append(gsp)
return(obj)
def __init__(self, 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()
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 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 __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = moveit_commander.PlanningSceneInterface()
robot = moveit_commander.RobotCommander()
right_arm = moveit_commander.MoveGroupCommander(GROUP_NAME_ARM)
right_gripper = moveit_commander.MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
#right_arm.set_named_target("resting")
#right_arm.go()
#right_gripper.set_named_target("open")
#right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.2)
rospy.spin()
moveit_commander.roscpp_shutdown()
def generate_grasp_msgs(self, grasps):
formatted_grasps = []
for i in range(0, len(grasps)):
g = Grasp()
g.id = "dupa_" + str(i)
gp = PoseStamped()
gp.header.frame_id = "camera_color_optical_frame"
org_q = self.trans_matrix_to_quaternion(grasps[i])
# rot_q = Quaternion(0.7071, 0.7071, 0, 0) # 90* around X axis (W, X, Y, Z)
# quat = rot_q * org_q
quat = org_q
# 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
#why this like Lukasz?
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])
#pprint(gp.pose.orientation)
g.grasp_pose = gp
g.pre_grasp_approach.direction.header.frame_id = "tool_link"
g.pre_grasp_approach.direction.vector.x = 1.0
g.pre_grasp_approach.direction.vector.y = 0.0
g.pre_grasp_approach.direction.vector.z = 0.0
g.pre_grasp_approach.min_distance = 0.04
g.pre_grasp_approach.desired_distance = 0.08
# 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.allowed_touch_objects = ["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
#Create virtual link so I can get the transform from the gripper_link to grasp pose
# transform_msg = geometry_msgs.msg.TransformStamped()
# transform_msg.header.frame_id = "camera_color_optical_frame"
# transform_msg.child_frame_id = "virtual_frame"
# transform_msg.transform.translation.x = g.grasp_pose.pose.position.x
# transform_msg.transform.translation.y = g.grasp_pose.pose.position.y
# transform_msg.transform.translation.z = g.grasp_pose.pose.position.z
# transform_msg.transform.rotation.x = g.grasp_pose.pose.orientation.x
# transform_msg.transform.rotation.y = g.grasp_pose.pose.orientation.y
# transform_msg.transform.rotation.z = g.grasp_pose.pose.orientation.z
# transform_msg.transform.rotation.w = g.grasp_pose.pose.orientation.w
# self.bcaster.sendTransform(transform_msg)
# time.sleep(1)
# #t = self.tf.getLatestCommonTime("virtual_frame", "gripper_link")
# t = rospy.Time(0)
# self.tf.waitForTransform("gripper_link", "virtual_frame", t, rospy.Duration(4.0))
# (v_trans, v_rot) = self.tf.lookupTransformFull("gripper_link", t, "virtual_frame", t, "base_link")
# #t = self.tf.getLatestCommonTime("tool_link", "base_link")
# self.tf.waitForTransform("base_link", "tool_link", t, rospy.Duration(4.0))
# (tool_trans, tool_rot) = self.tf.lookupTransformFull("base_link",t, "tool_link", t, "base_link")
# pprint((v_trans, tool_trans))
# #Update the grasp message, tool_link and gripper have the same orientation
# g.grasp_pose.pose.position.x = tool_trans[0] + v_trans[0]
# g.grasp_pose.pose.position.y = tool_trans[1] + v_trans[1]
# g.grasp_pose.pose.position.z = tool_trans[2] + v_trans[2]
# gp.header.frame_id = "base_link"
#t = rospy.Time(0)
#grasp_point = geometry_msgs.msg.PointStamped()
#grasp_point.header.frame_id = "camera_color_optical_frame"
#grasp_point.point = g.grasp_pose.pose.position
#Get grasp point in base_link coordinate system
#t = self.tf.getLatestCommonTime("camera_color_optical_frame", "base_link")
#print(t)
#self.tf.waitForTransform("camera_color_optical_frame", "base_link", t, rospy.Duration(4.0))
#grasp_base = self.transformer.TransformPose("base_link", grasp_point)
#grasp_base = self.transformer.transform(grasp_point, "base_link", timeout=rospy.Duration(4.0))
grasp_base = self.tf.transformPose("base_link", g.grasp_pose)
# #Get tool and gripper translations from base_link
# #self.tf.waitForTransform("base_link", "tool_link", rospy.Duration(4.0))
# tool_trans, _ = self.tf.lookupTransform("base_link", "tool_link", rospy.Time(0))
# gripper_trans, _ = self.tf.lookupTransform("base_link", "gripper_link", rospy.Time(0))
# g.grasp_pose.header.frame_id = "base_link"
# g.grasp_pose.pose.position.x = tool_trans[0] + grasp_base.pose.position.x - gripper_trans[0]
# g.grasp_pose.pose.position.y = tool_trans[1] + grasp_base.pose.position.y - gripper_trans[1]
# g.grasp_pose.pose.position.z = tool_trans[2] + grasp_base.pose.position.z - gripper_trans[2]
# g.grasp_pose.pose.orientation.x = grasp_base.pose.orientation.x
# g.grasp_pose.pose.orientation.y = grasp_base.pose.orientation.y
# g.grasp_pose.pose.orientation.z = grasp_base.pose.orientation.z
# g.grasp_pose.pose.orientation.w = grasp_base.pose.orientation.w
#pprint(g.grasp_pose)
# q = Quaternion(g.grasp_pose.pose.orientation.w,
# g.grasp_pose.pose.orientation.x,
# g.grasp_pose.pose.orientation.y,
# g.grasp_pose.pose.orientation.z)
# (x_axis, z_axis) = (q.rotate([1.0, 0.0, 0.0]),
# q.rotate([0.0, 0.0, 1.0]))
# g.grasp_pose.header.frame_id = "base_link"
# g.grasp_pose.pose.position.x = grasp_base.pose.position.x - 0.025 * x_axis[0] + 0.015 * z_axis[0]
# g.grasp_pose.pose.position.y = grasp_base.pose.position.y - 0.025 * x_axis[1] + 0.015 * z_axis[1]
# g.grasp_pose.pose.position.z = grasp_base.pose.position.z - 0.025 * x_axis[2] + 0.015 * z_axis[2]
# g.grasp_pose.pose.orientation.x = grasp_base.pose.orientation.x
# g.grasp_pose.pose.orientation.y = grasp_base.pose.orientation.y
# g.grasp_pose.pose.orientation.z = grasp_base.pose.orientation.z
# g.grasp_pose.pose.orientation.w = grasp_base.pose.orientation.w
t = rospy.Time.now()
self.br.sendTransform(
(grasp_base.pose.position.x, grasp_base.pose.position.y,
grasp_base.pose.position.z),
(grasp_base.pose.orientation.x, grasp_base.pose.orientation.y,
grasp_base.pose.orientation.z, grasp_base.pose.orientation.w),
t, "grasp_frame", "base_link")
self.br.sendTransform((-0.025, 0.0, 0.015), (0, 0, 0, 1), t,
"virtual_tool", "grasp_frame")
tool_pose = geometry_msgs.msg.PoseStamped()
tool_pose.header.frame_id = "virtual_tool"
tool_pose.pose.orientation.w = 1.0
self.tf.waitForTransform("base_link", "virtual_tool", t,
rospy.Duration(4.0))
g.grasp_pose.header.frame_id = "base_link"
g.grasp_pose = self.tf.transformPose("base_link", tool_pose)
formatted_grasps.append(g)
return formatted_grasps
# Init stuff
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moving_irb120_robot', anonymous=True)
robot = moveit_commander.RobotCommander()
#scene = moveit_commander.PlanningSceneInterface()
scene = moveit_commander.PlanningSceneInterface("base_link")
arm_group = moveit_commander.MoveGroupCommander("irb_120")
hand_group = moveit_commander.MoveGroupCommander("robotiq_85")
# Publish trajectory in RViz
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
grasp = Grasp()
place = PlaceLocation()
pick_and_place = PickPlaceInterface("irb_120", "robotiq_85")
# Inverse Kinematics (IK): move TCP to given position and orientation
def move_pose_arm(roll, pitch, yaw, x, y, z):
pose_goal = geometry_msgs.msg.Pose()
quat = quaternion_from_euler(roll, pitch, yaw)
pose_goal.orientation.x = quat[0]
pose_goal.orientation.y = quat[1]
pose_goal.orientation.z = quat[2]
pose_goal.orientation.w = quat[3]
pose_goal.position.x = x
pose_goal.position.y = y
pose_goal.position.z = z
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object("right_gripper_link", "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("start1")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
#grasps = self.make_grasps(start_pose)
#result = False
#n_attempts = 0
# repeat until will succeed
#while result == False:
#result = robot.right_arm.pick("part", grasps)
#n_attempts += 1
#print "Attempts: ", n_attempts
#rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
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
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
rospy.sleep(1)
grasps = []
grasp = Grasp()
grasp.id = "right_grasp"
grasp.grasp_pose.header.frame_id = "base_link"
grasp.grasp_pose.header.stamp = rospy.Time.now()
grasp.grasp_pose.pose.position.x = 0.127233849387
grasp.grasp_pose.pose.position.y = -0.288138890182
grasp.grasp_pose.pose.position.z = 0.127521900721
grasp.grasp_pose.pose.orientation.w = -0.0267003256568
grasp.grasp_pose.pose.orientation.x = -0.0163927540977
grasp.grasp_pose.pose.orientation.y = -0.681901741856
grasp.grasp_pose.pose.orientation.z = 0.730772457525
grasps.append(grasp)
# clean the scene
scene.remove_world_object("part")
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
#right_arm.set_planner_id("KPIECEkConfigDefault");
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("right_start")
right_arm.go()
right_gripper.set_named_target("right_gripper_open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
p.pose.position.x = 0.42
p.pose.position.y = -0.2
p.pose.position.z = 0.3
scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.40
p.pose.position.y = -0.0
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.37636
start_pose.pose.position.y = 0.21886
start_pose.pose.position.z = 0.9
start_pose.pose.orientation.x = 0.00080331
start_pose.pose.orientation.y = 0.001589
start_pose.pose.orientation.z = -2.4165e-06
start_pose.pose.orientation.w = 1
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.3)
rospy.spin()
roscpp_shutdown()
def 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(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 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")
# move to a random target
#right_arm.set_named_target("r_start")
#right_arm.go()
#rospy.sleep(1)
#right_arm.set_random_target()
#right_arm.go()
#rospy.sleep(1)
#right_arm.set_position_target([.75,-0.3, 1])
#right_arm.go()
#rospy.sleep(1)
grasps = []
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_link"
grasp_pose.pose.position.x = 0.47636
grasp_pose.pose.position.y = -0.21886
grasp_pose.pose.position.z = 0.7164
grasp_pose.pose.orientation.x = 0.00080331
grasp_pose.pose.orientation.y = 0.001589
grasp_pose.pose.orientation.z = -2.4165e-06
grasp_pose.pose.orientation.w = 1
rospy.logwarn("moving to arm")
right_arm.set_pose_target(grasp_pose)
right_arm.go()
rospy.sleep(3)
print(name)
print "\n\n\n=============== Press `Enter` to list of attached object ==============\n"
raw_input()
for name in s.getKnownAttachedObjects():
print(name)
if len(s.getKnownCollisionObjects() + s.getKnownAttachedObjects()) == 0:
print("No objects in planning scene.")
print "\n\n\n=============== Press `Enter` to pick cube ==============\n"
raw_input()
pp = PickPlaceInterface(
"panda_arm",
"hand") # also takes a third parameter "plan_only" which defaults to False
g = Grasp() # fill in g
print(g)
pp.pickup(object_name, [
g,
], support_name="supporting_surface")
l = PlaceLocation() # fill in l
print(l)
pp.place(object_name, [
l,
],
goal_is_eef=True,
support_name="supporting_surface")
print "\n\n\n=============== Press `Enter` to De-attach Box ==============\n"
raw_input()
s.removeAttachedObject(object_name)
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 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(pitch)
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
scene.add_box("part", p, (0.07, 0.01, 0.2))
# add a position for placement
p1 = PoseStamped()
p1.header.frame_id = robot.get_planning_frame()
p1.pose.position.x = 0.4
p1.pose.position.y = -0.3
p1.pose.position.z = 0.75
rospy.sleep(1)
#rospy.logwarn("moving to test")
grasps = []
0.67611
0.0091003
0.71731
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_footprint"
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()
def pickup(self):
g = Grasp()
self._pickplace.pickup("box", [g], support_name="box")
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
print(repr(filtered_orientation) + " Grasp filtered because coming from underneath the ground")
continue
g = Grasp()
g.id = "dupa"
gp = PoseStamped()
gp.header.frame_id = "summit_xl_base_footprint"
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])
g.grasp_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.pre_grasp_posture.joint_names = ["gripper_right_finger_joint", "gripper_left_finger_joint"]
# g.pre_grasp_posture.joint_names = ["arm_tool0"]
# g.pre_grasp_posture.header.frame_id = "arm_wrist_3_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.allowed_touch_objects = ["obj"]
# g.max_contact_force = 0.0
g.grasp_quality = selected_grasps[0].score.data
formatted_grasps.append(g)
#Added code lines for cartesian pick
gp_cartesian = PoseStamped()
gp_cartesian.header.frame_id = "summit_xl_base_footprint"
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])
g_cartesian = Grasp ()
g_cartesian.id = "cart"
g_cartesian.grasp_pose = gp_cartesian
g_cartesian.allowed_touch_objects = ["obj"]
formatted_grasps_cartesian.append(g_cartesian)
# print(repr(cont) + " grasps out of " + repr(tot_grasps) + " removed because of no IK_SOLUTION error")
# 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 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(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