def callback(states):
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
arm.set_planner_id("RRTConnectkConfigDefault")
arm.set_num_planning_attempts(20)
arm.allow_replanning(True)
pose = copy.deepcopy(states.pose[-1])
temp = tf.transformations.euler_from_quaternion(
(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w))
quaternion = tf.transformations.quaternion_from_euler(
math.pi, temp[1], temp[2])
pose.position.z += 0.22
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
print pose
arm.set_pose_target(pose)
move_plan = arm.plan()
i = 0
while (not move_plan.joint_trajectory.joint_names):
move_plan = arm.plan()
i += 1
if (i == 5): break
state = arm.execute(move_plan)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.pose_goal = Pose()
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
# print("====== move plan go to up ======")
self.robot_arm.set_named_target("up") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to up ======")
rospy.sleep(0.5)
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
def move_TF(self):
self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538]
self.pose_goal.position.x = self.ee_TF_list[0]
self.pose_goal.position.y = self.ee_TF_list[1]
self.pose_goal.position.z = self.ee_TF_list[2]
self.pose_goal.orientation.x = self.ee_TF_list[3]
self.pose_goal.orientation.y = self.ee_TF_list[4]
self.pose_goal.orientation.z = self.ee_TF_list[5]
self.pose_goal.orientation.w = self.ee_TF_list[6]
self.robot_arm.set_pose_target(self.pose_goal)
self.robot_arm.go(True)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.Pose_goal = Pose()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
# robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
# self.robot_arm.set_named_target("up") #go to goal state.
# self.robot_arm.go()
# print("====== move plan go to home 1 ======")
# rospy.sleep(2)
# print("====== move plan go to up ======")
# self.robot_arm.set_named_target("zeros") #go to goal state.
# self.robot_arm.go(wait=True)
# print("====== move plan go to zeros ======")
# rospy.sleep(1)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
self.robot_arm.set_pose_target(self.Pose_goal) # go to goal state.
self.robot_arm.go(True)
print("====== move plan go to Pose_goal ======")
#rospy.sleep(2)
robot_state = self.robot_arm.get_current_pose();
robot_angle = self.robot_arm.get_current_joint_values();
print(robot_state)
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
self.Pose_goal = data
self.move_code()
def listener(self):
rospy.Subscriber("chatter", Pose, self.callback)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
arm.set_max_velocity_scaling_factor(0.8)
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_planning_time(1) # 规划时间限制为2秒
arm.set_num_planning_attempts(2) # 规划两次
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print arm.get_current_pose(eef_link).pose
sub = rospy.Subscriber('/detect_grasps_yolo/juggle_rects',
Float64MultiArray, self.callback)
self.juggle_rects = Float64MultiArray()
self.box_name = ''
self.scene = scene
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.move_distance = 0.1
self.back_distance = 0.15
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
# print("====== move plan go to up ======")
self.robot_arm.set_named_target("up") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to zero ======")
rospy.sleep(0.5)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home2 ======")
rospy.sleep(0.5)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
#self.clear_octomap()
def ar_callback(self, msg):
#marker = msg.markers[1]
self.marker = msg.markers
ml = len(self.marker)
m_id = self.marker[0].id
print "marker length: ", len(self.marker)
print "marker id: ", m_id
m_idd = []
'''
for ii in range(0, 2):
print(ii)
m_idd[ii] = marker[ii].id
#m_id[ii] = marker[ii].id
print(m_idd[ii])
'''
pos_x = self.marker[0].pose.pose.position.x
pos_y = self.marker[0].pose.pose.position.y
pos_z = self.marker[0].pose.pose.position.z
dist = math.sqrt((pos_x * pos_x) + (pos_y * pos_y))
#ori_x = marker.pose.pose.orientation.x
#ori_y = marker.pose.pose.orientation.y
#ori_z = marker.pose.pose.orientation.z
#ori_w = marker.pose.pose.orientation.w
#print(m_id)
print "==========="
print 'id: ', m_id
print 'pos: ', pos_x, pos_y, pos_z
#print('ori: ', ori_x, ori_y, ori_z, ori_w)
self.goal_x = pos_x - 1.0
self.goal_y = pos_y - pos_y
self.goal_z = pos_z - pos_z
def move_code(self):
robot_state = self.robot_arm.get_current_pose()
print "====== robot pose: \n",
print robot_state.pose.position
#marker_joint_goal = self.robot_arm.get_current_joint_values()
marker_joint_goal = [
0.07100913858593216, -1.8767615298285376, 2.0393206555899503,
-1.8313959190971882, -0.6278395875738125, 1.6918219826764682
]
self.robot_arm.go(marker_joint_goal, wait=True)
print "====== robot joint value: \n"
print marker_joint_goal
self.robot_arm.go(marker_joint_goal, wait=True)
self.robot_arm.go(True)
print "look at the markers"
pose_goal = Pose()
pose_goal.orientation.w = 0.0
pose_goal.position.x = 0.4
pose_goal.position.y = -0.4
pose_goal.position.z = 0.7
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
#self.robot_arm.set_pose_target(pose_goal)
#7self.robot_arm.go(True)
def plan_cartesian_path(self, scale=1):
waypoints = []
wpose = self.robot_arm.get_current_pose().pose
wpose.position.z -= scale * 0.2
wpose.position.x -= scale * 0.2
waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
rospy.loginfo("Maker messages detected. Starting followers...")
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
0.07100913858593216, -1.8767615298285376, 2.0393206555899503,
-1.8313959190971882, -0.6278395875738125, 1.6918219826764682
]
self.robot_arm.go(marker_joint_goal, wait=True)
def look_object(self):
look_object = self.robot_arm.get_current_joint_values()
## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함
print "wrist3 joint value(deg, rad): ", math.degrees(
look_object[5]), look_object[5]
look_object[5] = math.radians(90)
'''
if look_object[5] > abs(math.radians(180)):
if look_object[5] < 0:
look_object[5] = look_object[5] + math.radians(90) # wrist3
elif look_object[5] > 0:
look_object[5] = look_object[5] - math.radians(90)
else:
look_object[5] = look_object[5] - math.radians(90) # wrist3
'''
print "wrist3 joint value(deg, rad): ", math.degrees(
look_object[5]), look_object[5]
#look_object[3] = look_object[3] - (math.radians(00)) # wrist1
self.robot_arm.go(look_object, wait=True)
#look_object[5] = look_object[5] + (math.radians(90)) # wrist3
#self.robot_arm.go(look_object, wait=True)
def look_up_down(self):
self.clear_octomap()
print "======== clear_octomap... Please wait...."
look_up_down = self.robot_arm.get_current_joint_values()
#print "before: ", look_up_down
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
def plan_cartesian_path(self, x_offset, y_offset, scale=1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.goal_y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
print "======= pos(meter): ", self.position_list
print "======= orientation: ", self.orientation_list
def go_to_move(self, scale=1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
print "========== robot arm plannig frame: \n", planning_frame
self.calculed_coke_pose.position.x = (
scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale * self.goal_y) - 0.25
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + 0.72 + 0.2 # world to base_link z-offset real version offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(0, 0, 1.54))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [
self.calculed_coke_pose.position.x,
self.calculed_coke_pose.position.y,
self.calculed_coke_pose.position.z
]
tf_display_orientation = [
self.calculed_coke_pose.orientation.x,
self.calculed_coke_pose.orientation.y,
self.calculed_coke_pose.orientation.z,
self.calculed_coke_pose.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def ar_tf_listener(self, msg):
try:
self.marker = msg.markers
ml = len(self.marker)
target_id = 9
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_id:
pass
#target_id_flage = False
elif self.m_idd == target_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[
target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[
target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[
target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[
target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[
target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[
target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[
target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [
self.ar_pose.orientation.x, self.ar_pose.orientation.y,
self.ar_pose.orientation.z, self.ar_pose.orientation.w
]
(self.goal_roll,
self.goal_pitch, self.goal_yaw) = euler_from_quaternion(
self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
init_table_goal = self.robot_arm.get_current_joint_values()
init_table_goal[0] = 0.2
init_table_goal[1] = -1.983025375996725
init_table_goal[2] = -2.4233086744891565
init_table_goal[3] = 0.9490636587142944
init_table_goal[4] = 1.4068996906280518
init_table_goal[5] = -3.060608450566427
self.robot_arm.go(init_table_goal, wait=True)
rospy.sleep(0.5)
self.clear_octomap()
print("====== move plan go to init table goal ======")
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
self.clear_octomap()
print("===== octomap clear =====")
# print("====== move plan go to up ======")
''' test code 04/20'''
joint_goal = self.robot_arm.get_current_joint_values()
joint_goal[0] = 0.2
joint_goal[1] = -pi/2
joint_goal[2] = 0
joint_goal[3] = -pi/2
joint_goal[4] = 0
joint_goal[5] = 0
self.robot_arm.go(joint_goal, wait=True)
rospy.sleep(0.5)
print("===== move plan test code =====")
''' ======================================= '''
self.clear_octomap()
print("===== octomap clear =====")
robot_state = self.robot_arm.get_current_pose();
robot_angle = self.robot_arm.get_current_joint_values();
last_goal = self.robot_arm.get_current_joint_values()
last_goal = [0.2, -1.98, -2.4233, 0.9490, 1.4068, -3.0606]
self.robot_arm.go(last_goal, wait=True)
rospy.sleep(0.5)
print(robot_state)
print(robot_angle)
self.clear_octomap()
print("===== octomap clear =====")
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_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
#right_arm.set_goal_position_tolerance(0.005)
right_arm.set_goal_orientation_tolerance(0.005)
right_arm.set_planning_time(5)
right_arm.set_num_planning_attempts(5)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
#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("default")
right_arm.go(wait=True)
right_gripper.set_named_target("open")
right_gripper.go(wait=True)
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add an object to be grasped
p.pose.position.x = 0.170
p.pose.position.y = 0.04
p.pose.position.z = 0.3
#scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
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.00756585784256
start_pose.pose.position.y = -0.225419849157
start_pose.pose.position.z = 0.117192693055
start_pose.pose.orientation.x = 0.95493721962
start_pose.pose.orientation.y = -0.0160209629685
start_pose.pose.orientation.z = -0.00497157918289
start_pose.pose.orientation.w = 0.296333402395
print("going to pick up pose")
right_arm.set_pose_target(start_pose)
right_gripper.set_named_target("close")
right_arm.go(wait=True)
right_gripper.go(wait=True)
rospy.sleep(1)
right_arm.set_named_target("default")
right_arm.go(wait=True)
next_pose = PoseStamped()
next_pose.header.frame_id = FIXED_FRAME
next_pose.pose.position.x = -0.100732862949
next_pose.pose.position.y = -0.210876911879
next_pose.pose.position.z = 0.244678631425
next_pose.pose.orientation.x = 0.784905433655
next_pose.pose.orientation.y = -0.177844554186
next_pose.pose.orientation.z = -0.131161093712
next_pose.pose.orientation.w = 0.578870952129
right_arm.set_pose_target(next_pose)
right_gripper.set_named_target("open")
right_arm.go(wait=True)
right_gripper.go(wait=True)
rospy.sleep(3)
right_arm.set_named_target("default")
right_arm.go(wait=True)
rospy.spin()
roscpp_shutdown()
GROUP_NAME_ARM = "manipulator"
FIXED_FRAME = 'world'
#GROUP_NAME_GRIPPER = "NAME OF GRIPPER"
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('ur3_wrist3_tester', anonymous=True)
scene = PlanningSceneInterface()
robot_cmd = RobotCommander()
robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
robot_arm.set_goal_orientation_tolerance(0.005)
robot_arm.set_planning_time(5)
robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
robot_arm.allow_replanning(True)
robot_joints = robot_arm.get_current_joint_values()
robot_joints[5] = math.radians(90)
print "robot_joints wrist3: deg, rad", math.degrees(
robot_joints[5]), robot_joints[5]
robot_arm.go(robot_joints, wait=True)
while not rospy.is_shutdown():
robot_joints = robot_arm.get_current_joint_values()
if robot_joints[5] > abs(math.radians(170)):
class Move():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.br = tf.TransformBroadcaster()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def initial_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
2.3884003162384033, -1.6826804319964808, 0.7239289283752441,
-2.2834256331073206, 4.623193740844727, -0.025881592427388966
]
print("init pose : ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def go_to_move(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
2.3886876106262207, -1.6729376951800745, 1.6410179138183594,
-2.855138603840963, 4.623205661773682, -0.025869671498433888
]
print("move_pose : ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def go_to_tomato(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
2.238821506500244, -0.8660662809955042, 0.8210840225219727,
-2.8785727659808558, 4.622917175292969, -0.025917832051412404
]
print("tomato_pose : ",
self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param(
'/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber(
'/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
self.distance = 0
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.calculated_tomato_coor = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
2.6482303142547607, -0.3752959410296839, -2.1118043104754847,
-0.4801228682147425, -1.4944542090045374, -4.647655431424276
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
def go_to_goal_point(self, scale=1.0):
planning_frame = self.robot_arm.get_planning_frame()
print(">> robot arm planning frame: \n", planning_frame)
self.calculated_tomato.position.x = (scale * goal_x)
self.calculated_tomato.position.y = (scale * goal_y)
self.calculated_tomato.position.z = (scale * goal_z)
self.calculated_tomato.orientation.w = (scale * ori_w)
self.calculated_tomato.orientation.x = (scale * ori_x)
self.calculated_tomato.orientation.y = (scale * ori_y)
self.calculated_tomato.orientation.z = (scale * ori_z)
print(">> robot_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("======= Press `Enter` to if plan in correct!======")
raw_input()
self.robot_arm.go(True)
'''
def go_to_designated_coordinate(self):
desired_goal_pose = [-0.537,0.166, 0.248]
Cplanning_frame = self.robot_arm.get_planning_frame()
print(">> current planning frame: \n",Cplanning_frame)
self.calculated_tomato_coor.position.x = desired_goal_pose[0]
self.calculated_tomato_coor.position.y = desired_goal_pose[1]
self.calculated_tomato_coor.position.z = desired_goal_pose[2]
self.calculated_tomato_coor.orientation = Quaternion(*quaternion_from_euler(desired_goal_pose[0],desired_goal_pose[1],desired_goal_pose[2]))
print(">> goal frame", self.calculated_tomato_coor)
self.robot_arm.set_pose_target(self.calculated_tomato_coor)
tf_display_position = [self.calculated_tomato_coor.position.x, self.calculated_tomato_coor.position.y, self.calculated_tomato_coor.position.z]
tf_display_orientation = [self.calculated_tomato_coor.orientation.x, self.calculated_tomato_coor.orientation.y, self.calculated_tomato_coor.orientation.z, self.calculated_tomato_coor.orientation.w]
jj = 0
while jj < 5:
jj += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato_coor))
(plan, fraction) = self.robot_arm.compute_cartesian_path(tomato_waypoints,0.01,0.0)
self.display_trajectory(plan)
print("=========== Press `Enter` to if plan is correct!!...")
raw_input()
self.robot_arm.go(True)
'''
def plan_cartesian_path(self, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x -= scale * 0.1
self.wpose.position.y += scale * 0.1
waypoints.append(copy.deepcopy(self.wpose))
'''
self.wpose.position.x -= scale*0.2
waypoints.append(copy.deepcopy(self.wpose))
'''
'''
self.wpose.position.x -= scale*0.1
waypoints.append(copy.deepcopy(self.wpose))
'''
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def collectJsk(self, msg):
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
try:
(trans,
rot) = self.listener.lookupTransform('base_link', 'yolo_output00',
rospy.Time(0))
goal_x = trans[0]
goal_y = trans[1]
goal_z = trans[2]
ori_w = rot[0]
ori_x = rot[1]
ori_y = rot[2]
ori_z = rot[3]
print("==== trans[x,y,z]: ", trans)
print("==== rot[x,y,z,w]: ", rot)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logwarn('there is no tf')
class DaArmServer:
"""The basic, design problem/tui agnostic arm server
"""
gestures = {}
pointing_height = 0.06
grasp_height = 0.05
drop_height = 0.07
cruising_height = 0.1
START_TOLERANCE = 0.05 # this is for moveit to check for change in joint angles before moving
GOAL_TOLERANCE = 0.005
moving = False
paused = False
move_group_state = "IDLE"
last_joint_trajectory_goal = ""
last_joint_trajectory_result = ""
def __init__(self, num_planning_attempts=100, safe_zone=None):
rospy.init_node("daarm_server", anonymous=True)
self.safe_zone = safe_zone # this is a fallback zone to drop a block on fail if nothing is passed: [{x,y},{xT,yT}]
self.init_params()
self.init_scene()
self.init_publishers()
self.init_subscribers()
self.init_action_clients()
self.init_service_clients()
self.init_arm(num_planning_attempts)
def init_arm(self, num_planning_attempts=700):
rospy.set_param(
"/move_group/trajectory_execution/allowed_start_tolerance",
self.START_TOLERANCE)
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE)
self.arm.set_goal_orientation_tolerance(0.02)
self.init_services()
self.init_action_servers()
def init_scene(self):
world_objects = [
"table", "tui", "monitor", "overHead", "wall", "farWall",
"frontWall", "backWall", "blockProtector", "rearCamera"
]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8836)
self.overHeadPose = PoseStamped()
self.overHeadPose.header.frame_id = self.robot.get_planning_frame()
self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97)
self.overHeadDimension = (0.9906, 0.8382, 0.05)
self.blockProtectorPose = PoseStamped()
self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame(
)
self.blockProtectorPose.pose.position = Point(
0.0056, -0.343, -0.51 + self.cruising_height)
self.blockProtectorDimension = (0.9906, 0.8382, 0.8636)
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
self.farWallPose = PoseStamped()
self.farWallPose.header.frame_id = self.robot.get_planning_frame()
self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048)
self.farWallDimension = (0.6096, 2, 3.35)
self.frontWallPose = PoseStamped()
self.frontWallPose.header.frame_id = self.robot.get_planning_frame()
self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51)
self.frontWallDimension = (1, 0.15, 4)
self.backWallPose = PoseStamped()
self.backWallPose.header.frame_id = self.robot.get_planning_frame()
self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51)
self.backWallDimension = (1, 0.005, 4)
self.rearCameraPose = PoseStamped()
self.rearCameraPose.header.frame_id = self.robot.get_planning_frame()
self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51)
self.rearCameraDimension = (0.5, 0.5, 2)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
self.scene.add_box("farWall", self.farWallPose, self.farWallDimension)
self.scene.add_box("overHead", self.overHeadPose,
self.overHeadDimension)
self.scene.add_box("backWall", self.backWallPose,
self.backWallDimension)
self.scene.add_box("frontWall", self.frontWallPose,
self.frontWallDimension)
self.scene.add_box("rearCamera", self.rearCameraPose,
self.rearCameraDimension)
def raise_table(self):
#raises the table obstacle to protect blocks on the table during transport
self.scene.remove_world_object("blockProtector")
self.scene.add_box("blockProtector", self.blockProtectorPose,
self.blockProtectorDimension)
def lower_table(self):
#lowers the table to allow grasping into it
self.scene.remove_world_object("blockProtector")
def init_params(self):
try:
self.grasp_height = get_ros_param(
"GRASP_HEIGHT", "Grasp height defaulting to 0.01")
self.drop_height = get_ros_param("DROP_HEIGHT",
"Drop height defaulting to 0.07")
self.cruising_height = get_ros_param(
"CRUISING_HEIGHT", "Cruising height defaulting to 0.1")
self.pointing_height = get_ros_param(
"POINT_HEIGHT", "Pointing height defaulting to 0.06")
except ValueError as e:
rospy.loginfo(e)
def handle_param_update(self, message):
self.init_params()
def init_publishers(self):
self.calibration_publisher = rospy.Publisher("/calibration_results",
CalibrationParams,
queue_size=1)
self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs",
String,
queue_size=1)
rospy.sleep(0.5)
def init_subscribers(self):
self.joint_angle_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/joint_angles', JointAngles,
self.update_joints)
self.joint_trajectory_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/status', GoalStatusArray,
self.update_joint_trajectory_state)
self.joint_trajectory_goal_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/goal',
FollowJointTrajectoryActionGoal, self.update_joint_trajectory_goal)
self.joint_trajectory_result_subscriber = rospy.Subscriber(
'/j2s7s300/follow_joint_trajectory/result',
FollowJointTrajectoryActionResult,
self.update_joint_trajectory_result)
self.finger_position_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/finger_position', FingerPosition,
self.update_finger_position)
self.param_update_subscriber = rospy.Subscriber(
"/param_update", String, self.handle_param_update)
self.moveit_status_subscriber = rospy.Subscriber(
'/move_group/status', GoalStatusArray,
self.update_move_group_status)
self.move_it_feedback_subscriber = rospy.Subscriber(
'/move_group/feedback', MoveGroupActionFeedback,
self.update_move_group_state)
#Topic for getting joint torques
rospy.Subscriber('/j2s7s300_driver/out/joint_torques', JointAngles,
self.monitorJointTorques)
#Topic for getting cartesian force on end effector
rospy.Subscriber('/j2s7s300_driver/out/tool_wrench',
geometry_msgs.msg.WrenchStamped,
self.monitorToolWrench)
def init_action_servers(self):
self.calibration_server = actionlib.SimpleActionServer(
"calibrate_arm", CalibrateAction, self.calibrate, auto_start=False)
self.calibration_server.start()
self.move_block_server = actionlib.SimpleActionServer(
"move_block",
MoveBlockAction,
self.handle_move_block,
auto_start=False)
self.move_block_server.start()
#self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
self.move_pose_server = actionlib.SimpleActionServer(
"move_pose",
MovePoseAction,
self.handle_move_pose,
auto_start=False)
self.move_pose_server.start()
def init_services(self):
self.home_arm_service = rospy.Service("/home_arm", ArmCommand,
self.handle_home_arm)
# emergency stop
self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand,
self.handle_stop_arm)
# stop and pause for a bit
self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand,
self.handle_pause_arm)
self.start_arm_service = rospy.Service("/restart_arm", ArmCommand,
self.handle_restart_arm)
def init_action_clients(self):
# Action Client for joint control
joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
self.joint_action_client = actionlib.SimpleActionClient(
joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
rospy.loginfo('Waiting for ArmJointAnglesAction server...')
self.joint_action_client.wait_for_server()
rospy.loginfo('ArmJointAnglesAction Server Connected')
# Service to move the gripper fingers
finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
self.finger_action_client = actionlib.SimpleActionClient(
finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
self.finger_action_client.wait_for_server()
#
def init_service_clients(self):
self.is_simulation = False
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def handle_start_arm(self, message):
return self.restart_arm()
def handle_stop_arm(self, message):
return self.stop_motion()
def handle_pause_arm(self, message):
self.stop_motion(home=True, pause=True)
return str(self.paused)
def handle_restart_arm(self, message):
self.restart_arm()
self.paused = False
return str(self.paused)
def handle_home_arm(self, message):
try:
status = self.home_arm()
return json.dumps(status)
except rospy.ServiceException as e:
rospy.loginfo("Homing arm failed")
def home_arm(self):
# send the arm home
# for now, let's just use the kinova home
#self.home_arm_client()
self.home_arm_kinova()
return "done"
def custom_home_arm(self):
angles_set = [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 539.743164062
]
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angles_set[0]
goal.angles.joint2 = angles_set[1]
goal.angles.joint3 = angles_set[2]
goal.angles.joint4 = angles_set[3]
goal.angles.joint5 = angles_set[4]
goal.angles.joint6 = angles_set[5]
goal.angles.joint7 = angles_set[6]
self.joint_action_client.send_goal(goal)
def home_arm_kinova(self):
"""Takes the arm to the kinova default home if possible
"""
# self.arm.set_named_target("Home")
angles_set = map(np.deg2rad, [
629.776062012, 150.076568694, -0.13603515923, 29.8505859375,
0.172727271914, 212.423721313, 269.743164062
])
self.arm.clear_pose_targets()
try:
self.arm.set_joint_value_target(angles_set)
except MoveItCommanderException as e:
pass #stupid bug in movegroupcommander wrapper throws an exception when trying to set joint angles
try:
self.arm.go()
return "successful home"
except:
return "failed to home"
# This callback function monitors the Joint Torques and stops the current execution if the Joint Torques exceed certain value
def monitorJointTorques(self, torques):
if abs(torques.joint1) > 1:
return
#self.emergency_stop() #Stop arm driver
#rospy.sleep(1.0)
#self.group.stop() #Stop moveit execution
# This callback function monitors the Joint Wrench and stops the current
# execution if the Joint Wrench exceeds certain value
def monitorToolWrench(self, wrenchStamped):
return
#toolwrench = abs(wrenchStamped.wrench.force.x**2 + wrenchStamped.wrench.force.y**2 + wrenchStamped.wrench.force.z**2)
##print toolwrench
#if toolwrench > 100:
# self.emergency_stop() # Stop arm driver
def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
finger_max_turn = 6800
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float((finger1_pct / 100.0) * finger_max_turn)
goal.fingers.finger2 = float((finger2_pct / 100.0) * finger_max_turn)
goal.fingers.finger3 = float((finger3_pct / 100.0) * finger_max_turn)
self.finger_action_client.send_goal(goal)
if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
return self.finger_action_client.get_result()
else:
self.finger_action_client.cancel_all_goals()
rospy.loginfo('the gripper action timed-out')
return None
def move_joint_angles(self, angle_set):
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angle_set[0]
goal.angles.joint2 = angle_set[1]
goal.angles.joint3 = angle_set[2]
goal.angles.joint4 = angle_set[3]
goal.angles.joint5 = angle_set[4]
goal.angles.joint6 = angle_set[5]
goal.angles.joint7 = angle_set[6]
self.joint_action_client.send_goal(goal)
if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
return self.joint_action_client.get_result()
else:
print(' the joint angle action timed-out')
self.joint_action_client.cancel_all_goals()
return None
def handle_move_block(self, message):
"""msg format: {id: int,
source: Point {x: float,y: float},
target: Point {x: float, y: float}
"""
print(message)
pick_x = message.source.x
pick_y = message.source.y
pick_x_threshold = message.source_x_tolerance
pick_y_threshold = message.source_y_tolerance
block_id = message.id
place_x = message.target.x
place_y = message.target.y
place_x_threshold = message.target_x_tolerance
place_y_threshold = message.target_y_tolerance
self.move_block(block_id, pick_x, pick_y, pick_x_threshold,
pick_y_threshold, place_x, place_y, place_x_threshold,
place_y_threshold, message.block_size)
def handle_pick_failure(self, exception):
rospy.loginfo("Pick failed, going home.")
self.open_gripper()
self.home_arm()
raise exception
def handle_place_failure(self, safe_zone, block_size, exception):
#should probably figure out if I'm holding the block first so it doesn't look weird
#figure out how to drop the block somewhere safe
#pass none and none if you are certain you haven't picked up a block yet
if safe_zone is None and block_size is None:
self.home_arm()
raise exception
rospy.loginfo("HANDLING PLACE FAILURE")
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(
safe_zone[0]['x'],
safe_zone[0]['y'],
safe_zone[1]['x_tolerance'],
safe_zone[1]['y_tolerance'],
current_block_state,
block_size,
num_attempts=1000)
try:
arm_drop_location = tuio_to_arm(drop_location.x, drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("panic arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
except Exception as e:
rospy.loginfo(
"ERROR: Cannot panic place the block! Get ready to catch it!")
self.open_gripper()
self.home_arm()
raise exception
def get_candidate_blocks(self, block_id, pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
return candidate_blocks
def move_block(self,
block_id,
pick_x,
pick_y,
pick_x_tolerance,
pick_y_tolerance,
place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
block_size=None,
safe_zone=None,
pick_tries=2,
place_tries=1,
point_at_block=True):
if block_size is None:
block_size = get_ros_param('DEFAULT_BLOCK_SIZE')
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
candidate_blocks = []
print("looking for ", block_id, " ", pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance)
# get candidate blocks -- blocks with the same id and within the error bounds/threshold given
print(current_block_state)
# check for a drop location before trying to pick, do this before checking source to prevent cases where we go for a block user
# moved while we are checking for a drop location
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
None, None,
Exception("no room in the target zone to drop the block"))
# here we are actually building a set of candidate blocks to pick
for block in current_block_state:
print(
block,
self.check_block_bounds(block['x'], block['y'], pick_x, pick_y,
pick_x_tolerance, pick_y_tolerance))
if block['id'] == block_id and self.check_block_bounds(
block['x'], block['y'], pick_x, pick_y, pick_x_tolerance,
pick_y_tolerance):
candidate_blocks.append(block)
# select best block to pick and pick up
pick_location = None
if len(candidate_blocks) < 1:
raise Exception("no block of id " + str(block_id) +
" found within the source zone")
elif len(candidate_blocks) == 1:
pick_location = Point(candidate_blocks[0]['x'],
candidate_blocks[0]['y'], 0)
else:
pick_location = Point(*self.find_most_isolated_block(
candidate_blocks, current_block_state))
if point_at_block == True:
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.close_gripper()
self.point_at_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0))
self.point_at_block(location=Point(arm_drop_location[0],
arm_drop_location[1], 0))
self.home_arm()
except Exception as e:
rospy.loginfo(str(e))
rospy.loginfo("failed trying to point at block. giving up.")
self.home_arm()
self.move_block_server.set_succeeded(
MoveBlockResult(drop_location))
return
for i in range(pick_tries):
try:
arm_pick_location = tuio_to_arm(pick_location.x,
pick_location.y,
get_tuio_bounds(),
get_arm_bounds())
self.pick_block(location=Point(arm_pick_location[0],
arm_pick_location[1], 0),
check_grasp=True)
break
except Exception as e:
if i < pick_tries - 1:
rospy.loginfo("pick failed and trying again..." + str(e))
else:
rospy.loginfo("pick failed and out of attempts..." +
str(e))
self.handle_pick_failure(e)
if safe_zone == None:
if self.safe_zone == None:
safe_zone = [{
'x': pick_x,
'y': pick_y
}, {
'x_tolerance': pick_x_tolerance,
'y_tolerance': pick_y_tolerance
}]
else:
safe_zone = self.safe_zone
# calculate drop location
block_response = json.loads(self.get_block_state('tuio').tui_state)
current_block_state = block_response
drop_location = self.calculate_drop_location(place_x,
place_y,
place_x_tolerance,
place_y_tolerance,
current_block_state,
block_size,
num_attempts=2000)
if drop_location == None:
self.handle_place_failure(
safe_zone, block_size,
Exception("no room in the target zone to drop the block"))
rospy.loginfo("tuio drop" + str(drop_location))
for i in range(place_tries):
try:
arm_drop_location = tuio_to_arm(drop_location.x,
drop_location.y,
get_tuio_bounds(),
get_arm_bounds())
rospy.loginfo("arm drop: " + str(arm_drop_location))
self.place_block(
Point(arm_drop_location[0], arm_drop_location[1], 0))
break
except Exception as e:
if i < place_tries - 1:
rospy.loginfo("place failed and trying again..." + str(e))
else:
rospy.loginfo("place failed and out of attempts..." +
str(e))
# check to see if we've defined a safe zone to drop the blocks
self.handle_place_failure(safe_zone, block_size, e)
# assume success if we made it this far
self.move_block_server.set_succeeded(MoveBlockResult(drop_location))
# check if a certain x, y position is within the bounds of another x,y position
@staticmethod
def check_block_bounds(x, y, x_origin, y_origin, x_threshold, y_threshold):
if x <= x_origin + x_threshold and x >= x_origin - x_threshold \
and y <= y_origin + y_threshold and y >= y_origin - y_threshold:
return True
return False
# calculate the best location to drop the block
def calculate_drop_location(self,
x,
y,
x_threshold,
y_threshold,
blocks,
block_size,
num_attempts=1000):
attempt = 0
x_original, y_original = x, y
while (attempt < num_attempts):
valid = True
for block in blocks:
if self.check_block_bounds(block['x'], block['y'], x, y,
0.8 * block_size, block_size):
valid = False
break
if valid:
return Point(x, y, 0)
else:
x = random.uniform(x_original - x_threshold,
x_original + x_threshold)
y = random.uniform(y_original - y_threshold,
y_original + y_threshold)
attempt += 1
#if none found in num_attempts, return none
return None
# candidates should have more than one element in it
@staticmethod
def find_most_isolated_block(candidates, all_blocks):
print(candidates)
min_distances = [] # tuples of candidate, distance to closest block
for candidate in candidates:
min_dist = -1
for block in all_blocks:
if block['x'] == candidate['x'] and block['y'] == candidate[
'y']:
continue
else:
dist = DaArmServer.block_dist(candidate, block)
if min_dist == -1 or dist < min_dist:
min_dist = dist
min_distances.append([candidate, min_dist])
most_isolated, _ = max(min_distances, key=lambda x: x[
1]) # get most isolated candidate, and min_distance
return most_isolated['x'], most_isolated['y'], 0
@staticmethod
def block_dist(block_1, block_2):
return np.sqrt((block_2['x'] - block_1['x'])**2 +
(block_2['y'] - block_1['y'])**2)
def update_finger_position(self, message):
self.finger_positions = [
message.finger1, message.finger2, message.finger3
]
def check_grasp(self):
closed_pos = 0.95 * 6800
distance_from_closed = 0
for fp in self.finger_positions:
distance_from_closed += (closed_pos - fp)**2
if np.sqrt(distance_from_closed
) > 130: #this is just some magic number for now
return True #if the fingers aren't fully closed, then grasp is good
else:
return False
def open_gripper(self, delay=0):
"""open the gripper ALL THE WAY, then delay
"""
if self.is_simulation is True:
self.gripper.set_named_target("Open")
self.gripper.go()
else:
try:
rospy.loginfo("Opening Gripper!!")
self.move_fingers(50, 50, 50)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def close_gripper(self, delay=0):
"""close the gripper ALL THE WAY, then delay
"""
# self.gripper.set_named_target("Close")
# self.gripper.go()
try:
rospy.loginfo("Closing Gripper!!")
self.move_fingers(95, 95, 95)
except Exception as e:
rospy.loginfo("Caught it!!" + str(e))
rospy.sleep(delay)
def handle_gesture(self, gesture):
# lookup the gesture from a table? or how about a message?
# one possibility, object that contains desired deltas in pos/orientation
# as well as specify the arm or gripper choice
pass
def handle_move_pose(self, message):
# takes a geometry_msgs/Pose message
self.move_arm_to_pose(message.target.position,
message.target.orientation,
action_server=self.move_pose_server)
self.move_pose_server.set_succeeded()
def check_plan_result(self, target_pose, cur_pose):
#we'll do a very lenient check, this is to find failures, not error
#also only checking position, not orientation
rospy.loginfo("checking pose:" + str(target_pose) + str(cur_pose))
if np.abs(target_pose.pose.position.x -
cur_pose.pose.position.x) > self.GOAL_TOLERANCE * 2:
print("x error too far")
return False
if np.abs(target_pose.pose.position.y -
cur_pose.pose.position.y) > self.GOAL_TOLERANCE * 2:
print("y error too far")
return False
if np.abs(target_pose.pose.position.z -
cur_pose.pose.position.z) > self.GOAL_TOLERANCE * 2:
print("z error too far")
return False
print("tolerable error")
return True
# expects cooridinates for position to be in arm space
def move_arm_to_pose(self,
position,
orientation,
delay=0.5,
waypoints=[],
corrections=4,
action_server=None):
for i in range(corrections + 1):
rospy.loginfo("TRY NUMBER " + str(i))
if len(waypoints) > 0 and i < 1:
# this is good for doing gestures
plan, fraction = self.arm.compute_cartesian_path(
waypoints, eef_step=0.01, jump_threshold=0.0)
else:
p = self.arm.get_current_pose()
p.pose.position = position
p.pose.orientation = orientation
rospy.loginfo("PLANNING TO " + str(p))
self.arm.set_pose_target(p)
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
plan = self.arm.go(wait=False)
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 90 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 90 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
if (self.check_plan_result(p, self.arm.get_current_pose())):
break #we're there, no need to retry
#rospy.loginfo("OK GOT THROUGH THE PLANNING PHASE")
if False:
# # get the last pose to correct if desired
# ptPos = plan.joint_trajectory.points[-1].positions
# # print "=================================="
# # print "Last point of the current trajectory: "
# angle_set = list()
# for i in range(len(ptPos)):
# tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
# angle_set.append(tempPos)
if action_server:
pass
#action_server.publish_feedback(CalibrateFeedback("Plan Found"))
last_traj_goal = self.last_joint_trajectory_goal
rospy.loginfo("EXECUTING!")
self.arm.execute(plan, wait=False)
# this is a bit naive, but I'm going to loop until a new trajectory goal gets published
timeout = 5 / 0.001
while self.last_joint_trajectory_goal == last_traj_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Timeout waiting for kinova to accept movement goal."
))
rospy.loginfo("KINOVA GOT THE MOVEMENT GOAL")
current_goal = self.last_joint_trajectory_goal
# then loop until a result for it gets published
timeout = 15 / 0.001
while self.last_joint_trajectory_result != current_goal:
rospy.sleep(0.001)
timeout -= 1
if timeout <= 0:
raise (Exception(
"Motion took longer than 15 seconds. aborting..."))
if self.paused is True:
self.arm.stop() # cancel the moveit goals
return
rospy.loginfo("LEAVING THE WHILE LOOP")
# for i in range(corrections):
# rospy.loginfo("Correcting the pose")
# self.move_joint_angles(angle_set)
rospy.sleep(delay)
else:
if action_server:
#action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
pass
# Let's have the caller handle sequences instead.
# def handle_move_sequence(self, message):
# # if the move fails, do we cancel the sequence
# cancellable = message.cancellable
# moves = message.moves
# for move in moves:
# try:
# self.handle_move_block(move)
# except Exception:
# if cancellable:
# rospy.loginfo("Part of move failed, cancelling the rest.")
# break
def update_move_group_state(self, message):
rospy.loginfo(message.feedback.state)
self.move_group_state = message.feedback.state
def update_move_group_status(self, message):
if message.status_list:
#rospy.loginfo("MoveGroup Status for "+str(message.status_list[0].goal_id.id)+": "+str(message.status_list[0].status))
self.move_group_status = message.status_list[0].status
def update_joint_trajectory_state(self, message):
# print(message.status_list)
if len(message.status_list) > 0:
self.joint_trajectory_state = message.status_list[0].status
else:
self.joint_trajectory_state = 0
def update_joint_trajectory_goal(self, message):
#print("goalisasis", message.goal_id.id)
self.last_joint_trajectory_goal = message.goal_id.id
def update_joint_trajectory_result(self, message):
#print("resultisasis", message.status.goal_id.id)
self.last_joint_trajectory_result = message.status.goal_id.id
def get_grasp_orientation(self, position):
#return Quaternion(0, 0, 1/np.sqrt(2), 1/np.sqrt(2))
return Quaternion(-0.707388, -0.706825, 0.0005629, 0.0005633)
def update_joints(self, joints):
self.joint_angles = [
joints.joint1, joints.joint2, joints.joint3, joints.joint4,
joints.joint5, joints.joint6, joints.joint7
]
def move_z_relative(self, distance):
p = self.arm.get_current_pose()
p.pose.position.z += distance
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def move_z_absolute(self, height):
p = self.arm.get_current_pose()
p.pose.position.z = height
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def pick_block(self,
location,
check_grasp=False,
retry_attempts=0,
delay=0,
action_server=None):
# go to a spot and pick up a block
# if check_grasp is true, it will check torque on gripper and retry or fail if not holding
# open the gripper
# print('Position: ', position)
self.open_gripper()
position = Point(location.x, location.y, self.cruising_height)
orientation = self.get_grasp_orientation(position)
try:
self.raise_table()
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
position = Point(location.x, location.y, self.grasp_height)
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.crusing_height)
self.raise_table()
raise (
e
) #problem because sometimes we get exception e.g. if we're already there
# and it will skip the close if so.
if action_server:
action_server.publish_feedback()
self.close_gripper()
self.move_z_absolute(self.cruising_height)
#try to wait until we're up to check grasp
rospy.sleep(0.5)
if check_grasp is True:
if (self.check_grasp() is False):
print("Grasp failed!")
self.raise_table()
raise (Exception("grasp failed!"))
# for now, but we want to check finger torques
# for i in range(retry_attempts):
# self.move_z(0.3)
self.raise_table()
rospy.sleep(delay)
def place_block(self,
location,
check_grasp=False,
delay=0,
action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.drop_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.open_gripper()
self.move_z_absolute(self.cruising_height)
self.raise_table()
self.close_gripper()
def point_at_block(self, location, delay=0, action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
# print('Position: ', position)
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < -.02: # if I'm significantly below cruisng height, correct
self.move_z_absolute(self.cruising_height)
position = Point(location.x, location.y, self.cruising_height)
rospy.loginfo("PLACE POSITION: " + str(position) + "(DROP HEIGHT: " +
str(self.drop_height))
orientation = self.get_grasp_orientation(position)
try:
self.raise_table(
) # only do this as a check in case it isn't already raised
self.move_arm_to_pose(position,
orientation,
delay=0,
action_server=action_server)
self.lower_table()
self.move_z_absolute(self.pointing_height)
self.move_z_absolute(self.cruising_height)
except Exception as e:
current_pose = self.arm.get_current_pose()
if current_pose.pose.position.z - self.cruising_height < 0:
self.move_z_absolute(self.cruising_height)
self.raise_table()
raise (e)
if action_server:
action_server.publish_feedback()
self.raise_table()
def stop_motion(self, home=False, pause=False):
rospy.loginfo("STOPPING the ARM")
# cancel the moveit_trajectory
# self.arm.stop()
# call the emergency stop on kinova
self.emergency_stop()
# rospy.sleep(0.5)
if pause is True:
self.paused = True
if home is True:
# self.restart_arm()
self.home_arm()
def calibrate(self, message):
print("calibrating ", message)
self.place_calibration_block()
rospy.sleep(5) # five seconds to correct the drop if it bounced, etc.
print("moving on...")
self.calibration_server.publish_feedback(
CalibrateFeedback("getting the coordinates"))
params = self.record_calibration_params()
self.set_arm_calibration_params(params[0], params[1])
self.calibration_publisher.publish(
CalibrationParams(params[0], params[1]))
self.calibration_server.set_succeeded(CalibrateResult(params))
def set_arm_calibration_params(self, arm_x_min, arm_y_min):
rospy.set_param("ARM_X_MIN", arm_x_min)
rospy.set_param("ARM_Y_MIN", arm_y_min)
def place_calibration_block(self):
# start by homing the arm (kinova home)
self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
self.home_arm_kinova()
# go to grab a block from a human
self.open_gripper()
self.close_gripper()
self.open_gripper(4)
self.close_gripper(2)
# move to a predetermined spot
self.calibration_server.publish_feedback(
CalibrateFeedback("moving to drop"))
try:
self.move_arm_to_pose(Point(0.4, -0.4, 0.1),
Quaternion(1, 0, 0, 0),
corrections=0)
except Exception as e:
rospy.loginfo("THIS IS TH PRoblem" + str(e))
# drop the block
self.open_gripper()
self.calibration_server.publish_feedback(
CalibrateFeedback("dropped the block"))
calibration_block = {'x': 0.4, 'y': -0.4, 'id': 0}
calibration_action_belief = {
"action": "add",
"block": calibration_block
}
self.action_belief_publisher.publish(
String(json.dumps(calibration_action_belief)))
rospy.loginfo("published arm belief")
return
def record_calibration_params(self):
""" Call the block_tracker service to get the current table config.
Find the calibration block and set the appropriate params.
"""
# make sure we know the dimensions of the table before we start
# fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
tuio_bounds = get_tuio_bounds()
arm_bounds = get_arm_bounds(calibrate=False)
try:
block_state = json.loads(self.get_block_state("tuio").tui_state)
except rospy.ServiceException as e:
# self.calibration_server.set_aborted()
raise (ValueError("Failed getting block state to calibrate: " +
str(e)))
if len(block_state) != 1:
# self.calibration_server.set_aborted()
raise (ValueError(
"Failed calibration, either couldn't find block or > 1 block on TUI!"
))
# if we made it here, let's continue!
# start by comparing the reported tuio coords where we dropped the block
# with the arm's localization of the end-effector that dropped it
# (we assume that the block fell straight below the drop point)
drop_pose = self.arm.get_current_pose()
end_effector_x = drop_pose.pose.position.x
end_effector_y = drop_pose.pose.position.y
block_tuio_x = block_state[0]['x']
block_tuio_y = block_state[0]['y']
x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y,
tuio_bounds)
arm_x_min = end_effector_x - x_ratio * arm_bounds["x_dist"]
arm_y_min = end_effector_y - y_ratio * arm_bounds["y_dist"]
return [arm_x_min, arm_y_min]
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param(
'/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber(
'/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
self.listener = tf.TransformListener()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.position_list = []
self.orientation_list = []
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.5)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
'''
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
'''
marker_joint_goal = [
4.721744537353516, -0.7451499144183558, -1.6199515501605433,
-1.2175200621234339, 1.6366002559661865, 3.1263363361358643
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
'''
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
'''
def move_code2(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose(1) : ", self.wpose
marker_joint_goal = [
4.768651485443115, -1.1165898481952112, -2.1672890822040003,
5.898628234863281, 1.7003079652786255, 3.2297513484954834
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose(2) : ", self.wpose
def plan_cartesian_path(self, scale=1.0):
waypoints = []
'''
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
'''
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x += scale * 0.009
self.wpose.position.y += scale * 0.07
self.wpose.position.z -= scale * 0.06
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
'''
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
'''
return plan, fraction
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
detection_start = time.time()
rospy.Subscriber('/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
print("detection time", format(time.time() - detection_start))
detection_start = time.time()
def collectJsk(self, msg):
# detection_start = time.time()
try:
detection_start = time.time()
(trans,
rot) = self.listener.lookupTransform('base_link', 'yolo_output00',
rospy.Time(0))
self.goal_x = trans[0]
self.goal_y = trans[1]
self.goal_z = trans[2]
self.goal_ori_x = rot[0]
self.goal_ori_y = rot[1]
self.goal_ori_z = rot[2]
self.goal_ori_w = rot[3]
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [
self.goal_ori_x, self.goal_ori_y, self.goal_ori_z,
self.goal_ori_w
]
(self.goal_roll, self.goal_pitch,
self.goal_yaw) = euler_from_quaternion(self.orientation_list)
except:
return
print("detection time", format(time.time() - detection_start))
# print("detection time",format(time.time()-detection_start))
def go_to_move(self, scale=1.0):
'''
planning_frame = self.robot_arm.get_planning_frame()
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
'''
self.calculated_tomato.position.x = (scale * self.goal_x)
self.calculated_tomato.position.y = (scale * self.goal_y)
self.calculated_tomato.position.z = (scale * self.goal_z)
self.calculated_tomato.orientation = Quaternion(
*quaternion_from_euler(3.14, 0, 1.57))
print("============ tomato_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_fraction) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("======= Press `Enter` to if plan in correct!======")
raw_input()
self.robot_arm.go(True)
'''
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
'''
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.target_ar_id = 9
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
"""
move to initial pose of the UR3
"""
#self.clear_octomap()
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print "====== current pose : ", self.wpose
marker_joint_goal = [
-0.535054565144069, -2.009213503260451, 1.8350906250920112,
-0.7794355413099039, -0.7980899690645948, 0.7782740454087982
]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def move_moveit_setting_pose(self, pose_name):
"""
move to one of the moveit_setup pose: "home", "zeros", "table"
"""
if pose_name == "home":
self.robot_arm.set_named_target("home")
elif pose_name == "zeros":
self.robot_arm.set_named_target("zeros")
elif pose_name == "table":
self.robot_arm.set_named_target("table")
#print "Press the Enter"
#raw_input()
self.robot_arm.go(wait=True)
def go_to_move(self, scale=1.0):
"""
manipulator is moving to the target ar_marker. \n
the start ar_marker id is 9, goal ar_marker id is 10.
"""
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
coke_offset = [0, -0.35, -0.1] #x y z
# gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품.
# linear offset = abs([0, 0.0625, 0.13])
robot_base_offset = 0.873
base_wrist2_offset = 0.1 #for avoiding link contact error
if self.target_ar_id == 9:
print ">> robot arm plannig frame: \n", planning_frame
print ">> move mode id: ", self.target_ar_id
self.calculed_coke_pose.position.x = (
scale * self.goal_x) # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale *
self.goal_y) + coke_offset[1]
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(3.14, 0, 1.57))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
elif self.target_ar_id == 10:
print ">> robot arm plannig frame: \n", planning_frame
print ">> move mode id: ", self.target_ar_id
self.calculed_coke_pose.position.x = (scale *
self.goal_x) + coke_offset[1]
self.calculed_coke_pose.position.y = (scale * self.goal_y) + 0
self.calculed_coke_pose.position.z = (
scale * self.goal_z
) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(
*quaternion_from_euler(3.14, 0, 0))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [
self.calculed_coke_pose.position.x,
self.calculed_coke_pose.position.y,
self.calculed_coke_pose.position.z
]
tf_display_orientation = [
self.calculed_coke_pose.orientation.x,
self.calculed_coke_pose.orientation.y,
self.calculed_coke_pose.orientation.z,
self.calculed_coke_pose.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
coke_waypoints = []
coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose))
(coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(
coke_waypoints, 0.01, 0.0)
self.display_trajectory(coke_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch,
yaw):
"""
Manipulator is moving to the desired coordinate. \n
Now move to the ar_10 marker.
"""
calculed_ar_id_10 = Pose()
#desired_goal_pose = [0.171, -0.113, 1.039]
#desired_goal_euler = [3.14, 0.17, 0]
desired_goal_pose = [pose_x, pose_y, pose_z]
desired_goal_euler = [roll, pitch, yaw]
Cplanning_frame = self.robot_arm.get_planning_frame()
print ">> current planning frame: \n", Cplanning_frame
calculed_ar_id_10.position.x = desired_goal_pose[0] + 0.1
calculed_ar_id_10.position.y = desired_goal_pose[1]
calculed_ar_id_10.position.z = desired_goal_pose[2]
calculed_ar_id_10.orientation = Quaternion(*quaternion_from_euler(
desired_goal_euler[0], desired_goal_euler[1],
desired_goal_euler[2]))
print ">>> ar id 10 goal frame: ", desired_goal_pose
self.robot_arm.set_pose_target(calculed_ar_id_10)
tf_display_position = [
calculed_ar_id_10.position.x, calculed_ar_id_10.position.y,
calculed_ar_id_10.position.z
]
tf_display_orientation = [
calculed_ar_id_10.orientation.x, calculed_ar_id_10.orientation.y,
calculed_ar_id_10.orientation.z, calculed_ar_id_10.orientation.w
]
jj = 0
while jj < 5:
jj += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
ar_id_10_waypoints = []
ar_id_10_waypoints.append(copy.deepcopy(calculed_ar_id_10))
(ar_id_10_plan,
ar_id_10_fraction) = self.robot_arm.compute_cartesian_path(
ar_id_10_waypoints, 0.01, 0.0)
self.display_trajectory(ar_id_10_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
"""
Visualized trajectory of the manipulator
"""
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0):
"""
Working on the "linear move": x-axis -> y-axis -> z-axis
"""
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
#print "waypoints:", waypoints
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
"""
Working on the "linear move": x-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
"""
Working on the "linear move": y-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
"""
Working on the "linear move": z-axis
"""
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
#print "===== robot arm pose: ", self.wpose
self.wpose.position.z = (scale *
self.wpose.position.z) + z_offset #-0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
"""
execute planned "plan" trajectory.
"""
group = self.robot_arm
group.execute(plan, wait=True)
def ar_pose_subscriber(self):
"""
ar_maker subscriber
"""
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
#print "======= pos(meter): ", self.position_list
#print "======= orientation: ", self.orientation_list
def ar_tf_listener(self, msg):
"""
Listening the ar_marker pose coordinate.
"""
try:
self.marker = msg.markers
ml = len(self.marker)
target_start_point_id = self.target_ar_id
#target_id = target_ar_id
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_start_point_id:
pass
#target_id_flage = False
elif self.m_idd == target_start_point_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[
target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[
target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[
target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[
target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[
target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[
target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[
target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [
self.ar_pose.orientation.x, self.ar_pose.orientation.y,
self.ar_pose.orientation.z, self.ar_pose.orientation.w
]
(self.goal_roll,
self.goal_pitch, self.goal_yaw) = euler_from_quaternion(
self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
class MoveItObstaclesDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
self.arm = MoveGroupCommander('manipulator')
# 设置机械臂工作空间
self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100])
# 设置机械臂最大速度
self.arm.set_max_velocity_scaling_factor(value=0.1)
# 获取终端link的名称
self.end_effector_link = self.arm.get_end_effector_link()
rospy.loginfo('end effector link: {}'.format(self.end_effector_link))
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
# self.arm.allow_replanning(True)
self.arm.set_num_planning_attempts(10)
# self.arm.allow_looking(True)
# 设置目标位置所使用的参考坐标系
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
# 设置每次运动规划的时间限制：5s
self.arm.set_planning_time(3)
# 控制机械臂先回到初始化位置
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(2)
def planning(self, start_point, end_point):
"""
功能：动态避障
:param start_point: 起始点, type: dict
:param end_point: 终点, type: dict
:return: None
"""
# 移动机械臂到指定位置，并获取当前位姿数据为机械臂运动的起始位姿
if start_point:
self.move_arm(p=start_point)
self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose
# 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中
# way_points = self.get_way_points(start_point, end_point)
while True:
print('set planner id: RRT')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('up')
self.arm.go()
rospy.sleep(5)
print('set planner id: PRM')
self.arm.set_planner_id('TRRTkConfigDefault')
self.arm.set_named_target('home')
self.arm.go()
rospy.sleep(5)
# -------------------------------------------------------------------
def get_way_points(self, a, b):
way_points = []
# plan 1
self.arm.set_named_target('up')
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
else:
rospy.logerr("Trajectory is empty. Planning false!")
self.arm.clear_pose_targets()
# plan 2
# traj = self.moveit_planning(p=b)
for i, p in enumerate(traj.joint_trajectory.points):
# rospy.loginfo('waypoint: {}'.format(i))
if i%2 == 0:
point = {
'x': p.positions[0],
'y': p.positions[1],
'z': p.positions[2],
'ox': p.positions[3],
'oy': p.positions[4],
'oz': p.positions[5]
}
way_points.append(point)
rospy.loginfo('waypoint: \n {}'.format(way_points))
rospy.loginfo(len(way_points))
return way_points
def moveit_planning(self, p):
"""
使用moveit求解器规划路径
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start moveit planning...')
target_pose = PoseStamped()
target_pose.header.frame_id = self.reference_frame
target_pose.pose.position.x = p['x']
target_pose.pose.position.y = p['y']
target_pose.pose.position.z = p['z']
if 'ox' in p.keys() and p['ox']:
target_pose.pose.orientation.x = p['ox']
if 'oy' in p.keys() and p['oy']:
target_pose.pose.orientation.y = p['oy']
if 'oz' in p.keys() and p['oz']:
target_pose.pose.orientation.z = p['oz']
if 'ow' in p.keys() and p['ow']:
target_pose.pose.orientation.w = p['ow']
# 传入一个PoseStamped
# self.arm.set_pose_target(target_pose, self.end_effector_link)
# 尝试直接传入一个列表
self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link)
traj = self.arm.plan()
if traj.joint_trajectory.points: # True if trajectory contains points
rospy.loginfo("get trajectory success")
return traj
else:
rospy.logerr("Trajectory is empty. Planning false!")
def move_arm(self, p):
"""
移动机械臂到p点
:param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
:return:
"""
rospy.loginfo('start arm moving...')
traj = self.moveit_planning(p)
self.arm.execute(traj)
# 设置当前位置为起始位置
self.arm.set_start_state_to_current_state()
rospy.sleep(1)
def stop_arm(self):
"""
急停
:return:
"""
pass
def exist_danger_obstacle(self):
"""
环境中是否存在危险的障碍物
:return: True or False
"""
return False
def get_obstacle_octomap(self):
"""
获取环境的octomap信息
:return:
"""
pass
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_roll = 0
self.goal_pitch = 0
self.goal_yaw = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.wpose = []
self.marker = []
self.tf_list = []
self.m_idd = []
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.trans = []
self.rot = []
self.pose_goal = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def ar_callback(self, msg):
#marker = msg.markers[1]
self.marker = msg.markers
ml = len(self.marker)
m_id = self.marker[0].id
#print "marker length: ", ml, ml-1
#print "marker id: ", m_id, self.marker[1].id
#self.m_idd = []
#self.m_pose_x = []
#self.m_pose_y = []
#self.m_pose_z = []
#self.m_ori_w = []
#self.m_ori_x = []
#self.m_ori_y = []
#self.m_ori_z = []
for ii in range(0, ml):
self.m_idd.append(self.marker[ii].id)
self.m_pose_x.append(self.marker[ii].pose.pose.position.x)
self.m_pose_y.append(self.marker[ii].pose.pose.position.y)
self.m_pose_z.append(self.marker[ii].pose.pose.position.z)
self.m_ori_w.append(self.marker[ii].pose.pose.orientation.w)
self.m_ori_x.append(self.marker[ii].pose.pose.orientation.x)
self.m_ori_y.append(self.marker[ii].pose.pose.orientation.y)
self.m_ori_z.append(self.marker[ii].pose.pose.orientation.z)
'''
print "id: ", self.m_idd
print "pose_x: ", self.m_pose_x
print "pose_y: ", self.m_pose_y
print "pose_z: ", self.m_pose_z
print "ori_w: ", self.m_ori_w
print "ori_x: ", self.m_ori_x
print "ori_y: ", self.m_ori_y
print "ori_z: ", self.m_ori_z
'''
m_pose_goal = Pose()
m_pose_goal.orientation = Quaternion(*quaternion_from_euler(0, -1.5, 0))
m_pose_goal.position.x = self.m_pose_x[0] + 0.029# red line 0.2 0.2
m_pose_goal.position.y = self.m_pose_y[0] - 0.7 # green line 0.15 0.15
m_pose_goal.position.z = self.m_pose_z[0] + 0.3 # blue line # 0.35 0.6
'''
print "goal_pose: ", m_pose_goal
self.robot_arm.set_pose_target(m_pose_goal)
self.robot_arm.go(True)
print "==== check check"
marker_joint_goal = [0.07100913858593216, -1.8767615298285376, 2.0393206555899503, -1.8313959190971882, -0.6278395875738125, 1.6918219826764682]
self.robot_arm.go(marker_joint_goal, wait=True)
print "====== robot joint value: \n"
print marker_joint_goal
#self.robot_arm.go(marker_joint_goal, wait=True)
pos_x = self.marker[0].pose.pose.position.x
pos_y = self.marker[0].pose.pose.position.y
pos_z = self.marker[0].pose.pose.position.z
dist = math.sqrt(
(pos_x*pos_x) + (pos_y * pos_y)
)
'''
#ori_x = marker.pose.pose.orientation.x
#ori_y = marker.pose.pose.orientation.y
#ori_z = marker.pose.pose.orientation.z
#ori_w = marker.pose.pose.orientation.w
#print(m_id)
#print "==========="
#print 'id: ', m_id
#print 'pos: ', pos_x, pos_y, pos_z
#print('ori: ', ori_x, ori_y, ori_z, ori_w)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose : ", self.wpose
#self.pose_goal = self.robot_arm.get_current_pose()
self.pose_goal.position.x = 0.062
self.pose_goal.position.y = 0.194
self.pose_goal.position.z = 0.878
self.pose_goal.orientation.x = 0.673
self.pose_goal.orientation.y = 0.673
self.pose_goal.orientation.z = -0.217
self.pose_goal.orientation.w = 0.217
#print "========== goal frame: ", self.pose_goal
self.robot_arm.set_pose_target(self.pose_goal)
self.robot_arm.go(True)
def look_up_down(self):
self.clear_octomap()
print "======== clear_octomap"
look_up_down = self.robot_arm.get_current_joint_values()
#print "before: ", look_up_down
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
def plan_cartesian_path(self, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
#self.wpose.position.z -= scale * (0.695) # First move up (z)
#self.wpose.position.y += scale * (0.199) # and sideways (y)
#waypoints.append(copy.deepcopy(self.wpose))
print "===== robot arm pose: ", self.wpose
''' # 19.06.11 10:45 분 코딩
print self.wpose.position.x," + (",scale," * ",self.goal_x,")"
print self.wpose.position.y," + (",scale," * ",self.goal_y,")"
self.wpose.position.x = self.wpose.position.x - (scale * self.goal_x)
self.wpose.position.y = self.wpose.position.y + (scale * self.goal_y)
print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
print self.wpose.position.z," + (",scale," * ",self.goal_z,")"
self.wpose.position.z += (scale * self.goal_z)
print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
'''
self.wpose.position.x = (scale * self.wpose.position.x) - 0.10
#self.wpose.position.y = (scale * self.goal_y) - 0.1
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
#print self.wpose.position.z," + (",scale," * ",self.goal_z,")"
#self.wpose.position.z = (scale * self.goal_z) + 0.1
self.wpose.position.y = (scale * self.goal_y) + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
rospy.loginfo("Maker messages detected. Starting followers...")
def go_to_move(self, scale = 1.0):
self.wpose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.pose_goal.position.x = (scale * self.goal_x) + 0.10 # base_link to wrist2 x-offset
self.pose_goal.position.y = (scale * self.goal_y) - 0.25
self.pose_goal.position.z = (scale * self.goal_z) + 0.72 - 0.115 # world to base_link z-offset and coke can offset
self.pose_goal.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54))
''' 19.06.1 15:58 코딩
self.pose_goal.orientation.x = self.rot[0]
self.pose_goal.orientation.y = self.rot[1]
self.pose_goal.orientation.z = self.rot[2]
self.pose_goal.orientation.w = self.rot[3]
'''
#self.pose_goal.orientation.x = self.goal_roll + 1.57
#self.pose_goal.orientation.y = self.goal_pitch + 1.57
#self.pose_goal.orientation.z = self.goal_yaw
print "========== goal frame: ", self.pose_goal
self.robot_arm.set_pose_target(self.pose_goal)
#self.robot_arm.go(True)
def tf_listener(self): # 수정사안: ar_pose id의 가져와서 위치 값을 읽어와야함.
try:
child_tf = '/ar_marker_9'
parent_tf = '/base_link'
(self.trans,self.rot) = self.listener.lookupTransform(parent_tf, child_tf, rospy.Time(0))
# rosrun tf tf_echo /base_link /ar_marker_9 랑 같은 역할
# transformation: linear transformation, rotation: quaternion
print "======= trans[x, y, z]: ", self.trans
print "======= rotat[x, y, z, w]: ", self.rot
(self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.rot)
print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#convert 확인용
#quat = quaternion_from_euler(goal_roll, goal_pitch, goal_yaw)
#print quat
self.goal_x = self.trans[0]
self.goal_y = self.trans[1]
self.goal_z = self.trans[2]
except Exception as ex:
print "======== tf_listener Exception!!"
rospy.loginfo(ex)
def look_object(self):
try:
look_object = self.robot_arm.get_current_joint_values()
#look_object[4] = look_object[4] + (math.radians(90)) # wrist2
look_object[5] = look_object[5] + (3.1) # wrist3
#look_object[3] = look_object[3] - (math.radians(00)) # wrist1
self.robot_arm.go(look_object, wait=True)
#look_object[5] = look_object[5] + (math.radians(90)) # wrist3
#self.robot_arm.go(look_object, wait=True)
except Exception as ex:
print "======== look_object Exception!!"
rospy.loginfo(ex)
class Arm():
def __init__(self):
self.p = Pose()
self.gripper = MoveGroupCommander("onine_gripper")
self.arm = MoveGroupCommander("onine_arm")
self.arm.set_goal_tolerance(0.004)
self.arm.allow_replanning(True)
# self.arm.set_goal_position_tolerance(0.005)
# self.arm.set_goal_orientation_tolerance(0.1)
self.arm.set_num_planning_attempts(10)
self.arm.set_planning_time(5)
self.arm.set_planner_id("RRTkConfigDefault")
def go(self, x, y, z, roll, pitch, yaw):
self.p.position.x = x
self.p.position.y = y
self.p.position.z = z
self.p.orientation = Quaternion(
*quaternion_from_euler(roll, pitch, yaw))
self.arm.set_pose_target(self.p)
os.system("rosservice call clear_octomap")
rospy.loginfo("Moving to arm target")
self.arm.go(wait=True)
rospy.sleep(1)
def get_valid_pose(self, x, y, z, distance):
origin_translation = [0.095, 0.00, 0.00]
delta_x = x - origin_translation[0]
delta_y = y - origin_translation[1]
theta = math.atan(delta_y / delta_x)
grasp_yaw = theta
grasp_x = x + (distance * math.cos(theta))
grasp_y = y + (distance * math.sin(theta))
return (grasp_x, grasp_y, z, grasp_yaw)
def ready(self):
self.arm.set_named_target("onine_ready")
self.arm.go(wait=True)
def home(self):
self.arm.set_named_target("onine_home")
self.arm.go(wait=True)
def feed_pos(self):
self.arm.set_named_target("feed_pos")
self.arm.go(wait=True)
def tilt_food(self):
self.arm.set_named_target("tilt_food")
self.arm.go(wait=True)
def open_gripper(self):
os.system("rostopic pub /onine_gripper std_msgs/Float64 0.085 -1")
os.system("rosservice call clear_octomap")
def close_gripper(self):
os.system("rostopic pub /onine_gripper std_msgs/Float64 0.035 -1")
os.system("rosservice call clear_octomap")
def pickup_sim(self, x, y, z):
# self.ready()
self.open_gripper()
(aim_x, aim_y, aim_z,
aim_yaw) = self.get_valid_pose(x, y, z + 0.15, 0.00)
self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw)
(aim_x, aim_y, aim_z, aim_yaw) = self.get_valid_pose(x, y, z, 0.00)
self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw)
self.close_gripper()
class UR5_Gripped_Manipulator:
"""
Class Attributes
Robot : Robot Commander Object
Scene : Planning Scene Interface Object (current scene)
Man : MoveGroupCommander object to control manipulator
EEF : MoveGroupCommander object to control endeffector
Target_poses : Pose list of created target objects
Picked : List of part indexes to ensure to picked non-picked ones
"""
def __init__(self, manip_name="manipulator", eef_name="endeffector"):
self.robot = RobotCommander()
self.man = MoveGroupCommander(manip_name)
self.eef = MoveGroupCommander(eef_name)
self.target_poses = []
self.picked = []
"""
----------------Function Name: clean_scene----------------
Definition: Clears the target_poses and picked lists in order to establish a new execution. Necessary for test_simulation method
"""
def clear_poses(self):
for i in xrange(len(self.target_poses)):
self.target_poses.pop()
for i in xrange(len(self.picked)):
self.picked.pop()
"""
----------------Function Name: clean_scene----------------
Definition : Clears all non-attached objects from planning scene
Group : MoveGroupCommander object to control given group
"""
def default_state_gripper(self, grp):
rospy.loginfo("Openning Gripper")
joint_vals = grp.get_current_joint_values()
joint_vals[0] = 0.0
grp.set_joint_value_target(joint_vals)
init_plan = grp.plan()
return grp.execute(init_plan)
"""
----------------Function Name: closed_state_gripper----------------
Definition : Function that opens gripper and detachs the gripped object
Obj : Name of the Object that is needed to detach
"""
def closed_state_gripper(self, curr_scene, obj):
rospy.loginfo("Closing Gripper")
def convert(width):
return 0.77 - width / 0.15316 * 0.77
width = curr_scene.get_objects([obj])[obj].primitives[0].dimensions[0]
width = convert(width)
now = self.robot.endeffector.get_current_joint_values()[0]
cnt = 0
while abs(now - width) > 0.05:
now = self.robot.endeffector.get_current_joint_values()[0]
cnt = cnt + 1
tmp = width - abs(now - width) / 2.0
self.robot.endeffector.set_joint_value_target('finger_joint', tmp)
self.robot.endeffector.go()
rospy.sleep(0.05)
if cnt == 7:
break
rospy.sleep(1.0)
ret = self.robot.manipulator.attach_object(obj)
return ret
"""
----------------Function Name: set_mid_state----------------
Definition : Motion planning function for defined mid state for manipulator, can be used for enhancing the performance if needed
Group : MoveGroupCommander object to control given group
"""
def set_mid_state(self, group):
rospy.loginfo("Going to Mid-State")
pos = PoseStamped()
pos.header.frame_id = self.robot.get_planning_frame()
pos.pose.position.x = 0.45358
pos.pose.position.y = 0.188518
pos.pose.position.z = 0.473063
pos.pose.orientation.x = 0.0004738
pos.pose.orientation.y = 1
pos.pose.orientation.z = 0
pos.pose.orientation.w = 0.000155894
group.set_pose_target(pos)
move_plan = group.plan()
while (not move_plan.joint_trajectory.joint_names):
move_plan = group.plan()
return group.execute(move_plan)
"""
----------------Function Name: add_object----------------
Name : Object Name
Pose : Pose of the Object (x,y,z,xo,yo,zo,wo)
Dimension : Dimensions of the Obhect (Tuple) (d1,d2,d3),
Type : Box(0),Sphere(1)
d1 is radius for sphere i.e typ==1,
"""
def add_object(self,
name,
x,
y,
z,
xo=0.0,
yo=0.0,
zo=0.0,
wo=0.0,
d1=0.1,
d2=0.1,
d3=0.1,
curr_scene=None,
typ=0):
pos = PoseStamped()
pos.header.frame_id = self.robot.get_planning_frame()
pos.pose.position.x = x
pos.pose.position.y = y
pos.pose.position.z = z
pos.pose.orientation.x = xo
pos.pose.orientation.y = yo
pos.pose.orientation.z = zo
pos.pose.orientation.w = wo
rospy.loginfo("ADDING OBJECT")
if (typ == 0):
curr_scene.add_box(name, pos, (d1, d2, d3))
elif (typ == 1):
curr_scene.add_sphere(name, pos, d1)
else:
print("ERROR in Type")
return pos
"""
----------------Function Name: clean_scene----------------
Definition: Clears all non-attached objects from planning scene
"""
def clean_scene(self, curr_scene):
rospy.loginfo("Clearing the Scene")
curr_scene.remove_world_object()
"""
----------------Function Name: create_environment----------------
Definition : Creates the simulation environment with non-target collision objects, can be edited to create different default environment
"""
def create_environment(self, curr_scene):
self.add_object(name="wall",
x=0.0,
y=0.8,
z=0.5,
d1=0.1,
d2=0.35,
d3=1,
curr_scene=curr_scene,
typ=0)
self.add_object(name="wall_2",
x=0.0,
y=-0.8,
z=0.5,
d1=0.1,
d2=0.35,
d3=1,
curr_scene=curr_scene,
typ=0)
self.add_object(name="table",
x=0.0,
y=0.0,
z=-0.05,
d1=2,
d2=2,
d3=0.0001,
curr_scene=curr_scene,
typ=0)
"""
----------------Function Name: check_targets----------------
Definition : Creates given number of objects within the distance provided. It prevents collision object overlapping.
It returns the pose list of the objects
Number : Number of collision objects required to spawn
Distance : Required minimum distance between each collision objects
"""
def check_targets(self, curr_scene, number, distance):
rospy.loginfo("Creating Boxes!")
def al(typ, x=0.0):
if typ == 'x':
return random.uniform(0.35, 0.65)
elif typ == "y":
rang = math.sqrt(0.7**2 - x**2)
return random.uniform(-rang, rang)
hemme = []
dims = []
cnt = 0
while len(hemme) != number:
if cnt == 200:
hemme = []
cnt = 0
cnt = cnt + 1
dim = (random.uniform(0.08, 0.12), random.uniform(0.08, 0.10),
random.uniform(0.05, 0.2))
quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, random.uniform(-math.pi, math.pi))
box = PoseStamped()
box.header.frame_id = self.robot.get_planning_frame()
box.pose.position.z = (-0.04 + (dim[2] / 2))
box.pose.position.x = al('x')
box.pose.position.y = al('y', box.pose.position.x)
box.pose.orientation.x = quaternion[0]
box.pose.orientation.y = quaternion[1]
box.pose.orientation.z = quaternion[2]
box.pose.orientation.w = quaternion[3]
flag = 1
for i in hemme:
if abs(i.pose.position.x -
box.pose.position.x) < distance or abs(
i.pose.position.y - box.pose.position.y) < distance:
flag = 0
if flag == 0:
continue
hemme.append(box)
dims.append(dim)
cnt = 0
names = []
for i in xrange(len(hemme)):
now = "part" + str(cnt)
cnt = cnt + 1
names.append(now)
self.add_object(name=now,
x=hemme[i].pose.position.x,
y=hemme[i].pose.position.y,
z=hemme[i].pose.position.z,
xo=hemme[i].pose.orientation.x,
yo=hemme[i].pose.orientation.y,
zo=hemme[i].pose.orientation.z,
wo=hemme[i].pose.orientation.w,
d1=dims[i][0],
d2=dims[i][1],
d3=dims[i][2],
curr_scene=curr_scene,
typ=0)
print("End Check!")
return hemme
"""
----------------Function Name: pick_object----------------
Definition : It moves the given group i.e robot to the collision object whose index is given and picks that object.
Group : MoveGroupCommander object to control given group
Part_index : Index of the target object to obtain pose
"""
def pick_object(self, curr_scene, group, part_index):
rospy.loginfo("Pick Operation starts!")
gripped_object = curr_scene.get_objects(["part" + str(part_index)
])["part" + str(part_index)]
pos = copy.deepcopy(self.target_poses[part_index])
temp = tf.transformations.euler_from_quaternion(
(pos.pose.orientation.x, pos.pose.orientation.y,
pos.pose.orientation.z, pos.pose.orientation.w))
quaternion = tf.transformations.quaternion_from_euler(
math.pi, temp[1], temp[2])
pos.pose.position.z += 0.17 + (
gripped_object.primitives[0].dimensions[2] / 2.0)
pos.pose.orientation.y = 1
group.set_pose_target(pos)
move_plan = group.plan()
i = 0
while (not move_plan.joint_trajectory.joint_names):
move_plan = group.plan()
i += 1
if (i == 500): break
state = group.execute(move_plan)
rospy.loginfo("Execute operation for Object is %s" % str(part_index))
if (state):
self.closed_state_gripper(curr_scene, "part" + str(part_index))
rospy.sleep(1.0)
self.place_object(curr_scene, group, part_index)
return
else:
return
"""
----------------Function Name: place_object----------------
Definition : It places the gripped object to the target location
Group : MoveGroupCommander object to control given group
Part_index : Index of the target object to obtain pose
"""
def place_object(self, curr_scene, group, part_index):
pos = PoseStamped()
def al(typ, x=0.0):
if typ == 'x':
return random.uniform(-0.35, -0.6)
elif typ == 'y':
rang = math.sqrt(0.75**2 - x**2)
x = random.uniform(-rang, rang)
while abs(x) < 0.15:
x = random.uniform(-rang, rang)
return x
pos = PoseStamped()
pos.header.frame_id = robot.get_planning_frame()
pos.pose.position.x = al("x")
pos.pose.position.y = al("y", pos.pose.position.x)
#This line makes placing possible by setting a valid z position for gripped object
pos.pose.position.z = -0.04 + 0.17 + ((curr_scene.get_attached_objects(
["part" + str(part_index)
])["part" + str(part_index)]).object.primitives[0].dimensions[2])
pos.pose.orientation.y = 1.0
group.set_pose_target(pos)
move_plan = group.plan()
while (not move_plan.joint_trajectory.joint_names):
move_plan = group.plan()
state = group.execute(move_plan)
if (state):
detached = group.detach_object("part" + str(part_index))
rospy.sleep(1.5)
if (detached):
curr_scene.remove_world_object("part" + str(part_index))
rospy.sleep(1.5)
self.default_state_gripper(self.eef)
self.picked.append(part_index)
else:
self.default_state_gripper(robot.endeffector)
group.detach_object("part" + str(part_index))
rospy.sleep(2)
"""
----------------Function Name: execute_simulation----------------
Definition : Creates the specified environment , generates motion plans and does the pick and place operation based on these plans. To visualize RViz is needed.
Planner ID can be changed based on MoveIT's supported OMPL.
"""
def execute_simulation(self, curr_scene=None, num_of_objects=3):
is_success = False
#Reset State of the Gripper
self.default_state_gripper(self.eef)
####
#Reset the position of the Arm
# Will be implemented if needed
####
#Clean the scene
self.clean_scene(curr_scene=curr_scene)
#Create environment
self.create_environment(curr_scene=curr_scene)
#Create targets
self.target_poses = self.check_targets(curr_scene, num_of_objects,
0.10)
rospy.sleep(5)
#Planner setup
self.man.set_planner_id("RRTConnectkConfigDefault")
self.man.set_num_planning_attempts(20)
self.man.allow_replanning(True)
#Pick and place every object
for i in xrange(len(self.target_poses)):
if i not in self.picked:
self.pick_object(curr_scene=curr_scene,
group=self.man,
part_index=i)
rospy.loginfo("END OF PICK AND PLACE")
if (len(self.picked) == len(self.target_poses)):
is_success = True
self.clear_poses()
return is_success
"""
----------------Function Name: execute_simulation----------------
Definition : Executes the simulation num_attempts times. Stores the success rate
and writes into the specified document
Num_Attempts : Limiting number for executing simulation. (Default Value: 10)
File_Name : File name (with path if needed) to write test results
"""
def test_simulation(self,
curr_scene=None,
num_attempts=10,
num_of_objects=3,
file_name="ur5_pick_place_test.txt"):
success_rate = 0
file = open(file_name, "a+")
file.write("Start Test \n")
for case in xrange(1, num_attempts + 1):
state = self.execute_simulation(curr_scene=curr_scene,
num_of_objects=num_of_objects)
if (state):
success_rate += 1
rospy.loginfo("Success Rate: " + str(success_rate) + "/" +
str(case))
file.write("Test Case %s is successfull" % str(case))
else:
rospy.loginfo("Success Rate: " + str(success_rate) + "/" +
str(case))
file.write("Test Case %s is failedl" % str(case))
file.write("Success: %" + str(((success_rate / (case - 1) * 100.0))))
file.write("----END OF TEST----")
file.close()
return
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk)
self.listener = tf.TransformListener()
# self.object_name_sub = rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect)
self.tomatoboundingBox = BoundingBox()
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
global goal_x
global goal_y
global goal_z
global goal_roll
global goal_pitch
global goal_yaw
self.distance = 0
self.trans = [0.0, 0.0, 0.0]
# self.marker = []
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print ("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print ("====== current pose : ", self.wpose)
joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982]
print ("INIT POSE: ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait = True)
def go_to_desired_coordinate(self):
self.calculated_tomato.position.x = goal_x
self.calculated_tomato.position.y = goal_y
self.calculated_tomato.position.z = goal_z
self.calculated_tomato.orientation.x = goal_roll
self.calculated_tomato.orientation.y = goal_pitch
self.calculated_tomato.orientation.z = goal_yaw
tf_display_position = [self.calculated_tomato.position.x, self.calculated_tomato.position.y, self.calculated_tomato.position.z]
tf_display_orientation = [self.calculated_tomato.orientation.x, self.calculated_tomato.orientation.y, self.calculated_tomato.orientation.z, self.calculated_tomato.orientation.w]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
rate.sleep()
## ## ## show how to move on the Rviz
tomato_id_goal_waypoints = []
tomato_id_goal_waypoints.append(copy.deepcopy(calculated_tomato))
(tomato_id_goal_plan, tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(tomato_id_goal_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_id_goal_plan)
print ("============ Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # - 0.10
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.get_param('/darknet_ros/yolo_model/detection_classes/names')
rospy.Subscriber('/cluster_decomposer/centroid_pose_array',PoseArray,self.collectJsk)
# rospy.Subscriber('/darknet_ros/bounding_boxes',BoundingBoxes,self.collect)
def collectJsk(self, msg):
global goal_x
global goal_y
global goal_z
global goal_roll
global goal_pitch
global goal_yaw
try:
(trans, rot) = self.listener.lookupTransform('base_link','yolo_output00', rospy.Time(0))
goal_x = round(trans[0],2)
goal_y = round(trans[1],2)
goal_z = round(trans[2],2)
print("====== trans [x, y, z]: ", trans)
print("====== rotat [x, y, z, w]: ", rot)
orientation = euler_from_quaternion(rot)
goal_roll = orientation[0]
goal_pitch = orientation[1]
goal_yaw = orientation[2]
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn('there is no tf')
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.position_list = []
self.orientation_list = []
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.5)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(1) : ", self.wpose
marker_joint_goal = [4.721744537353516, -0.7451499144183558, -1.6199515501605433, -1.2175200621234339, 1.6366002559661865, 3.1263363361358643]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose(2) : ", self.wpose
def execute_plan(self,plan):
group = self.robot_arm
group.execute(plan, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
#Reset the position of the Arm
# Will be implemented if needed
####
#Clean the scene
clean_scene(scene)
#Create environment
create_environment(scene, robot)
#Create target object
target_poses = check_targets(robot, scene, 3, 0.10)
f.write("Used Library: " + libraries[2])
rospy.loginfo("TEST NUMBER: " + str(tr))
rospy.sleep(1)
# Planner setup
arm.set_planner_id("RRTConnectkConfigDefault")
arm.set_num_planning_attempts(20)
arm.allow_replanning(True)
#Pick and place every object
for i in xrange(len(target_poses)):
if i not in picked:
pick_object(group=arm, part_index=i, robot=robot, scene=scene)
#For testing
if (len(picked) == len(target_poses)):
success_rate += 1
rospy.loginfo("END OF PICK AND PLACE")
f.write("After try " + str(tr) + str(success_rate) + "/" + str(tr))
rospy.loginfo("Success Rate: " + str(success_rate) + "/" + str(tr))
clear_poses()
rospy.sleep(5.0)
class PlannerAnnotationParser(AnnotationParserBase):
"""
Parses the annotations files that contains the benchmarking
information.
"""
def __init__(self, path_to_annotation, path_to_data):
super(PlannerAnnotationParser, self).__init__(path_to_annotation,
path_to_data)
self.parse()
self._load_scene()
self._init_planning()
self.benchmark()
def check_results(self, results):
"""
Returns the results from the planner, checking them against any eventual validation data
(no validation data in our case).
"""
return self.planner_data
def _load_scene(self):
"""
Loads the proper scene for the planner.
It can be either a python static scene or bag containing an occupancy map.
"""
scene = self._annotations["scene"]
for element in scene:
if element["type"] == "launch":
self.play_launch(element["name"])
elif element["type"] == "python":
self.load_python(element["name"])
elif element["type"] == "bag":
self.play_bag(element["name"])
for _ in range(150):
rospy.sleep(0.3)
# wait for the scene to be spawned properly
rospy.sleep(0.5)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def benchmark(self):
for test_id, test in enumerate(self._annotations["tests"]):
marker_position_1 = test["start_xyz"]
marker_position_2 = test["goal_xyz"]
self._add_markers(marker_position_1, "Start test \n sequence",
marker_position_2, "Goal")
# Start planning in a given joint position
joints = test["start_joints"]
current = RobotState()
current.joint_state.name = self.robot.get_current_state(
).joint_state.name
current_joints = list(
self.robot.get_current_state().joint_state.position)
current_joints[0:6] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
joints = test["goal_joints"]
for planner in self.planners:
if planner == "stomp":
planner = "STOMP"
elif planner == "sbpl":
planner = "AnytimeD*"
self.planner_id = planner
self.group.set_planner_id(planner)
self._plan_joints(
joints,
self._annotations["name"] + "-test_" + str(test_id))
return self.planner_data
def _add_markers(self, point, text1, point_2, text2):
# add marker for start and goal pose of EE
marker_array = MarkerArray()
marker_1 = Marker()
marker_1.header.frame_id = "world"
marker_1.header.stamp = rospy.Time.now()
marker_1.type = Marker.SPHERE
marker_1.scale.x = 0.04
marker_1.scale.y = 0.04
marker_1.scale.z = 0.04
marker_1.lifetime = rospy.Duration()
marker_2 = deepcopy(marker_1)
marker_1.color.g = 0.5
marker_1.color.a = 1.0
marker_1.id = 0
marker_1.pose.position.x = point[0]
marker_1.pose.position.y = point[1]
marker_1.pose.position.z = point[2]
marker_2.color.r = 0.5
marker_2.color.a = 1.0
marker_2.id = 1
marker_2.pose.position.x = point_2[0]
marker_2.pose.position.y = point_2[1]
marker_2.pose.position.z = point_2[2]
marker_3 = Marker()
marker_3.header.frame_id = "world"
marker_3.header.stamp = rospy.Time.now()
marker_3.type = Marker.TEXT_VIEW_FACING
marker_3.scale.z = 0.10
marker_3.lifetime = rospy.Duration()
marker_4 = deepcopy(marker_3)
marker_3.text = text1
marker_3.id = 2
marker_3.color.g = 0.5
marker_3.color.a = 1.0
marker_3.pose.position.x = point[0]
marker_3.pose.position.y = point[1]
marker_3.pose.position.z = point[2] + 0.15
marker_4.text = text2
marker_4.id = 3
marker_4.color.r = 0.5
marker_4.color.a = 1.0
marker_4.pose.position.x = point_2[0]
marker_4.pose.position.y = point_2[1]
marker_4.pose.position.z = point_2[2] + 0.15
marker_array.markers = [marker_1, marker_2, marker_3, marker_4]
self._marker_pub.publish(marker_array)
rospy.sleep(1)
def _plan_joints(self, joints, test_name):
# plan to joint target and determine success
self.group.clear_pose_targets()
group_variable_values = self.group.get_current_joint_values()
group_variable_values[0:6] = joints[0:6]
self.group.set_joint_value_target(group_variable_values)
plan = self.group.plan()
plan_time = "N/A"
total_joint_rotation = "N/A"
comp_time = "N/A"
plan_success = self._check_plan_success(plan)
if plan_success:
plan_time = self._check_plan_time(plan)
total_joint_rotation = self._check_plan_total_rotation(plan)
while not self._comp_time:
rospy.sleep(0.5)
comp_time = self._comp_time.pop(0)
self.planner_data.append([
self.planner_id, test_name,
str(plan_success), plan_time, total_joint_rotation, comp_time
])
@staticmethod
def _check_plan_success(plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
@staticmethod
def _check_plan_time(plan):
# find duration of successful plan
number_of_points = len(plan.joint_trajectory.points)
time = plan.joint_trajectory.points[number_of_points -
1].time_from_start.to_sec()
return time
@staticmethod
def _check_plan_total_rotation(plan):
# find total joint rotation in successful trajectory
angles = [0, 0, 0, 0, 0, 0]
number_of_points = len(plan.joint_trajectory.points)
for i in range(number_of_points - 1):
angles_temp = [
abs(x - y)
for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
plan.joint_trajectory.points[i].positions)
]
angles = [x + y for x, y in zip(angles, angles_temp)]
total_angle_change = sum(angles)
return total_angle_change
def _check_computation_time(self, msg):
# get computation time for successful plan to be found
if msg.status.status == 3:
self._comp_time.append(msg.result.planning_time)
def _check_path_length(self, plan):
# find distance travelled by end effector
# not yet implemented
return
class DaArmServer:
"""The basic, design problem/tui agnostic arm server
"""
gestures = {}
grasp_height = 0.1
drop_height = 0.2
moving = False
paused = False
move_group_state = "IDLE"
def __init__(self, num_planning_attempts=20):
rospy.init_node("daarm_server", anonymous=True)
self.init_params()
self.init_scene()
self.init_publishers()
self.init_subscribers()
self.init_action_clients()
self.init_service_clients()
self.init_arm(num_planning_attempts)
def init_arm(self, num_planning_attempts=20):
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_tolerance(0.2)
self.init_services()
self.init_action_servers()
def init_scene(self):
world_objects = ["table", "tui", "monitor", "overhead", "wall"]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.3556, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8636)
self.wallPose.pose.position = Point(-0.508, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
def init_params(self):
try:
self.grasp_height = get_ros_param("GRASP_HEIGHT", "Grasp height defaulting to 0.1")
self.drop_height = get_ros_param("DROP_HEIGHT", "Drop height defaulting to 0.2")
except ValueError as e:
rospy.loginfo(e)
def init_publishers(self):
self.calibration_publisher = rospy.Publisher("/calibration_results", CalibrationParams)
self.action_belief_publisher = rospy.Publisher("/arm_action_beliefs", String, queue_size=1)
rospy.sleep(0.5)
def init_subscribers(self):
self.joint_angle_subscriber = rospy.Subscriber(
'/j2s7s300_driver/out/joint_angles', JointAngles, self.update_joints)
self.move_it_feedback_subscriber = rospy.Subscriber(
'/move_group/feedback', MoveGroupActionFeedback, self.update_move_group_state)
def init_action_servers(self):
self.calibration_server = actionlib.SimpleActionServer("calibrate_arm", CalibrateAction, self.calibrate)
self.move_block_server = actionlib.SimpleActionServer("move_block", MoveBlockAction, self.handle_move_block)
#self.home_arm_server = actionlib.SimpleActionServer("home_arm", HomeArmAction, self.home_arm)
def init_services(self):
self.home_arm_service = rospy.Service("/home_arm", ArmCommand, self.handle_home_arm)
# emergency stop
self.stop_arm_service = rospy.Service("/stop_arm", ArmCommand, self.handle_stop_arm)
# stop and pause for a bit
self.pause_arm_service = rospy.Service("/pause_arm", ArmCommand, self.handle_pause_arm)
self.start_arm_service = rospy.Service("/restart_arm", ArmCommand, self.handle_restart_arm)
def init_action_clients(self):
# Action Client for joint control
joint_action_address = '/j2s7s300_driver/joints_action/joint_angles'
self.joint_action_client = actionlib.SimpleActionClient(
joint_action_address, kinova_msgs.msg.ArmJointAnglesAction)
rospy.loginfo('Waiting for ArmJointAnglesAction server...')
self.joint_action_client.wait_for_server()
rospy.loginfo('ArmJointAnglesAction Server Connected')
# Service to move the gripper fingers
finger_action_address = '/j2s7s300_driver/fingers_action/finger_positions'
self.finger_action_client = actionlib.SimpleActionClient(
finger_action_address, kinova_msgs.msg.SetFingersPositionAction)
self.finger_action_client.wait_for_server()
def init_service_clients(self):
self.is_simulation = None
try:
self.is_simulation = get_ros_param("IS_SIMULATION", "")
except:
self.is_simulation = False
if self.is_simulation is True:
# setup alternatives to jaco services for emergency stop, joint control, and finger control
pass
# Service to get TUI State
rospy.wait_for_service('get_tui_blocks')
self.get_block_state = rospy.ServiceProxy('get_tui_blocks', TuiState)
# Service for homing the arm
home_arm_service = '/j2s7s300_driver/in/home_arm'
self.home_arm_client = rospy.ServiceProxy(home_arm_service, HomeArm)
rospy.loginfo('Waiting for kinova home arm service')
rospy.wait_for_service(home_arm_service)
rospy.loginfo('Kinova home arm service server connected')
# Service for emergency stop
emergency_service = '/j2s7s300_driver/in/stop'
self.emergency_stop = rospy.ServiceProxy(emergency_service, Stop)
rospy.loginfo('Waiting for Stop service')
rospy.wait_for_service(emergency_service)
rospy.loginfo('Stop service server connected')
# Service for restarting the arm
start_service = '/j2s7s300_driver/in/start'
self.restart_arm = rospy.ServiceProxy(start_service, Start)
rospy.loginfo('Waiting for Start service')
rospy.wait_for_service(start_service)
rospy.loginfo('Start service server connected')
def handle_start_arm(self, message):
return self.restart_arm()
def handle_stop_arm(self, message):
return self.stop_motion()
def handle_pause_arm(self, message):
self.stop_motion(home=True, pause=True)
return str(self.paused)
def handle_restart_arm(self, message):
self.restart_arm()
self.paused = False
return str(self.paused)
def handle_home_arm(self, message):
try:
status = self.home_arm_kinova()
return json.dumps(status)
except rospy.ServiceException as e:
rospy.loginfo("Homing arm failed")
def home_arm(self):
# send the arm home
# for now, let's just use the kinova home
self.home_arm_client()
def home_arm_kinova(self):
"""Takes the arm to the kinova default home if possible
"""
self.arm.set_named_target("Home")
try:
self.arm.go()
return "successful home"
except:
return "failed to home"
def move_fingers(self, finger1_pct, finger2_pct, finger3_pct):
finger_max_turn = 6800
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = float((finger1_pct/100.0)*finger_max_turn)
goal.fingers.finger2 = float((finger2_pct/100.0)*finger_max_turn)
goal.fingers.finger3 = float((finger3_pct/100.0)*finger_max_turn)
self.finger_action_client.send_goal(goal)
if self.finger_action_client.wait_for_result(rospy.Duration(5.0)):
return self.finger_action_client.get_result()
else:
self.finger_action_client.cancel_all_goals()
rospy.loginfo('the gripper action timed-out')
return None
def move_joint_angles(self, angle_set):
goal = kinova_msgs.msg.ArmJointAnglesGoal()
goal.angles.joint1 = angle_set[0]
goal.angles.joint2 = angle_set[1]
goal.angles.joint3 = angle_set[2]
goal.angles.joint4 = angle_set[3]
goal.angles.joint5 = angle_set[4]
goal.angles.joint6 = angle_set[5]
goal.angles.joint7 = angle_set[6]
self.joint_action_client.send_goal(goal)
if self.joint_action_client.wait_for_result(rospy.Duration(20.0)):
return self.joint_action_client.get_result()
else:
print(' the joint angle action timed-out')
self.joint_action_client.cancel_all_goals()
return None
def handle_move_block(self, message):
"""msg format: {id: int,
source: Point {x: float,y: float},
target: Point {x: float, y: float}
"""
def open_gripper(self, delay=0):
"""open the gripper ALL THE WAY, then delay
"""
if self.is_simulation is True:
self.gripper.set_named_target("Open")
self.gripper.go()
else:
self.move_fingers(50, 50, 50)
rospy.sleep(delay)
def close_gripper(self, delay=0):
"""close the gripper ALL THE WAY, then delay
"""
self.gripper.set_named_target("Close")
self.gripper.go()
rospy.sleep(delay)
def handle_gesture(self, gesture):
# lookup the gesture from a table? or how about a message?
# one possibility, object that contains desired deltas in pos/orientation
# as well as specify the arm or gripper choice
pass
def move_arm_to_pose(self, position, orientation, delay=0, waypoints=[], corrections=1, action_server=None):
if len(waypoints) > 0:
# this is good for doing gestures
plan, fraction = self.arm.compute_cartesian_path(waypoints, eef_step=0.01, jump_threshold=0.0)
else:
p = self.arm.get_current_pose()
p.pose.position = position
p.pose.orientation = orientation
self.arm.set_pose_target(p)
plan = self.arm.plan()
if plan:
# get the last pose to correct if desired
ptPos = plan.joint_trajectory.points[-1].positions
# print "=================================="
# print "Last point of the current trajectory: "
angle_set = list()
for i in range(len(ptPos)):
tempPos = ptPos[i]*180/np.pi + int(round((self.joint_angles[i] - ptPos[i]*180/np.pi)/(360)))*360
angle_set.append(tempPos)
if action_server:
action_server.publish_feedback(CalibrateFeedback("Plan Found"))
self.arm.execute(plan, wait=False)
while self.move_group_state is not "IDLE":
rospy.sleep(0.001)
print(self.move_group_state)
if self.paused is True:
self.arm.stop()
return
rospy.loginfo("LEAVING THE WHILE LOOP")
print("LEAVING THE LOOOOOOOOOP!!!!")
if corrections > 0:
rospy.loginfo("Correcting the pose")
self.move_joint_angles(angle_set)
rospy.sleep(delay)
else:
if action_server:
action_server.publish_feedback(CalibrateFeedback("Planning Failed"))
# Let's have the caller handle sequences instead.
# def handle_move_sequence(self, message):
# # if the move fails, do we cancel the sequence
# cancellable = message.cancellable
# moves = message.moves
# for move in moves:
# try:
# self.handle_move_block(move)
# except Exception:
# if cancellable:
# rospy.loginfo("Part of move failed, cancelling the rest.")
# break
def update_move_group_state(self, message):
rospy.loginfo(message.feedback.state)
self.move_group_state = message.feedback.state
def get_grasp_orientation(self, position):
return Quaternion(1, 0, 0, 0)
def update_joints(self, joints):
self.joint_angles = [joints.joint1, joints.joint2, joints.joint3,
joints.joint4, joints.joint5, joints.joint6, joints.joint7]
def move_z(self, distance):
p = self.arm.get_current_pose()
p.pose.position.z += distance
self.move_arm_to_pose(p.pose.position, p.pose.orientation, delay=0)
def pick_block(self, location, check_grasp=False, retry_attempts=0, delay=0, action_server=None):
# go to a spot and pick up a block
# if check_grasp is true, it will check torque on gripper and retry or fail if not holding
# open the gripper
self.open_gripper()
position = Point(location.x, location.y, self.grasp_height)
orientation = self.get_grasp_orientation(position)
try:
self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server)
except Exception as e:
raise(e)
if action_server:
action_server.publish_feedback()
if check_grasp is True:
pass # for now, but we want to check finger torques
# for i in range(retry_attempts):
# self.move_z(0.3)
self.close_gripper()
self.move_z(0.1)
rospy.sleep(delay)
def place_block(self, location, check_grasp=False, delay=0, action_server=None):
# go to a spot an drop a block
# if check_grasp is true, it will check torque on gripper and fail if not holding a block
position = Point(location.x, location.y, self.drop_height)
orientation = self.get_grasp_orientation(position)
try:
self.move_arm_to_pose(position, orientation, delay=0, action_server=action_server)
except Exception as e:
raise(e)
if action_server:
action_server.publish_feedback()
self.open_gripper()
self.move_z(0.1)
self.close_gripper()
def stop_motion(self, home=False, pause=False):
rospy.loginfo("STOPPING the ARM")
# cancel the moveit_trajectory
# self.arm.stop()
# call the emergency stop on kinova
# self.emergency_stop()
# rospy.sleep(0.5)
if pause is True:
self.paused = True
if home is True:
# self.restart_arm()
self.home_arm()
def calibrate(self, message):
print("calibrating ", message)
self.place_calibration_block()
rospy.sleep(5) # five seconds to correct the drop if it bounced, etc.
self.calibration_server.publish_feedback(CalibrateFeedback("getting the coordinates"))
params = self.record_calibration_params()
self.set_arm_calibration_params(params[0], params[1])
self.calibration_publisher.publish(CalibrationParams(params[0], params[1]))
self.calibration_server.set_succeeded(CalibrateResult(params))
def set_arm_calibration_params(self, arm_x_min, arm_y_min):
rospy.set_param("ARM_X_MIN", arm_x_min)
rospy.set_param("ARM_Y_MIN", arm_y_min)
def place_calibration_block(self):
# start by homing the arm (kinova home)
self.calibration_server.publish_feedback(CalibrateFeedback("homing"))
self.home_arm_kinova()
# go to grab a block from a human
self.open_gripper()
self.close_gripper()
self.open_gripper(4)
self.close_gripper(2)
# move to a predetermined spot
self.calibration_server.publish_feedback(CalibrateFeedback("moving to drop"))
self.move_arm_to_pose(Point(0.4, -0.4, 0.1), Quaternion(0, 1, 0, 0))
# drop the block
self.open_gripper()
self.calibration_server.publish_feedback(CalibrateFeedback("dropped the block"))
calibration_block = {'x': 0.4, 'y': 0.4, 'id': 0}
calibration_action_belief = {"action": "add", "block": calibration_block}
self.action_belief_publisher.publish(String(json.dumps(calibration_action_belief)))
rospy.loginfo("published arm belief")
return
def record_calibration_params(self):
""" Call the block_tracker service to get the current table config.
Find the calibration block and set the appropriate params.
"""
# make sure we know the dimensions of the table before we start
# fixed table dimensions include tuio_min_x,tuio_min_y,tuio_dist_x,tuio_dist_y,arm_dist_x,arm_dist_y
tuio_bounds = get_tuio_bounds()
arm_bounds = get_arm_bounds(calibrate=False)
try:
block_state = json.loads(self.get_block_state("tuio").tui_state)
except rospy.ServiceException as e:
# self.calibration_server.set_aborted()
raise(ValueError("Failed getting block state to calibrate: "+str(e)))
if len(block_state) != 1:
# self.calibration_server.set_aborted()
raise(ValueError("Failed calibration, either couldn't find block or > 1 block on TUI!"))
# if we made it here, let's continue!
# start by comparing the reported tuio coords where we dropped the block
# with the arm's localization of the end-effector that dropped it
# (we assume that the block fell straight below the drop point)
drop_pose = self.arm.get_current_pose()
end_effector_x = drop_pose.pose.position.x
end_effector_y = drop_pose.pose.position.y
block_tuio_x = block_state[0]['x']
block_tuio_y = block_state[0]['y']
x_ratio, y_ratio = tuio_to_ratio(block_tuio_x, block_tuio_y, tuio_bounds)
arm_x_min = end_effector_x - x_ratio*arm_bounds["x_dist"]
arm_y_min = end_effector_y - y_ratio*arm_bounds["y_dist"]
return [arm_x_min, arm_y_min]
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.Subscriber('camera/depth_registered/points', PointCloud2,
point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes,
bbox_callback)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
# self.listener = tf.TransformListener()
#self.goal_x = 0
#self.goal_y = 0
#self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
# self.marker = []
self.tomato = []
self.position_list = []
self.orientation_list = []
self.t_idd = 0
self.t_pose_x = []
self.t_pose_y = []
self.t_pose_z = []
self.t_ori_w = []
self.t_ori_x = []
self.t_ori_y = []
self.t_ori_z = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
planning_frame = self.robot_arm.get_planning_frame()
print("====== planning frame: ", planning_frame)
self.wpose = self.robot_arm.get_current_pose()
print("====== current pose : ", self.wpose)
joint_goal = [
-0.535054565144069, -2.009213503260451, 1.8350906250920112,
-0.7794355413099039, -0.7980899690645948, 0.7782740454087982
]
print("INIT POSE: ", self.robot_arm.get_current_pose().pose.position)
self.robot_arm.go(joint_goal, wait=True)
def go_to_move(self, scale=1.0):
planning_frame = self.robot_arm.get_planning_frame()
tomato_offset = [0, -0.35, -0.1]
robot_base_offset = 0.873
base_wrist2_offset = 0.1
print(">> robot arm planning frame: \n", planning_frame)
'''
self.calculated_tomato.position.x = (scale*self.goal_x)
self.calculated_tomato.position.y = (scale*self.goal_y)
self.calculated_tomato.position.z = (scale*self.goal_z)
'''
self.calculated_tomato.position.x = (scale * goal_x)
self.calculated_tomato.position.y = (scale * goal_y)
self.calculated_tomato.position.z = (scale * goal_z)
self.calculated_tomato.orientation = Quaternion(
*quaternion_from_euler(3.14, 0.1, 1.57))
print("=== robot_pose goal frame: ", self.calculated_tomato)
self.robot_arm.set_pose_target(self.calculated_tomato)
tf_display_position = [
self.calculated_tomato.position.x,
self.calculated_tomato.position.y,
self.calculated_tomato.position.z
]
tf_display_orientation = [
self.calculated_tomato.orientation.x,
self.calculated_tomato.orientation.y,
self.calculated_tomato.orientation.z,
self.calculated_tomato.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
tomato_waypoints = []
tomato_waypoints.append(copy.deepcopy(self.calculated_tomato))
(tomato_plan, tomato_offset) = self.robot_arm.compute_cartesian_path(
tomato_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_plan)
print("=== Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def go_to_desired_coordinate(self, pose_x, pose_y, pose_z, roll, pitch,
yaw):
'''
Manipulator is moving to the desired coordinate
Now move to the calculated_tomato_id_goal
'''
calculated_tomato_id_goal = Pose()
desired_goal_pose = [pose_x, pose_y, pose_z]
desired_goal_euler = [roll, pitch, yaw]
Cplanning_frame = self.robot_arm.get_planning_frame()
print(">> current planning frame: \n", Cplanning_frame)
calculated_tomato_id_goal.position.x = desired_goal_pose[0]
calculated_tomato_id_goal.position.y = desired_goal_pose[1]
calculated_tomato_id_goal.position.z = desired_goal_pose[2]
calculated_tomato_id_goal.orientation = Quaternion(
*quaternion_from_euler(desired_goal_euler[0],
desired_goal_euler[1],
desired_goal_euler[2]))
print(">> tomato_id_goal frame: ", desired_goal_pose)
self.robot_arm.set_pose_target(calculated_tomato_id_goal)
tf_display_position = [
calculated_tomato_id_goal.position.x,
calculated_tomato_id_goal.position.x,
calculated_tomato_id_goal.position.z
]
tf_display_orientation = [
calculated_tomato_id_goal.orientation.x,
calculated_tomato_id_goal.orientation.y,
calculated_tomato_id_goal.orientation.z,
calculated_tomato_id_goal.orientation.w
]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(tf_display_position, tf_display_orientation,
rospy.Time.now(), "goal_wpose", "world")
rate.sleep()
## ## ## show how to move on the Rviz
tomato_id_goal_waypoints = []
tomato_id_goal_waypoints.append(
copy.deepcopy(calculated_tomato_id_goal))
(tomato_id_goal_plan,
tomato_id_goal_fraction) = self.robot_arm.compute_cartesian_path(
tomato_id_goal_waypoints, 0.01, 0.0)
self.display_trajectory(tomato_id_goal_plan)
print("============ Press `Enter` to if plan is correct!! ...")
raw_input()
self.robot_arm.go(True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state(
)
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory)
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale=1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset # - 0.10
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_x(self, x_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.x = (scale *
self.wpose.position.x) + x_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_y(self, y_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.y = (scale *
self.wpose.position.y) + y_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def plan_cartesian_z(self, z_offset, scale=1.0):
waypoints = []
self.wpose = self.robot_arm.get_current_pose().pose
self.wpose.position.z = (scale *
self.wpose.position.z) + z_offset # -0.10
waypoints.append(copy.deepcopy(self.wpose))
(plan, fraction) = self.robot_arm.compute_cartesian_path(
waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def pose_subscriber(self):
rospy.loginfo("waiting for pose topic...")
rospy.Subscriber('camera/depth_registered/points', PointCloud2,
point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes,
bbox_callback)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
#self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.ar_pose = Pose()
self.goalPoseFromAR = Pose()
self.br = tf.TransformBroadcaster()
#self.goalPose_from_arPose = Pose()
self.trans = []
self.rot = []
self.calculed_coke_pose = Pose()
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def move_code(self):
self.clear_octomap()
planning_frame = self.robot_arm.get_planning_frame()
print "========== plannig frame: ", planning_frame
self.wpose = self.robot_arm.get_current_pose()
print"====== current pose : ", self.wpose
marker_joint_goal = [-0.535054565144069, -2.009213503260451, 1.8350906250920112, -0.7794355413099039, -0.7980899690645948, 0.7782740454087982]
print "INIT POSE: ", self.robot_arm.get_current_pose().pose.position
self.robot_arm.go(marker_joint_goal, wait=True)
def look_object(self):
look_object = self.robot_arm.get_current_joint_values()
## wrist3 현재 상태가 0도 인지, 360도인지에 따라 90도의 움직임을 -로 할지, + 할지 고려함
print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5]
#look_object[5] = math.radians(90)
look_object[5] = 0
'''
if look_object[5] > abs(math.radians(180)):
if look_object[5] < 0:
look_object[5] = look_object[5] + math.radians(90) # wrist3
elif look_object[5] > 0:
look_object[5] = look_object[5] - math.radians(90)
else:
look_object[5] = look_object[5] - math.radians(90) # wrist3
'''
print "wrist3 joint value(deg, rad): ", math.degrees(look_object[5]), look_object[5]
#look_object[3] = look_object[3] - (math.radians(00)) # wrist1
self.robot_arm.go(look_object, wait=True)
#look_object[5] = look_object[5] + (math.radians(90)) # wrist3
#self.robot_arm.go(look_object, wait=True)
def look_up_down(self):
self.clear_octomap()
print "======== clear_octomap... Please wait...."
look_up_down = self.robot_arm.get_current_joint_values()
#print "before: ", look_up_down
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] - (math.radians(60)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
look_up_down[4] = look_up_down[4] + (math.radians(30)) # wrist2
self.robot_arm.go(look_up_down, wait=True)
def display_trajectory(self, plan):
display_trajectory_publisher = self.display_trajectory_publisher
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = self.robot_cmd.get_current_state()
display_trajectory.trajectory.append(plan)
display_trajectory_publisher.publish(display_trajectory);
def plan_cartesian_path(self, x_offset, y_offset, z_offset, scale = 1.0):
waypoints = []
ii = 1
self.wpose = self.robot_arm.get_current_pose().pose
print "===== robot arm pose: ", self.wpose
self.wpose.position.x = (scale * self.wpose.position.x) + x_offset #-0.10
#print "self.wpose ", ii, ": [",self.wpose.position.x, self.wpose.position.y, self.wpose.position.z,"]"
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.y = (scale * self.wpose.position.y) + y_offset # + 0.05
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
self.wpose.position.z = (scale * self.wpose.position.z) + z_offset #
waypoints.append(copy.deepcopy(self.wpose))
ii += 1
print "waypoints:", waypoints
(plan, fraction) = self.robot_arm.compute_cartesian_path(waypoints, 0.01, 0.0)
return plan, fraction
def execute_plan(self, plan):
group = self.robot_arm
group.execute(plan, wait=True)
def print_ar_pose(self):
rospy.loginfo("Waiting for ar_pose_marker topic...")
rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_tf_listener)
rospy.loginfo("Maker messages detected. Starting followers...")
print "======= pos(meter): ", self.position_list
print "======= orientation: ", self.orientation_list
def go_to_move(self, scale = 1.0): # 로봇 팔: 마커 밑에 있는 물체 쪽으로 움직이기
#self.calculed_coke_pose = self.robot_arm.get_current_pose()
planning_frame = self.robot_arm.get_planning_frame()
coke_offset = [0, -0.35, -0.1] #x y z
# gazebo_coke_offset = [0, -0.2875, -0.23] gazebo 에서의 마커와 코크 캔의 offset, 바로 명령하면 해를 못 품.
# linear offset = abs([0, 0.0625, 0.13])
robot_base_offset = 0.873
base_wrist2_offset = 0.1 #for avoiding link contact error
print "========== robot arm plannig frame: \n", planning_frame
self.calculed_coke_pose.position.x = (scale * self.goal_x) + base_wrist2_offset # base_link to wrist2 x-offset
self.calculed_coke_pose.position.y = (scale * self.goal_y) + coke_offset[1]
#self.calculed_coke_pose.position.z = (scale * self.goal_z) + 0.72 + coke_offset# world to base_link z-offset
self.calculed_coke_pose.position.z = (scale * self.goal_z) + robot_base_offset # world to base_link z-offset and coke can offset
self.calculed_coke_pose.orientation = Quaternion(*quaternion_from_euler(0, 0, 1.54))
print "========== coke_pose goal frame: ", self.calculed_coke_pose
self.robot_arm.set_pose_target(self.calculed_coke_pose)
tf_display_position = [self.calculed_coke_pose.position.x, self.calculed_coke_pose.position.y, self.calculed_coke_pose.position.z]
tf_display_orientation = [self.calculed_coke_pose.orientation.x, self.calculed_coke_pose.orientation.y, self.calculed_coke_pose.orientation.z, self.calculed_coke_pose.orientation.w]
ii = 0
while ii < 5:
ii += 1
self.br.sendTransform(
tf_display_position,
tf_display_orientation,
rospy.Time.now(),
"goal_wpose",
"world")
rate.sleep()
## ## ## show how to move on the Rviz
coke_waypoints = []
coke_waypoints.append(copy.deepcopy(self.calculed_coke_pose))
(coke_plan, coke_fraction) = self.robot_arm.compute_cartesian_path(coke_waypoints, 0.01, 0.0)
self.display_trajectory(coke_plan)
## ## ##
print "============ Press `Enter` to if plan is correct!! ..."
raw_input()
self.robot_arm.go(True)
def ar_tf_listener(self, msg):
try:
self.marker = msg.markers
ml = len(self.marker)
target_id = 9
#self.m_idd = self.marker[0].id # 임시용
for ii in range(0, ml): # 0 <= ii < ml
self.m_idd = self.marker[ii].id
#print "checked all id: ", self.m_idd
if self.m_idd != target_id:
pass
#target_id_flage = False
elif self.m_idd == target_id:
target_id_flage = True
target_id = self.m_idd
target_id_index = ii
#print "target id: ", target_id_index, target_id, target_id_flage
if target_id_flage == True:
self.ar_pose.position.x = self.marker[target_id_index].pose.pose.position.x
self.ar_pose.position.y = self.marker[target_id_index].pose.pose.position.y
self.ar_pose.position.z = self.marker[target_id_index].pose.pose.position.z
self.ar_pose.orientation.x = self.marker[target_id_index].pose.pose.orientation.x
self.ar_pose.orientation.y = self.marker[target_id_index].pose.pose.orientation.y
self.ar_pose.orientation.z = self.marker[target_id_index].pose.pose.orientation.z
self.ar_pose.orientation.w = self.marker[target_id_index].pose.pose.orientation.w
self.goal_x = self.ar_pose.position.x
self.goal_y = self.ar_pose.position.y
self.goal_z = self.ar_pose.position.z
self.position_list = [self.goal_x, self.goal_y, self.goal_z]
self.orientation_list = [self.ar_pose.orientation.x, self.ar_pose.orientation.y, self.ar_pose.orientation.z, self.ar_pose.orientation.w]
(self.goal_roll, self.goal_pitch, self.goal_yaw) = euler_from_quaternion(self.orientation_list) #list form으로 넘겨주어야 함
#print "======= pos(meter): ", self.goal_x, self.goal_y, self.goal_z
#print "======= rot(rad): ", self.goal_roll, self.goal_pitch, self.goal_yaw
#print "ar_pos(meter): \n", self.position_list
#print "ar_orientation: \n", self.orientation_list
except:
return
class SrRobotCommander(object):
"""
Base class for hand and arm commanders
"""
def __init__(self, name):
"""
Initialize MoveGroupCommander object
@param name - name of the MoveIt group
"""
self._name = name
self._move_group_commander = MoveGroupCommander(name)
self._robot_commander = RobotCommander()
self._robot_name = self._robot_commander._r.get_robot_name()
self.refresh_named_targets()
self._warehouse_name_get_srv = rospy.ServiceProxy(
"get_robot_state", GetState)
self._planning_scene = PlanningSceneInterface()
self._joint_states_lock = threading.Lock()
self._joint_states_listener = \
rospy.Subscriber("joint_states", JointState,
self._joint_states_callback, queue_size=1)
self._joints_position = {}
self._joints_velocity = {}
self._joints_effort = {}
self._joints_state = None
self._clients = {}
self.__plan = None
self._controllers = {}
rospy.wait_for_service('compute_ik')
self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)
controller_list_param = rospy.get_param("/move_group/controller_list")
# create dictionary with name of controllers and corresponding joints
self._controllers = {
item["name"]: item["joints"]
for item in controller_list_param
}
self._set_up_action_client(self._controllers)
self.tf_buffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tf_buffer)
threading.Thread(None, rospy.spin)
def set_planner_id(self, planner_id):
self._move_group_commander.set_planner_id(planner_id)
def set_num_planning_attempts(self, num_planning_attempts):
self._move_group_commander.set_num_planning_attempts(
num_planning_attempts)
def set_planning_time(self, seconds):
self._move_group_commander.set_planning_time(seconds)
def get_end_effector_pose_from_named_state(self, name):
state = self._warehouse_name_get_srv(name, self._robot_name).state
return self.get_end_effector_pose_from_state(state)
def get_end_effector_pose_from_state(self, state):
header = Header()
fk_link_names = [self._move_group_commander.get_end_effector_link()]
header.frame_id = self._move_group_commander.get_pose_reference_frame()
response = self._forward_k(header, fk_link_names, state)
return response.pose_stamped[0]
def get_planning_frame(self):
return self._move_group_commander.get_planning_frame()
def set_pose_reference_frame(self, reference_frame):
self._move_group_commander.set_pose_reference_frame(reference_frame)
def get_group_name(self):
return self._name
def refresh_named_targets(self):
self._srdf_names = self.__get_srdf_names()
self._warehouse_names = self.__get_warehouse_names()
def set_max_velocity_scaling_factor(self, value):
self._move_group_commander.set_max_velocity_scaling_factor(value)
def set_max_acceleration_scaling_factor(self, value):
self._move_group_commander.set_max_acceleration_scaling_factor(value)
def allow_looking(self, value):
self._move_group_commander.allow_looking(value)
def allow_replanning(self, value):
self._move_group_commander.allow_replanning(value)
def execute(self):
"""
Executes the last plan made.
"""
if self.check_plan_is_valid():
self._move_group_commander.execute(self.__plan)
self.__plan = None
else:
rospy.logwarn("No plans were made, not executing anything.")
def execute_plan(self, plan):
if self.check_given_plan_is_valid(plan):
self._move_group_commander.execute(plan)
self.__plan = None
else:
rospy.logwarn("Plan is not valid, not executing anything.")
def move_to_joint_value_target(self,
joint_states,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self._move_group_commander.go(wait=wait)
def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
"""
Set target of the robot's links and plans.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param angle_degrees - are joint_states in degrees or not
This is a blocking method.
"""
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_joint_value_target(joint_states_cpy)
self.__plan = self._move_group_commander.plan()
return self.__plan
def check_plan_is_valid(self):
"""
Checks if current plan contains a valid trajectory
"""
return (self.__plan is not None
and len(self.__plan.joint_trajectory.points) > 0)
def check_given_plan_is_valid(self, plan):
"""
Checks if given plan contains a valid trajectory
"""
return (plan is not None and len(plan.joint_trajectory.points) > 0)
def get_robot_name(self):
return self._robot_name
def named_target_in_srdf(self, name):
return name in self._srdf_names
def set_named_target(self, name):
if name in self._srdf_names:
self._move_group_commander.set_named_target(name)
elif (name in self._warehouse_names):
response = self._warehouse_name_get_srv(name, self._robot_name)
active_names = self._move_group_commander._g.get_active_joints()
joints = response.state.joint_state.name
positions = response.state.joint_state.position
js = {}
for n, this_name in enumerate(joints):
if this_name in active_names:
js[this_name] = positions[n]
self._move_group_commander.set_joint_value_target(js)
else:
rospy.logerr("Unknown named state '%s'..." % name)
return False
return True
def get_named_target_joint_values(self, name):
output = dict()
if (name in self._srdf_names):
output = self._move_group_commander.\
_g.get_named_target_values(str(name))
elif (name in self._warehouse_names):
js = self._warehouse_name_get_srv(
name, self._robot_name).state.joint_state
for x, n in enumerate(js.name):
if n in self._move_group_commander._g.get_joints():
output[n] = js.position[x]
else:
rospy.logerr("No target named %s" % name)
return None
return output
def get_end_effector_link(self):
return self._move_group_commander.get_end_effector_link()
def get_current_pose(self, reference_frame=None):
"""
Get the current pose of the end effector.
@param reference_frame - The desired reference frame in which end effector pose should be returned.
If none is passed, it will use the planning frame as reference.
@return geometry_msgs.msg.Pose() - current pose of the end effector
"""
if reference_frame is not None:
try:
trans = self.tf_buffer.lookup_transform(
reference_frame,
self._move_group_commander.get_end_effector_link(),
rospy.Time(0), rospy.Duration(5.0))
current_pose = geometry_msgs.msg.Pose()
current_pose.position.x = trans.transform.translation.x
current_pose.position.y = trans.transform.translation.y
current_pose.position.z = trans.transform.translation.z
current_pose.orientation.x = trans.transform.rotation.x
current_pose.orientation.y = trans.transform.rotation.y
current_pose.orientation.z = trans.transform.rotation.z
current_pose.orientation.w = trans.transform.rotation.w
return current_pose
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn(
"Couldn't get the pose from " +
self._move_group_commander.get_end_effector_link() +
" in " + reference_frame + " reference frame")
return None
else:
return self._move_group_commander.get_current_pose().pose
def get_current_state(self):
"""
Get the current joint state of the group being used.
@return a dictionary with the joint names as keys and current joint values
"""
joint_names = self._move_group_commander._g.get_active_joints()
joint_values = self._move_group_commander._g.get_current_joint_values()
return dict(zip(joint_names, joint_values))
def get_current_state_bounded(self):
"""
Get the current joint state of the group being used, enforcing that they are within each joint limits.
@return a dictionary with the joint names as keys and current joint values
"""
current = self._move_group_commander._g.get_current_state_bounded()
names = self._move_group_commander._g.get_active_joints()
output = {n: current[n] for n in names if n in current}
return output
def get_robot_state_bounded(self):
return self._move_group_commander._g.get_current_state_bounded()
def move_to_named_target(self, name, wait=True):
"""
Set target of the robot's links and moves to it
@param name - name of the target pose defined in SRDF
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self._move_group_commander.go(wait=wait)
def plan_to_named_target(self, name):
"""
Set target of the robot's links and plans
This is a blocking method.
@param name - name of the target pose defined in SRDF
"""
self._move_group_commander.set_start_state_to_current_state()
if self.set_named_target(name):
self.__plan = self._move_group_commander.plan()
def __get_warehouse_names(self):
try:
list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
return list_srv("", self._robot_name).states
except rospy.ServiceException as exc:
rospy.logwarn("Couldn't access warehouse: " + str(exc))
return list()
def _reset_plan(self):
self.__plan = None
def _set_plan(self, plan):
self.__plan = plan
def __get_srdf_names(self):
return self._move_group_commander._g.get_named_targets()
def get_named_targets(self):
"""
Get the complete list of named targets, from SRDF
as well as warehouse poses if available.
@return list of strings containing names of targets.
"""
return self._srdf_names + self._warehouse_names
def get_joints_position(self):
"""
Returns joints position
@return - dictionary with joints positions
"""
with self._joint_states_lock:
return self._joints_position
def get_joints_velocity(self):
"""
Returns joints velocities
@return - dictionary with joints velocities
"""
with self._joint_states_lock:
return self._joints_velocity
def _get_joints_effort(self):
"""
Returns joints effort
@return - dictionary with joints efforts
"""
with self._joint_states_lock:
return self._joints_effort
def get_joints_state(self):
"""
Returns joints state
@return - JointState message
"""
with self._joint_states_lock:
return self._joints_state
def run_joint_trajectory(self, joint_trajectory):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
"""
plan = RobotTrajectory()
plan.joint_trajectory = joint_trajectory
self._move_group_commander.execute(plan)
def make_named_trajectory(self, trajectory):
"""
Makes joint value trajectory from specified by named poses (either from
SRDF or from warehouse)
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements (n.b either name or joint_angles is required)
- name -> the name of the way point
- joint_angles -> a dict of joint names and angles
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
- degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
"""
current = self.get_current_state_bounded()
joint_trajectory = JointTrajectory()
joint_names = current.keys()
joint_trajectory.joint_names = joint_names
start = JointTrajectoryPoint()
start.positions = current.values()
start.time_from_start = rospy.Duration.from_sec(0.001)
joint_trajectory.points.append(start)
time_from_start = 0.0
for wp in trajectory:
joint_positions = None
if 'name' in wp.keys():
joint_positions = self.get_named_target_joint_values(
wp['name'])
elif 'joint_angles' in wp.keys():
joint_positions = copy.deepcopy(wp['joint_angles'])
if 'degrees' in wp.keys() and wp['degrees']:
for joint, angle in joint_positions.iteritems():
joint_positions[joint] = radians(angle)
if joint_positions is None:
rospy.logerr(
"Invalid waypoint. Must contain valid name for named target or dict of joint angles."
)
return None
new_positions = {}
for n in joint_names:
new_positions[n] = joint_positions[
n] if n in joint_positions else current[n]
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = [
new_positions[n] for n in joint_names
]
current = new_positions
time_from_start += wp['interpolate_time']
trajectory_point.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(trajectory_point)
if 'pause_time' in wp and wp['pause_time'] > 0:
extra = JointTrajectoryPoint()
extra.positions = trajectory_point.positions
time_from_start += wp['pause_time']
extra.time_from_start = rospy.Duration.from_sec(
time_from_start)
joint_trajectory.points.append(extra)
return joint_trajectory
def send_stop_trajectory_unsafe(self):
"""
Sends a trajectory of all active joints at their current position.
This stops the robot.
"""
current = self.get_current_state_bounded()
trajectory_point = JointTrajectoryPoint()
trajectory_point.positions = current.values()
trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)
trajectory = JointTrajectory()
trajectory.points.append(trajectory_point)
trajectory.joint_names = current.keys()
self.run_joint_trajectory_unsafe(trajectory)
def run_named_trajectory_unsafe(self, trajectory, wait=False):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory directly via contoller.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory_unsafe(joint_trajectory, wait)
def run_named_trajectory(self, trajectory):
"""
Moves robot through trajectory specified by named poses, either from
SRDF or from warehouse. Runs trajectory via moveit.
@param trajectory - list of waypoints, each waypoint is a dict with
the following elements:
- name -> the name of the way point
- interpolate_time -> time to move from last wp
- pause_time -> time to wait at this wp
"""
joint_trajectory = self.make_named_trajectory(trajectory)
if joint_trajectory is not None:
self.run_joint_trajectory(joint_trajectory)
def move_to_position_target(self, xyz, end_effector_link="", wait=True):
"""
Specify a target position for the end-effector and moves to it
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_position_target(self, xyz, end_effector_link=""):
"""
Specify a target position for the end-effector and plans.
This is a blocking method.
@param xyz - new position of end-effector
@param end_effector_link - name of the end effector link
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_position_target(xyz, end_effector_link)
self.__plan = self._move_group_commander.plan()
def move_to_pose_target(self, pose, end_effector_link="", wait=True):
"""
Specify a target pose for the end-effector and moves to it
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param wait - should method wait for movement end or not
"""
self._move_group_commander.set_start_state_to_current_state()
self._move_group_commander.set_pose_target(pose, end_effector_link)
self._move_group_commander.go(wait=wait)
def plan_to_pose_target(self,
pose,
end_effector_link="",
alternative_method=False):
"""
Specify a target pose for the end-effector and plans.
This is a blocking method.
@param pose - new pose of end-effector: a Pose message, a PoseStamped
message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
of 7 floats [x, y, z, qx, qy, qz, qw]
@param end_effector_link - name of the end effector link
@param alternative_method - use set_joint_value_target instead of set_pose_target
"""
self._move_group_commander.set_start_state_to_current_state()
if alternative_method:
self._move_group_commander.set_joint_value_target(
pose, end_effector_link)
else:
self._move_group_commander.set_pose_target(pose, end_effector_link)
self.__plan = self._move_group_commander.plan()
return self.__plan
def _joint_states_callback(self, joint_state):
"""
The callback function for the topic joint_states.
It will store the received joint position, velocity and efforts
information into dictionaries
@param joint_state - the message containing the joints data.
"""
with self._joint_states_lock:
self._joints_state = joint_state
self._joints_position = {
n: p
for n, p in zip(joint_state.name, joint_state.position)
}
self._joints_velocity = {
n: v
for n, v in zip(joint_state.name, joint_state.velocity)
}
self._joints_effort = {
n: v
for n, v in zip(joint_state.name, joint_state.effort)
}
def _set_up_action_client(self, controller_list):
"""
Sets up an action client to communicate with the trajectory controller
"""
self._action_running = {}
for controller_name in controller_list.keys():
self._action_running[controller_name] = False
service_name = controller_name + "/follow_joint_trajectory"
self._clients[controller_name] = SimpleActionClient(
service_name, FollowJointTrajectoryAction)
if self._clients[controller_name].wait_for_server(
timeout=rospy.Duration(4)) is False:
err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
service_name)
rospy.logwarn(err_msg)
def move_to_joint_value_target_unsafe(self,
joint_states,
time=0.002,
wait=True,
angle_degrees=False):
"""
Set target of the robot's links and moves to it.
@param joint_states - dictionary with joint name and value. It can
contain only joints values of which need to be changed.
@param time - time in s (counting from now) for the robot to reach the
target (it needs to be greater than 0.0 for it not to be rejected by
the trajectory controller)
@param wait - should method wait for movement end or not
@param angle_degrees - are joint_states in degrees or not
"""
# self._update_default_trajectory()
# self._set_targets_to_default_trajectory(joint_states)
goals = {}
joint_states_cpy = copy.deepcopy(joint_states)
if angle_degrees:
joint_states_cpy.update(
(joint, radians(i)) for joint, i in joint_states_cpy.items())
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = []
point = JointTrajectoryPoint()
point.positions = []
for x in joint_states_cpy.keys():
if x in controller_joints:
goal.trajectory.joint_names.append(x)
point.positions.append(joint_states_cpy[x])
point.time_from_start = rospy.Duration.from_sec(time)
goal.trajectory.points = [point]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def action_is_running(self, controller=None):
if controller is not None:
return self._action_running[controller]
for controller_running in self._action_running.values():
if controller_running:
return True
return False
def _action_done_cb(self, controller, terminal_state, result):
self._action_running[controller] = False
def _call_action(self, goals):
for client in self._clients:
self._action_running[client] = True
self._clients[client].send_goal(
goals[client],
lambda terminal_state, result: self._action_done_cb(
client, terminal_state, result))
def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
"""
Moves robot through all joint states with specified timeouts
@param joint_trajectory - JointTrajectory class object. Represents
trajectory of the joints which would be executed.
@param wait - should method wait for movement end or not
"""
goals = {}
for controller in self._controllers:
controller_joints = self._controllers[controller]
goal = FollowJointTrajectoryGoal()
goal.trajectory = copy.deepcopy(joint_trajectory)
indices_of_joints_in_this_controller = []
for i, joint in enumerate(joint_trajectory.joint_names):
if joint in controller_joints:
indices_of_joints_in_this_controller.append(i)
goal.trajectory.joint_names = [
joint_trajectory.joint_names[i]
for i in indices_of_joints_in_this_controller
]
for point in goal.trajectory.points:
if point.positions:
point.positions = [
point.positions[i]
for i in indices_of_joints_in_this_controller
]
if point.velocities:
point.velocities = [
point.velocities[i]
for i in indices_of_joints_in_this_controller
]
if point.effort:
point.effort = [
point.effort[i]
for i in indices_of_joints_in_this_controller
]
goals[controller] = goal
self._call_action(goals)
if not wait:
return
for i in self._clients.keys():
if not self._clients[i].wait_for_result():
rospy.loginfo("Trajectory not completed")
def plan_to_waypoints_target(self,
waypoints,
reference_frame=None,
eef_step=0.005,
jump_threshold=0.0):
"""
Specify a set of waypoints for the end-effector and plans.
This is a blocking method.
@param reference_frame - the reference frame in which the waypoints are given
@param waypoints - an array of poses of end-effector
@param eef_step - configurations are computed for every eef_step meters
@param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
"""
old_frame = self._move_group_commander.get_pose_reference_frame()
if reference_frame is not None:
self.set_pose_reference_frame(reference_frame)
(self.__plan,
fraction) = self._move_group_commander.compute_cartesian_path(
waypoints, eef_step, jump_threshold)
self.set_pose_reference_frame(old_frame)
def set_teach_mode(self, teach):
"""
Activates/deactivates the teach mode for the robot.
Activation: stops the the trajectory controllers for the robot, and
sets it to teach mode.
Deactivation: stops the teach mode and starts trajectory controllers
for the robot.
Currently this method blocks for a few seconds when called on a hand,
while the hand parameters are reloaded.
@param teach - bool to activate or deactivate teach mode
"""
if teach:
mode = RobotTeachModeRequest.TEACH_MODE
else:
mode = RobotTeachModeRequest.TRAJECTORY_MODE
self.change_teach_mode(mode, self._name)
def move_to_trajectory_start(self, trajectory, wait=True):
"""
Make and execute a plan from the current state to the first state in an pre-existing trajectory
@param trajectory - moveit_msgs/JointTrajectory
@param wait - Bool to specify if movement should block untill finished.
"""
if len(trajectory.points) <= 0:
rospy.logerr("Trajectory has no points in it, can't reverse...")
return None
first_point = trajectory.points[0]
end_state = dict(zip(trajectory.joint_names, first_point.positions))
self.move_to_joint_value_target(end_state, wait=wait)
@staticmethod
def change_teach_mode(mode, robot):
teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)
req = RobotTeachModeRequest()
req.teach_mode = mode
req.robot = robot
try:
resp = teach_mode_client(req)
if resp.result == RobotTeachModeResponse.ERROR:
rospy.logerr("Failed to change robot %s to mode %d", robot,
mode)
else:
rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
mode, resp.result)
except rospy.ServiceException:
rospy.logerr("Failed to call service teach_mode")
def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
"""
Computes the inverse kinematics for a given pose. It returns a JointState
@param target_pose - A given pose of type PoseStamped
@param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
"""
service_request = PositionIKRequest()
service_request.group_name = self._name
service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
)
service_request.pose_stamped = target_pose
service_request.timeout.secs = 0.5
service_request.avoid_collisions = avoid_collisions
if joint_states is None:
service_request.robot_state.joint_state = self.get_joints_state()
else:
service_request.robot_state.joint_state = joint_states
try:
resp = self._compute_ik(ik_request=service_request)
# Check if error_code.val is SUCCESS=1
if resp.error_code.val != 1:
if resp.error_code.val == -10:
rospy.logerr("Unreachable point: Start state in collision")
elif resp.error_code.val == -12:
rospy.logerr("Unreachable point: Goal state in collision")
elif resp.error_code.val == -31:
rospy.logerr("Unreachable point: No IK solution")
else:
rospy.logerr("Unreachable point (error: %s)" %
resp.error_code)
return
else:
return resp.solution.joint_state
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.robot_arm.remember_joint_values("zeros", None)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
#self.robot_arm.allow_replanning(True)
#scene = moveit_commander.PlanningSceneInterface()
#robot = moveit_commander.RobotCommander()
#rospy.sleep(2)
#add Table
p = PoseStamped()
p.header.frame_id = FIXED_FRAME
p.pose.position.x = 0.0
p.pose.position.y = 0.2
p.pose.position.z = 0.1
self.scene.add_box("table", p, (0.3, 0.3, 0.3))
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(1)
self.robot_arm.attach_object("table")
pose_goal.orientation.w = 0.0
pose_goal.position.x = 0.4 # red line 0.2 0.2
pose_goal.position.y = 0.15 # green line 0.15 0.15
pose_goal.position.z = 0.2 # blue line # 0.35 0.6
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
#if (input == 'w')
#pose_goal.position.x += 0.1 # red line 0.2 0.2
print("====== move plan go to up ======")
self.robot_arm.set_named_target("zeros") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to zeros ======") # rospy.sleep(1)
#self.robot_arm.place("table", pose_goal)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
self.robot_arm.detach_object("table")
pose_goal.orientation.w = 0.0
pose_goal.position.x = 0.2 # red line 0.2 0.2
pose_goal.position.y = 0.3 # green line 0.15 0.15
pose_goal.position.z = 0.1 # blue line # 0.35 0.6
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
self.robot_arm.set_pose_target(pose_goal)
self.robot_arm.go(True)
print(robot_state)
