def execute(self, userdata):
mc = MoveGroupCommander("r1_arm")
userdata.robot_1_state = mc.get_current_joint_values()
userdata.ik_link_1 = mc.get_end_effector_link()
userdata.robot_1_pose = mc.get_current_pose()
mc = MoveGroupCommander("r2_arm")
userdata.robot_2_state = mc.get_current_joint_values()
userdata.ik_link_2 = mc.get_end_effector_link()
userdata.robot_2_pose = mc.get_current_pose()
mc = MoveGroupCommander("arms")
userdata.complete_state = mc.get_current_joint_values()
return 'succeeded'
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)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('get_joint_states', anonymous=True)
# Initialize the MoveIt! commander for the right arm
right_arm = MoveGroupCommander('tx90_arm')
# Get the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Joints are stored in the order they appear in the kinematic chain
joint_names = right_arm.get_active_joints()
# Display the joint names
rospy.loginfo("Joint names:\n" + str(joint_names))
# Get the current joint angles
joint_values = right_arm.get_current_joint_values()
# Display the joint values
rospy.loginfo("Joint values:\n" + str(joint_values) + "\n")
# # Get the end-effector pose
ee_pose = right_arm.get_current_pose(end_effector_link)
# Display the end-effector pose
rospy.loginfo("End effector pose:\n" + str(ee_pose))
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('get_joint_states', anonymous=True)
# Initialize the MoveIt! commander for the right arm
arm = MoveGroupCommander('right_arm')
# Get the end-effector link
end_effector_link = arm.get_end_effector_link()
rospy.loginfo("End effector: %s" % end_effector_link)
planning_frame = arm.get_planning_frame()
# Joints are stored in the order they appear in the kinematic chain
joint_names = arm.get_active_joints()
joint_names = [
'right_arm_shoulder_rotate_joint', 'right_arm_shoulder_lift_joint',
'right_arm_elbow_rotate_joint', 'right_arm_elbow_bend_joint',
'right_arm_wrist_bend_joint', 'right_arm_wrist_rotate_joint'
]
# Display the joint names
#rospy.loginfo("Joint names:\n" + str(joint_names) + "\n")
# Get the current joint angles
joint_values = arm.get_current_joint_values()
# Display the joint values
rospy.loginfo("Joint values:\n" + str(joint_values) + "\n")
# Get the end-effector pose
ee_pose = arm.get_current_pose(end_effector_link)
orientation = ee_pose.pose.orientation
ox = orientation.x
oy = orientation.y
oz = orientation.z
ow = orientation.w
euler_pose = euler_from_quaternion([ow, ox, oy, oz])
#euler_pose = euler_from_quaternion([0.0, 0.0, 0.0, 1.0])
# Display the end-effector pose
rospy.loginfo("End effector pose:\n" + str(ee_pose))
rospy.loginfo("RPY?:\n" + str(euler_pose))
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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)
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 = MoveGroupComm # 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 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)
Pose_goal.header.frame_id = 'rightbase_link'
Pose_goal.pose.position.x = -0.1955793462195291 # red line 0.2 0.2
Pose_goal.pose.position.y = 0.3456909607161672 # green line 0.15 0.15
Pose_goal.pose.position.z = 0.16049011785234568 # blue line # 0.35 0.6
# Pose_goal.pose.orientation = start_pose.orientation
Pose_goal.pose.orientation.x = 0.28520761755123414
Pose_goal.pose.orientation.y = 0.24468120052517786
Pose_goal.pose.orientation.z = 0.6034841927127607
Pose_goal.pose.orientation.w = 0.7032741671255489
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):
# Initialize the move_group API
print 'starting'
moveit_commander.roscpp_initialize(sys.argv)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('manipulator')
right_arm.set_planner_id('RRTConnectkConfigDefault')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
#right_arm.set_end_effector_link(end_effector_link)
# Get the current pose so we can add it as a waypoint
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(2)
right_arm.execute(plan2)#先移到观测姿态
try:
rate = rospy.Rate(10)
while not rospy.is_shutdown():
print '.'
graspobject( )
rate.sleep()
except rospy.ROSInterruptException:
pass
raw_input("please input")
# [00000000]
print right_arm.get_joint_value_target()
print both_arms.get_joint_value_target()
# no this functions
# print right_arm.get_named_targets()
print right_arm.get_remembered_joint_values()
print both_arms.get_remembered_joint_values()
print right_arm.get_path_constraints()
print both_arms.get_path_constraints()
print right_arm.get_active_joints()
print both_arms.get_active_joints()
print right_arm.get_current_joint_values()
print right_arm.get_current_pose()
print right_arm.get_current_rpy()
print both_arms.get_current_joint_values()
print both_arms.get_current_pose()
print both_arms.get_current_rpy()
right_arm.clear_pose_targets()
left_current_pose = both_arms.get_current_pose(end_effector_link='left_gripper').pose
right_current_pose = both_arms.get_current_pose(end_effector_link='right_gripper').pose
print left_current_pose
def graspobject():
# Connect to the right_arm move group
right_arm = MoveGroupCommander('manipulator')
right_arm.set_planner_id('RRTConnectkConfigDefault')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_link')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
#right_arm.set_end_effector_link(end_effector_link)
#start_pose = Pose()
#start_pose.position.x = 0.587
#start_pose.position.y = 0.034
#start_pose.position.z = -0.010
#start_pose.orientation.x =0.508
#start_pose.orientation.y = 0.508
#start_pose.orientation.z = -0.491
#start_pose.orientation.w = 0.491
#right_arm.set_pose_target(start_pose)
#traj = right_arm.plan()
#rospy.sleep(2)
#right_arm.execute(traj)
#rospy.sleep(2)
global getkey
print '回到初始位置结束'
startclient = rospy.ServiceProxy('/detect_grasps/actionstart', npy)
startclient(0)
print 'call start over'
sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, graspcallback)
rate = rospy.Rate(200)
while not rospy.is_shutdown():
if getkey==1:
break
rate.sleep()
getkey=0
global grasps
print '检测到抓取位置，开始执行'
global ser
ser.write(('0\n').encode())
rospy.sleep(2)
print "-start movingA------------------------------------"
pose = []
currentcamera=current_cameralinkpose()
grasppose1,grasppose2= transformgrasp(currentcamera,grasps,0.24,0.15)
start_pose = current_pose()
print 'start_pose',start_pose
pose.append(start_pose)
pose.append(grasppose1)
pose.append(grasppose2)
#end_pose = copy.deepcopy(start_pose)
# end_pose.position.x = grasps.approach.x
# end_pose.position.y = grasps.approach.y
# end_pose.position.z = grasps.approach.z
catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态
print "-start movingB------------------------------------"
print '抓'
ser.write(('1\n').encode())
rospy.sleep(2)
##############################################################
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(1)
right_arm.execute(plan2)#先移到观测姿态
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =2.6649599075317383#机器人的观测姿态定义在这里
group_variable_values[1] = -1.493981663380758
group_variable_values[2] = -1.7679532209979456
group_variable_values[3] =-1.3932693640338343
group_variable_values[4] = 1.5719022750854492
group_variable_values[5] = 1.5657243728637695
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(2)
right_arm.execute(plan2)#先移到观测姿态
print "-start movingB------------------------------------"
print '笛卡尔插值到放置位置完成'
ser.write(('0\n').encode())
rospy.sleep(2)
# pose = []
#posecurrent=current_pose()#获取现有的机器人末端姿态
# pose.append(posecurrent)
#start_pose = Pose()#似乎这里不能接受相对的移动
#start_pose= objectpose#获取目标末端姿态
# posecurrent.position.z =posecurrent.position.z -0.1
# pose.append(posecurrent)
# print start_pose
print "-------------------------------------"
# print pose
print "-------------------------------------"
# catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态
#回到初始位置
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
group_variable_values[1] = -1.5415833632098597
group_variable_values[2] = -1.2562201658831995
group_variable_values[3] = -1.8575199286090296
group_variable_values[4] = 1.572489857673645
group_variable_values[5] = 1.5713907480239868
right_arm.set_joint_value_target(group_variable_values)
plan2 = right_arm.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(1)
right_arm.execute(plan2)#先移到观测姿态
rospy.sleep(3)
print '回到初始位置结束'
startclient2 = rospy.ServiceProxy('/detect_grasps/actionover', npy)
startclient2(0)
print 'call start over'
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 =====")
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
# reach
rospy.loginfo("reach")
arm.set_pose_target([0.90, 0.16, 0.255, 0, 0, 0])
arm.plan() and arm.go()
arm.plan() and arm.go()
# approach
rospy.loginfo("approach")
arm.set_pose_target([1.13, 0.16, 0.255, 0, 0, 0])
arm.plan() and arm.go()
# rotate
for i in range(4):
# close
rospy.loginfo("close")
close_hand()
# rotate
angles = arm.get_current_joint_values()
import numpy
start_angle = angles[5]
print("current angles=", start_angle)
for r in numpy.arange(start_angle, start_angle-3.14*2, -1.0):
rospy.loginfo(angles)
angles[5] = r
rospy.loginfo("rotate (%f)" % (r))
msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
msg.trajectory.points = [JointTrajectoryPoint(positions=angles, time_from_start=rospy.Duration(1))]
client.send_goal(msg)
client.wait_for_result()
# open
rospy.loginfo("open")
open_hand()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene=PlanningSceneInterface()
pub_traj= rospy.Publisher('/joint_path_command', trajectory_msgs.msg.JointTrajectory, queue_size=1)
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.gripperCtrl=rospy.ServiceProxy("/two_finger/gripper/gotoPositionUntilTouch",SetPosition)
self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
cartesian=rospy.get_param('~cartesian',True)
self.arm_end_effector_link=arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
self.reference_frame='base_link'
arm.set_pose_reference_frame(self.reference_frame)
arm.set_planning_time(5)
#scene planning
self.l_id='l_tool'
self.r_id='r_tool'
self.table_id='table'
self.target_id='target_object'
self.f_target_id='receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.scene.remove_world_object(self.f_target_id)
self.table_ground=0.13
self.table_size=[0.9,0.6,0.018]
self.setupSence()
joint_names= ['joint_'+jkey for jkey in ('s','l','e','u','r','b','t')]
joint_names= rospy.get_param('controller_joint_names')
traj= trajectory_msgs.msg.JointTrajectory()
traj.joint_names= joint_names
target_size=[0.058,0.058,0.19]
f_target_size=[0.2,0.2,0.04]
#final target
f_target_pose=PoseStamped()
f_target_pose.header.frame_id=self.reference_frame
f_target_pose.pose.position.x=-0.184+0.27
f_target_pose.pose.position.y=0.62+0.1
f_target_pose.pose.position.z=self.table_ground+self.table_size[2]+f_target_size[2]/2.0
f_target_pose.pose.orientation.x=0
f_target_pose.pose.orientation.y=0
f_target_pose.pose.orientation.z=0
f_target_pose.pose.orientation.w=1
self.scene.add_box(self.f_target_id,f_target_pose,f_target_size)
#pouring pose
pour_pose=f_target_pose
pour_pose.pose.position.x-=0.06
pour_pose.pose.position.y-=0.12
pour_pose.pose.position.z+=0.15
#pour_pose.pose.position.y+=0.17
pour_pose.pose.orientation.x=-0.5
pour_pose.pose.orientation.y=-0.5
pour_pose.pose.orientation.z=-0.5
pour_pose.pose.orientation.w=0.5
#set color
self.setColor(self.table_id,0.8,0,0,1.0)
self.setColor(self.f_target_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor(self.target_id,0,1,0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(5)
#j_ori_state=[0.72316974401474, 0.4691084027290344, 0.41043856739997864, -2.3381359577178955, 0.5580500364303589, 1.1085972785949707, 3.14]
j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_trans_state=[1.708616018295288, 0.9996442198753357, -0.4782222807407379, -1.7541601657867432, 0.13480804860591888, 1.1835496425628662, 2.689549684524536]
signal= True
arm.set_joint_value_target(j_ori_state)
arm.go()
rospy.sleep(3)
#arm.set_joint_value_target(j_trans_state)
#arm.go()
#rospy.sleep(5)
waypoints_1=[]
waypoints_2=[]
start_pick_pose=arm.get_current_pose(self.arm_end_effector_link).pose
if cartesian:
msg = rospy.wait_for_message('/aruco_single/pose',PoseStamped)
target_pose=PoseStamped()
target_pose.header.frame_id=self.reference_frame
target_pose.pose.position.x=-(msg.pose.position.y)-0.28
target_pose.pose.position.y=-msg.pose.position.x+0.81-0.072
target_pose.pose.position.z=1.36-msg.pose.position.z+0.1
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=-0
target_pose.pose.orientation.w=1
self.scene.add_box(self.target_id,target_pose,target_size)
self.setColor(self.target_id,0,0.8,0)
self.sendColors()
#pre_g_pose
pre_grasp_pose=PoseStamped()
pre_grasp_pose.header.frame_id=self.reference_frame
pre_grasp_pose.pose.position=target_pose.pose.position
pre_grasp_pose.pose.position.y-=0.2
pre_grasp_pose.pose.position.z+=0.01
pre_grasp_pose.pose.orientation.x=-0.5
pre_grasp_pose.pose.orientation.y=-0.5
pre_grasp_pose.pose.orientation.z=-0.5
pre_grasp_pose.pose.orientation.w=0.5
#grasp_pose
grasp_pose=PoseStamped()
grasp_pose.header.frame_id=self.reference_frame
grasp_pose.pose.position=target_pose.pose.position
grasp_pose.pose.position.y+=0.025
grasp_pose.pose.orientation.x=-0.5
grasp_pose.pose.orientation.y=-0.5
grasp_pose.pose.orientation.z=-0.5
grasp_pose.pose.orientation.w=0.5
g_p=PoseStamped()
g_p.header.frame_id=self.arm_end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y= -0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
waypoints_1.append(start_pick_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_1.append(deepcopy(grasp_pose.pose))
fraction=0.0
attempts=0
maxtries=300
while fraction!=1 and attempts<maxtries:
(plan_1,fraction)=arm.compute_cartesian_path(waypoints_1,0.01,0.0,True)
attempts+=1
if (attempts %300==0 and fraction!=1):
rospy.loginfo("path planning failed with " + str(fraction*100)+"% success.")
signal_pick=False
if fraction==1:
rospy.loginfo("path compute successfully with "+str(attempts)+" sttempts.")
rospy.loginfo("Arm moving.")
rospy.sleep(3)
#arm.execute(plan_1)
p_pick_1=plan_1.joint_trajectory.points[-1]
#print p
arm.set_joint_value_target(p_pick_1.positions)
arm.go()
rospy.sleep(7)
self.gripperCtrl(0)
rospy.sleep(3)
self.scene.remove_world_object(self.target_id)
#p_pick_2=plan_1.joint_trajectory.points[1]
#print p
#arm.set_joint_value_target(p_pick_2.positions)
#arm.go()
#rospy.sleep(4)
signal_pick=True
#p=plan_1.joint_trajectory.points[-1]
#print p
#arm.set_joint_value_target(p.positions)
#arm.go()
if signal_pick:
start_trans_pose=arm.get_current_pose(self.arm_end_effector_link).pose
waypoints_2.append(start_trans_pose)
#waypoints_1.append(deepcopy(pre_grasp_pose.pose))
waypoints_2.append(deepcopy(pour_pose.pose))
fraction_2=0.0
attempts_2=0
maxtries_2=300
while fraction_2!=1 and attempts_2<maxtries_2:
(plan_2,fraction_2)=arm.compute_cartesian_path(waypoints_2,0.01,0.0,True)
attempts_2+=1
if (attempts_2 %300==0 and fraction_2!=1):
rospy.loginfo("path planning failed with " + str(fraction_2*100)+"% success.")
if fraction_2==1:
#signal=False
rospy.loginfo("path compute successfully with "+str(attempts_2)+" sttempts.")
rospy.loginfo("Arm moving.")
p_trans_shake=plan_2.joint_trajectory.points[-1]
p_trans=plan_2.joint_trajectory.points[-2]
#print p
arm.set_joint_value_target(p_trans.positions)
arm.go()
rospy.sleep(3)
j_pre_pour_state=arm.get_current_joint_values()
j_pour_state=j_pre_pour_state
j_pour_state[6]-=(1.57+0.26)
arm.set_joint_value_target(j_pour_state)
arm.go()
print "shaking balabala"
rospy.sleep(1)
print "shaking balabala"
rospy.sleep(1)
print "shaking balabala"
rospy.sleep(1)
#shake=True
for i in range(10):
self.add_point(traj, 0.3, p_trans_shake, [0.0]*7)
self.add_point(traj, 0.3, p_trans, [0.0]*7)
traj.header.stamp= rospy.Time.now()
pub_traj.publish(traj)
arm.set_joint_value_target(j_pre_pour_state)
arm.go()
rospy.sleep(6)
arm.set_joint_value_target(p_pick_1.positions)
arm.go()
rospy.sleep(7)
self.gripperCtrl(255)
#remove and shut down
#self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
#self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
rospy.Subscriber('/aruco_single/pose', PoseStamped, self.pose_cb,queue_size=1)
scene=PlanningSceneInterface()
self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
self.colors=dict()
rospy.sleep(1)
arm=MoveGroupCommander('arm')
#gripper=MoveGroupCommander('gripper')
end_effector_link=arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
#gripper.set_goal_position_tolerance(0.005)
#gripper.set_goal_orientation_tolerance(0.025)
#gripper.allow_replanning(True)
reference_frame='base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id='table'
#cylinder_id='cylinder'
box2_id='box2'
target_id='target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground=0.59
table_size=[0.5,1,0.01]
#box1_size=[0.1,0.05,0.03]
box2_size=[0.15,0.15,0.02]
r_tool_size=[0.05,0.04,0.22]
l_tool_size=[0.05,0.04,0.22]
target_size=[0.05,0.05,0.1]
table_pose=PoseStamped()
table_pose.header.frame_id=reference_frame
table_pose.pose.position.x=0.7
table_pose.pose.position.y=0.0
table_pose.pose.position.z=table_ground+table_size[2]/2.0
table_pose.pose.orientation.w=1.0
scene.add_box(table_id,table_pose,table_size)
'''
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.7
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
'''
box2_pose=PoseStamped()
box2_pose.header.frame_id=reference_frame
box2_pose.pose.position.x=0.55
box2_pose.pose.position.y=-0.12
box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0
box2_pose.pose.orientation.w=1.0
scene.add_box(box2_id,box2_pose,box2_size)
target_pose=PoseStamped()
target_pose.header.frame_id=reference_frame
target_pose.pose.position.x=0.58
target_pose.pose.position.y=0.05
target_pose.pose.position.z=table_ground+table_size[2]+target_size[2]/2.0
target_pose.pose.orientation.x=0
target_pose.pose.orientation.y=0
target_pose.pose.orientation.z=0
target_pose.pose.orientation.w=1
scene.add_box(target_id,target_pose,target_size)
#left gripper
l_p=PoseStamped()
l_p.header.frame_id=end_effector_link
l_p.pose.position.x=0.00
l_p.pose.position.y=0.06
l_p.pose.position.z=0.11
l_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'l_tool',l_p,l_tool_size)
#right gripper
r_p=PoseStamped()
r_p.header.frame_id=end_effector_link
r_p.pose.position.x=0.00
r_p.pose.position.y= -0.06
r_p.pose.position.z=0.11
r_p.pose.orientation.w=1
scene.attach_box(end_effector_link,'r_tool',r_p,r_tool_size)
#grasp
g_p=PoseStamped()
g_p.header.frame_id=end_effector_link
g_p.pose.position.x=0.00
g_p.pose.position.y= -0.00
g_p.pose.position.z=0.025
g_p.pose.orientation.w=0.707
g_p.pose.orientation.x=0
g_p.pose.orientation.y=-0.707
g_p.pose.orientation.z=0
self.setColor(table_id,0.8,0,0,1.0)
#self.setColor(box1_id,0.8,0.4,0,1.0)
self.setColor(box2_id,0.8,0.4,0,1.0)
self.setColor('r_tool',0.8,0,0)
self.setColor('l_tool',0.8,0,0)
self.setColor('target_object',0,1,0)
self.sendColors()
#motion planning
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
grasp_pose=target_pose
grasp_pose.pose.position.x-=0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x=0
grasp_pose.pose.orientation.y=0.707
grasp_pose.pose.orientation.z=0
grasp_pose.pose.orientation.w=0.707
#arm.set_start_state_to_current_state()
'''
arm.set_pose_target(grasp_pose,end_effector_link)
traj=arm.plan()
arm.execute(traj)
print arm.get_current_joint_values()
'''
pre_joint_state=[0.16588150906995922, 1.7060146047438647, -0.00961761728757362, 1.8614674591892713, -2.9556667436476847, 1.7432451233907822, 3.1415]
arm.set_joint_value_target(pre_joint_state)
traj=arm.plan()
arm.execute(traj)
rospy.sleep(2)
arm.shift_pose_target(0,0.09,end_effector_link)
arm.go()
rospy.sleep(2)
scene.attach_box(end_effector_link,target_id,g_p,target_size)
rospy.sleep(2)
#grasping is over , from now is pouring
arm.shift_pose_target(2,0.15,end_effector_link)
arm.go()
rospy.sleep(2)
joint_state_1=arm.get_current_joint_values()
joint_state_1[0]-=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
joint_state_2=arm.get_current_joint_values()
joint_state_2[6]-=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(1)
#print arm.get_current_joint_values()
#pouring test
for i in range(1,5):
joint_state_2[6]+=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
joint_state_2[6]-=0.087
arm.set_joint_value_target(joint_state_2)
arm.go()
time.sleep(0.05)
print i
joint_state_2[6]+=1.8
arm.set_joint_value_target(joint_state_2)
arm.go()
rospy.sleep(2)
joint_state_1[0]+=0.17
arm.set_joint_value_target(joint_state_1)
arm.go()
rospy.sleep(1)
arm.shift_pose_target(2,-0.15,end_effector_link)
arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link,target_id)
rospy.sleep(2)
arm.set_named_target("initial_arm")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link,'l_tool')
rospy.sleep(1)
scene.remove_attached_object(end_effector_link,'r_tool')
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(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 TestPlanners(object):
def __init__(self, group_id, planner):
rospy.init_node('moveit_test_planners', anonymous=True)
self.pass_list = []
self.fail_list = []
self.planner = planner
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(group_id)
rospy.sleep(1)
self.group.set_planner_id(self.planner)
self.test_trajectories_empty_environment()
self.test_waypoints()
self.test_trajectories_with_walls_and_ground()
for pass_test in self.pass_list:
print(pass_test)
for fail_test in self.fail_list:
print(fail_test)
def _add_walls_and_ground(self):
# publish a scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = self.robot.get_planning_frame()
p.pose.position.x = 0
p.pose.position.y = 0
# offset such that the box is below ground (to prevent collision with
# the robot itself)
p.pose.position.z = -0.11
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
self.scene.add_box("ground", p, (3, 3, 0.1))
p.pose.position.x = 0.4
p.pose.position.y = 0.85
p.pose.position.z = 0.4
p.pose.orientation.x = 0.5
p.pose.orientation.y = -0.5
p.pose.orientation.z = 0.5
p.pose.orientation.w = 0.5
self.scene.add_box("wall_front", p, (0.8, 2, 0.01))
p.pose.position.x = 1.33
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707388
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.706825
self.scene.add_box("wall_right", p, (0.8, 2, 0.01))
p.pose.position.x = -0.5
p.pose.position.y = 0.4
p.pose.position.z = 0.4
p.pose.orientation.x = 0.0
p.pose.orientation.y = -0.707107
p.pose.orientation.z = 0.0
p.pose.orientation.w = 0.707107
self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
# rospy.sleep(1)
def _check_plan(self, plan):
if len(plan.joint_trajectory.points) > 0:
return True
else:
return False
def _plan_joints(self, joints):
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()
return self._check_plan(plan)
def test_trajectories_rotating_each_joint(self):
# test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
test_joint_values = [numpy.pi / 2.0]
joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
# Joint 4th is colliding with the hand
# for joint in range(6):
for joint in [0, 1, 2, 3, 5]:
for value in test_joint_values:
joints[joint] = value
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_rotating_each_joint, " +
self.planner + ", joints:" + str(joints))
def test_trajectories_empty_environment(self):
# Up - Does not work with sbpl but it does with ompl
joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# All joints up
joints = [
-1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Down
joints = [
-0.000348431194526, 0.397651011661, 0.0766181197394,
-0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# left
joints = [
0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
0.146075718452, 0.00420656698366, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Front
joints = [
1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
-1.42700242769, 3.1415450794, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Behind
joints = [
1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
0.694689321451, -0.390769365032, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
# Should fail because it is in self-collision
joints = [
-0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
1.04235601797, -1.69730925867, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_empty_environment, " +
self.planner + ", joints:" + str(joints))
def test_waypoints(self):
# Start planning in a given joint position
joints = [
-0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
0.344227622185, -3.03250264451, 0.0, 0.0
]
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:8] = joints
current.joint_state.position = current_joints
self.group.set_start_state(current)
waypoints = []
initial_pose = self.group.get_current_pose().pose
initial_pose.position.x = -0.301185959729
initial_pose.position.y = 0.517069787724
initial_pose.position.z = 1.20681710541
initial_pose.orientation.x = 0.0207499700474
initial_pose.orientation.y = -0.723943002716
initial_pose.orientation.z = -0.689528413407
initial_pose.orientation.w = 0.00515118111221
# start with a specific position
waypoints.append(initial_pose)
# first move it down
wpose = geometry_msgs.msg.Pose()
wpose.orientation = waypoints[0].orientation
wpose.position.x = waypoints[0].position.x
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z - 0.20
waypoints.append(copy.deepcopy(wpose))
# second front
wpose.position.y += 0.20
waypoints.append(copy.deepcopy(wpose))
# third side
wpose.position.x -= 0.20
waypoints.append(copy.deepcopy(wpose))
# fourth return to initial pose
wpose = waypoints[0]
waypoints.append(copy.deepcopy(wpose))
(plan3,
fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
if not self._check_plan(plan3) and fraction > 0.8:
self.fail_list.append("Failed: test_waypoints, " + self.planner)
else:
self.pass_list.append("Passed: test_waypoints, " + self.planner)
def test_trajectories_with_walls_and_ground(self):
self._add_walls_and_ground()
# Should fail to plan: Goal is in collision with the wall_front
joints = [
0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
0.302143499777, 0.00130280337897, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Should fail to plan: Goal is in collision with the ground
joints = [
3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
0.000133883149666, -0.594498939239, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
# Goal close to right corner
joints = [
0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
-1.3516255551, 2.8225061435, 0.0, 0.0
]
if not self._plan_joints(joints):
self.fail_list.append(
"Failed: test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
else:
self.pass_list.append(
"Passed, test_trajectories_with_walls_and_ground, " +
self.planner + ", joints:" + str(joints))
self.scene.remove_world_object("ground")
self.scene.remove_world_object("wall_left")
self.scene.remove_world_object("wall_right")
self.scene.remove_world_object("wall_front")
class MoveitMove(EventState):
'''
Move the arm to a specific pose or point or named_target
-- move wetter or not the arm should move or simply check if the move is possible
-- waitForExecution wait for execution to end before continuing
># pose Pose Targetpose.
<= done No error occurred.
<= failed The plan could't be done.
'''
def __init__(self,
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15):
# See example_state.py for basic explanations.
super(MoveitMove, self).__init__(outcomes=['done', 'failed'],
input_keys=['target'])
self.move = move
self.waitForExecution = waitForExecution
self.group = MoveGroupCommander(group)
self.tol = 0.06
self.result = None
self.count = 0
self.timeout = rospy.Time.now()
self.watchdog = watchdog
def execute(self, userdata):
if self.result:
return self.result
if self.waitForExecution:
curState = self.group.get_current_joint_values()
diff = compareStates(curState, self.endState)
print("diff=" + str(diff))
if diff < self.tol or self.timeout + rospy.Duration(
self.watchdog) < rospy.Time.now():
self.count += 1
if self.count > 3:
Logger.loginfo('Target reached :)')
return "done"
else:
self.count = 0
else:
return "done"
def on_enter(self, userdata):
Logger.loginfo('Enter Move Arm')
self.timeout = rospy.Time.now()
if type(userdata.target) is Pose:
Logger.loginfo('the target is a pose')
self.group.set_pose_target(userdata.target)
elif type(userdata.target) is Point:
Logger.loginfo('the target is a point')
xyz = [userdata.target.x, userdata.target.y, userdata.target.z]
self.group.set_position_target(xyz)
elif type(userdata.target) is str:
Logger.loginfo('the target is a named_target')
self.group.set_named_target(userdata.target)
else:
Logger.loginfo(
'ERROR in ' + str(self.name) +
' : target is not a Pose() nor a Point() nor a string')
self.result = 'failed'
Logger.loginfo('target defined')
try:
plan = self.group.plan()
Logger.loginfo(str(plan))
# Logger.loginfo(str(plan.joint_trajectory.points))
self.endState = plan.joint_trajectory.points[
len(plan.joint_trajectory.points) - 1].positions
Logger.loginfo(str(self.endState))
except:
Logger.loginfo('Planning failed')
self.result = 'failed'
return
Logger.loginfo('Plan done successfully')
if self.move:
Logger.loginfo('Initiating movement')
self.group.execute(plan, wait=False)
if not self.waitForExecution:
self.result = "done"
else:
Logger.loginfo('The target is reachable')
self.result = "done"
def on_exit(self, userdata):
Logger.loginfo('Stoping movement')
self.group.stop()
def on_pause(self):
Logger.loginfo('Pausing movement')
self.group.stop()
def on_resume(self, userdata):
self.on_enter(userdata)
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 MoveitMoveCartesian(EventState):
'''
Move the arm between two specific poses or points or named_targets
-- move Bool Whether or not the arm should move or simply check if the move is possible
-- waitForExecution Bool Wait for execution to end before continuing
-- group string Name of the Moveit Planning group
># targetPose Pose Pose to reach
<= done No error occurred.
<= failed The plan could't be done.
'''
def __init__(self,
move=True,
waitForExecution=True,
group="RightArm",
watchdog=15):
# See example_state.py for basic explanations.
super(MoveitMove, self).__init__(outcomes=['done', 'failed'],
input_keys=['targetPose'])
self.move = move
self.waitForExecution = waitForExecution
self.group = MoveGroupCommander(group)
self.tol = 0.06
self.result = None
self.count = 0
self.countlimit = 0
self.movSteps = 0.01
self.jumpThresh = 0.0
self.timeout = rospy.Time.now()
self.watchdog = watchdog
def execute(self, userdata):
if self.result:
return self.result
if self.waitForExecution:
curState = self.group.get_current_joint_values()
diff = compareStates(curState, self.endState)
print("diff=" + str(diff))
if diff < self.tol or self.timeout + rospy.Duration(
self.watchdog) < rospy.Time.now():
self.count += 1
if self.count > 3:
Logger.loginfo('Target reached :)')
return "done"
else:
self.count = 0
else:
return "done"
def on_enter(self, userdata):
Logger.loginfo('Enter Move Arm')
self.timeout = rospy.Time.now()
if type(userdata.targetPose) is Pose:
Logger.loginfo('target is a pose')
waypoints = [
self.group.get_current_pose().pose,
copy.deepcopy(userdata.targetPose)
]
try:
(plan, fraction) = self.group.compute_cartesian_path(
waypoints, self.movSteps, self.jumpThresh)
except:
Logger.loginfo('Planning failed; could not compute path')
self.result = 'failed'
return
else:
Logger.loginfo('ERROR in ' + str(self.name) +
' : target is not a Pose()')
self.result = 'failed'
Logger.loginfo('target defined')
try:
Logger.loginfo(str(plan))
self.endState = plan.joint_trajectory.points[
len(plan.joint_trajectory.points) - 1].positions
Logger.loginfo(str(self.endState))
except:
Logger.loginfo('Planning failed; path is invalid')
self.result = 'failed'
return
Logger.loginfo('Plan done successfully')
if self.move:
Logger.loginfo('Initiating movement')
self.group.execute(plan, wait=False)
if not self.waitForExecution:
self.result = "done"
else:
Logger.loginfo('The target is reachable')
self.result = "done"
def on_exit(self, userdata):
Logger.loginfo('Stopping movement')
self.group.stop()
def on_pause(self):
Logger.loginfo('Pausing movement')
self.group.stop()
def on_resume(self, userdata):
self.on_enter(userdata)
#! /usr/bin/env python
import sys
import rospy
from moveit_commander import MoveGroupCommander, roscpp_initialize, roscpp_shutdown
joint_state_topic = ['joint_states:=/j2s7s300/joint_states']
roscpp_initialize(joint_state_topic)
rospy.init_node('kinova_get_joints', anonymous=False)
group = MoveGroupCommander('arm')
joints = group.get_current_joint_values()
print(joints)
rospy.sleep(1)
roscpp_shutdown()
class RazerControl():
def __init__(self):
self.pub_right_hand_pose = rospy.Publisher(RIGHT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_right_hand_pose_reference = rospy.Publisher(RIGHT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_left_hand_pose = rospy.Publisher(LEFT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_left_hand_pose_reference = rospy.Publisher(LEFT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.hydra_data_subs = rospy.Subscriber(HYDRA_DATA_TOPIC, Hydra, self.hydraDataCallback)
self.pub_move_base = rospy.Publisher(MOVE_BASE_TOPIC, Twist)
self.subs = rospy.Subscriber('/joint_states', JointState, self.getJointStates)
self.current_joint_states = None
rospy.loginfo("Getting first joint_states")
while self.current_joint_states == None:
rospy.sleep(0.1)
rospy.loginfo("Gotten!")
rospy.loginfo("Connecting with right hand AS")
self.right_hand_as = actionlib.SimpleActionClient(HAND_RIGHT_AS, FollowJointTrajectoryAction)
self.right_hand_as.wait_for_server()
rospy.loginfo("Connecting with left hand AS")
self.left_hand_as = actionlib.SimpleActionClient(HAND_LEFT_AS, FollowJointTrajectoryAction)
self.left_hand_as.wait_for_server()
rospy.loginfo("Starting up move group commander for right, left, torso and head... (slow)")
self.right_arm_mgc = MoveGroupCommander("right_arm")
self.right_arm_mgc.set_pose_reference_frame('base_link')
self.left_arm_mgc = MoveGroupCommander("left_arm")
self.left_arm_mgc.set_pose_reference_frame('base_link')
self.torso_mgc = MoveGroupCommander("right_arm_torso")
self.torso_mgc.set_pose_reference_frame('base_link')
self.head_mgc = MoveGroupCommander("head")
self.head_mgc.set_pose_reference_frame('base_link')
self.last_hydra_message = None
self.tmp_pose_right = PoseStamped()
self.tmp_pose_left = PoseStamped()
self.read_message = False
def getJointStates(self, data):
self.current_joint_states = data
def create_hand_goal(self, hand_side="right", hand_pose="closed", values=0.0):
"""Returns the hand goal to send
possible poses: closed, open, intermediate"""
hand_goal = FollowJointTrajectoryGoal()
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_thumb_joint')
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_middle_joint')
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_index_joint')
jtp = JointTrajectoryPoint()
joint_list = ['hand_'+ hand_side +'_thumb_joint',
'hand_'+ hand_side +'_middle_joint',
'hand_'+ hand_side +'_index_joint']
ids_list = []
values_list = []
rospy.loginfo("current_joint_state is:\n" + str(self.current_joint_states))
for joint in joint_list:
idx_in_message = self.current_joint_states.name.index(joint)
ids_list.append(idx_in_message)
values_list.append(self.current_joint_states.position[idx_in_message])
if hand_pose == "closed":
jtp.positions.append(2.0)
jtp.positions.append(values_list[1]) # TODO: read values and keep them
jtp.positions.append(values_list[2]) # TODO: read values and keep them
elif hand_pose == "open":
jtp.positions.append(0.0)
jtp.positions.append(values_list[1]) # TODO: read values and keep them
jtp.positions.append(values_list[2]) # TODO: read values and keep them
elif hand_pose == "intermediate":
jtp.positions.append(values_list[0]) # TODO: read values and keep them
jtp.positions.append(values)
jtp.positions.append(values)
jtp.velocities.append(0.0)
jtp.velocities.append(0.0)
jtp.velocities.append(0.0)
jtp.time_from_start.secs = 2
hand_goal.trajectory.points.append(jtp)
return hand_goal
def hydraDataCallback(self, data):
#rospy.loginfo("Received data from " + HYDRA_DATA_TOPIC)
self.last_hydra_message = data
self.tmp_pose_right = PoseStamped()
self.tmp_pose_right.header.frame_id = 'base_link'
self.tmp_pose_right.header.stamp = rospy.Time.now()
self.tmp_pose_right.pose.position.x = self.last_hydra_message.paddles[1].transform.translation.x
self.tmp_pose_right.pose.position.y = self.last_hydra_message.paddles[1].transform.translation.y
self.tmp_pose_right.pose.position.z = self.last_hydra_message.paddles[1].transform.translation.z
self.tmp_pose_right.pose.position.x += RIGHT_HAND_INITIAL_POINT.x
self.tmp_pose_right.pose.position.y += RIGHT_HAND_INITIAL_POINT.y
self.tmp_pose_right.pose.position.z += RIGHT_HAND_INITIAL_POINT.z
self.tmp_pose_right.pose.orientation = self.last_hydra_message.paddles[1].transform.rotation
self.tmp_pose_left = PoseStamped()
self.tmp_pose_left.header.frame_id = 'base_link'
self.tmp_pose_left.header.stamp = rospy.Time.now()
self.tmp_pose_left.pose.position.x = self.last_hydra_message.paddles[0].transform.translation.x
self.tmp_pose_left.pose.position.y = self.last_hydra_message.paddles[0].transform.translation.y
self.tmp_pose_left.pose.position.z = self.last_hydra_message.paddles[0].transform.translation.z
self.tmp_pose_left.pose.position.x += LEFT_HAND_INITIAL_POINT.x
self.tmp_pose_left.pose.position.y += LEFT_HAND_INITIAL_POINT.y
self.tmp_pose_left.pose.position.z += LEFT_HAND_INITIAL_POINT.z
self.tmp_pose_left.pose.orientation = self.last_hydra_message.paddles[0].transform.rotation
if self.last_hydra_message.paddles[1].buttons[0] == True:
self.pub_right_hand_pose.publish(self.tmp_pose_right)
if self.last_hydra_message.paddles[0].buttons[0] == True:
self.pub_left_hand_pose.publish(self.tmp_pose_left)
self.pub_right_hand_pose_reference.publish(self.tmp_pose_right)
self.pub_left_hand_pose_reference.publish(self.tmp_pose_left)
self.read_message = False
def run(self):
rospy.loginfo("Press LB / RB to send the current pose")
while self.last_hydra_message == None:
rospy.sleep(0.1)
rospy.loginfo("Got the first data of the razer... Now we can do stuff")
sleep_rate=0.05 # check at 20Hz
counter = 0
while True:
counter += 1
rospy.loginfo("Loop #" + str(counter))
if not self.read_message:
self.read_message = True
if self.last_hydra_message.paddles[1].buttons[0] == True:
# send curr left paddle pos to move_group right
rospy.loginfo("sending curr right hand")
self.right_arm_mgc.set_pose_target(self.tmp_pose_right)
self.right_arm_mgc.go(wait=False)
if self.last_hydra_message.paddles[0].buttons[0] == True:
# send curr right paddle pos to move_group left
rospy.loginfo("sending curr left hand")
self.left_arm_mgc.set_pose_target(self.tmp_pose_left)
self.left_arm_mgc.go(wait=False)
if self.last_hydra_message.paddles[1].trigger > 0.0:
# send goal right hand close proportional to trigger value (2.0 max?)
rospy.loginfo("Closing right hand to value: " + str(self.last_hydra_message.paddles[1].trigger * 2.0))
right_hand_goal = self.create_hand_goal(hand_side="right", hand_pose="intermediate", values=self.last_hydra_message.paddles[1].trigger * 2.0)
self.right_hand_as.send_goal(right_hand_goal)
if self.last_hydra_message.paddles[0].trigger > 0.0:
# send goal left hand close proportional to trigger value (2.0 max?)
rospy.loginfo("Closing left hand to value: " + str(self.last_hydra_message.paddles[0].trigger * 2.0))
left_hand_goal = self.create_hand_goal(hand_side="left", hand_pose="intermediate", values=self.last_hydra_message.paddles[0].trigger * 2.0)
self.left_hand_as.send_goal(left_hand_goal)
if self.last_hydra_message.paddles[1].joy[0] != 0.0:
# send torso rotation left(neg)/right (pos)
rospy.loginfo("Rotation torso")
curr_joint_val = self.torso_mgc.get_current_joint_values()
self.torso_mgc.set_joint_value_target("torso_1_joint", curr_joint_val[0] + (self.last_hydra_message.paddles[1].joy[0] * 0.1 * -1))
self.torso_mgc.go(wait=True)
rospy.loginfo("Rotation torso sent!")
if self.last_hydra_message.paddles[1].joy[1] != 0.0:
# send torso inclination front(pos)/back(neg)
rospy.loginfo("Inclination torso")
curr_joint_val = self.torso_mgc.get_current_joint_values()
self.torso_mgc.set_joint_value_target("torso_2_joint", curr_joint_val[1] + (self.last_hydra_message.paddles[1].joy[1] * 0.1))
self.torso_mgc.go(wait=True)
rospy.loginfo("Inclination torso sent!")
if self.last_hydra_message.paddles[0].joy[0] != 0.0 or self.last_hydra_message.paddles[0].joy[1] != 0.0:
twist_goal = Twist()
twist_goal.linear.x = 1.0 * self.last_hydra_message.paddles[0].joy[1]
twist_goal.angular.z = 1.0 * self.last_hydra_message.paddles[0].joy[0] * -1.0
self.pub_move_base.publish(twist_goal)
# move base rotate left (neg)/ right(pos)
rospy.loginfo("Move base")
if self.last_hydra_message.paddles[1].buttons[3] == True:
# thumb up
rospy.loginfo("Right thumb up")
right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="open")
self.right_hand_as.send_goal(right_thumb_up)
if self.last_hydra_message.paddles[0].buttons[3] == True:
# thumb up
rospy.loginfo("Left thumb up")
left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="open")
self.left_hand_as.send_goal(left_thumb_up)
if self.last_hydra_message.paddles[1].buttons[1] == True:
# thumb down
rospy.loginfo("Right thumb down")
right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="closed")
self.right_hand_as.send_goal(right_thumb_up)
if self.last_hydra_message.paddles[0].buttons[1] == True:
# thumb down
rospy.loginfo("Left thumb down")
left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="closed")
self.left_hand_as.send_goal(left_thumb_up)
rospy.sleep(sleep_rate)
import moveit_msgs.msg
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from math import sin, copysign, sqrt, pi
if __name__ == '__main__':
print "============ Dynamic hand gestures"
roscpp_initialize(sys.argv)
rospy.init_node('pumpkin_planning', anonymous=True)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
group_variable_values = right_arm.get_current_joint_values()
print "============ Joint values: ", group_variable_values
group_variable_values[0] = -0.357569385437786
group_variable_values[1] = -0.6320268016619928
group_variable_values[2] = 0.24177580736353846
group_variable_values[3] = 1.586101553004471
group_variable_values[4] = -0.5805943181752088
group_variable_values[5] = -1.1821499952996368
right_arm.set_joint_value_target(group_variable_values)
right_arm.go(wait=True)
print "============ Waiting while RVIZ displays Up..."
group_variable_values[0] = 1.0
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.colors = dict()
rospy.sleep(1)
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.set_goal_position_tolerance(0.005)
arm.set_goal_orientation_tolerance(0.025)
arm.allow_replanning(True)
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.set_planning_time(5)
#scene planning
table_id = 'table'
#cylinder_id='cylinder'
#box1_id='box1'
box2_id = 'box2'
target_id = 'target_object'
#scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
rospy.sleep(2)
table_ground = 0.68
table_size = [0.5, 1, 0.01]
#box1_size=[0.1,0.05,0.03]
box2_size = [0.05, 0.05, 0.1]
r_tool_size = [0.03, 0.01, 0.06]
l_tool_size = [0.03, 0.01, 0.06]
target_size = [0.05, 0.05, 0.1]
table_pose = PoseStamped()
table_pose.header.frame_id = reference_frame
table_pose.pose.position.x = 0.75
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
'''
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.7
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
'''
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.6
box2_pose.pose.position.y = -0.05
box2_pose.pose.position.z = table_ground + table_size[
2] + box2_size[2] / 2.0
box2_pose.pose.orientation.w = 1.0
scene.add_box(box2_id, box2_pose, box2_size)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.6
target_pose.pose.position.y = 0.05
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
scene.add_box(target_id, target_pose, target_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.04
l_p.pose.position.z = 0.04
l_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'l_tool', l_p, l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.04
r_p.pose.position.z = 0.04
r_p.pose.orientation.w = 1
scene.attach_box(end_effector_link, 'r_tool', r_p, r_tool_size)
#grasp
g_p = PoseStamped()
g_p.header.frame_id = end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
self.setColor(table_id, 0.8, 0, 0, 1.0)
#self.setColor(box1_id,0.8,0.4,0,1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.setColor('target_object', 0, 1, 0)
self.sendColors()
#motion planning
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
grasp_pose = target_pose
grasp_pose.pose.position.x -= 0.15
#grasp_pose.pose.position.z=
grasp_pose.pose.orientation.x = 0
grasp_pose.pose.orientation.y = 0.707
grasp_pose.pose.orientation.z = 0
grasp_pose.pose.orientation.w = 0.707
arm.set_start_state_to_current_state()
arm.set_pose_target(grasp_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(2)
print arm.get_current_joint_values()
#arm.shift_pose_target(4,1.57,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.shift_pose_target(0, 0.11, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
saved_target_pose = arm.get_current_pose(end_effector_link)
#arm.set_named_target("initial_arm2")
#grasp
scene.attach_box(end_effector_link, target_id, g_p, target_size)
rospy.sleep(2)
#grasping is over , from now is placing
arm.shift_pose_target(2, 0.15, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(1, -0.2, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
arm.shift_pose_target(2, -0.15, end_effector_link)
arm.go()
rospy.sleep(2)
print arm.get_current_joint_values()
scene.remove_attached_object(end_effector_link, target_id)
rospy.sleep(2)
#arm.set_pose_target(saved_target_pose,end_effector_link)
#arm.go()
#rospy.sleep(2)
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
#remove and shut down
scene.remove_attached_object(end_effector_link, 'l_tool')
scene.remove_attached_object(end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线
cartesian = True #rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('armc_arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('init_pose')
arm.go()
rospy.sleep(1)
# 设置机械臂运动的起始位姿
joint_goal = arm.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi / 6
joint_goal[2] = 0
joint_goal[3] = pi / 3
joint_goal[4] = 0
joint_goal[5] = pi / 3
joint_goal[6] = 0
arm.go(joint_goal)
rospy.sleep(1)
#获取当前位置为规划初始位置
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 如果为True,将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置路点数据，并加入路点列表，所有的点都加入
wpose = deepcopy(start_pose) #拷贝对象
wpose.position.x += 0.1
waypoints.append(deepcopy(wpose))
if cartesian: #如果设置为True，那么走直线
#wpose.position.x += 0.020
waypoints.append(deepcopy(wpose))
else: #否则就走曲线
arm.set_pose_target(wpose) #自由曲线
arm.go()
rospy.sleep(1)
wpose.position.x += 0.01
if cartesian:
#wpose.position.x += 0.030
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
wpose.position.y += 0.1
if cartesian:
#wpose.position.x += 0.040
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
wpose.position.x -= 0.15
wpose.position.y -= 0.1
if cartesian:
#wpose.position.x += 0.050
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
#规划过程
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
#规划路径 ，fraction返回1代表规划成功
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表，这里是5个点
0.01, # eef_step，终端步进值，每隔0.01m计算一次逆解判断能否可达
0.0, # jump_threshold，跳跃阈值，设置为0代表不允许跳跃
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
rospy.sleep(5)
# 控制机械臂先回到初始化位置
#arm.set_named_target('init_pose')
#arm.go()
#rospy.sleep(5)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# reach
rospy.loginfo("reach")
arm.set_pose_target([0.90, 0.16, 0.255, 0, 0, 0])
arm.plan() and arm.go()
arm.plan() and arm.go()
# approach
rospy.loginfo("approach")
arm.set_pose_target([1.13, 0.16, 0.255, 0, 0, 0])
arm.plan() and arm.go()
# rotate
for i in range(4):
# close
rospy.loginfo("close")
close_hand()
# rotate
angles = arm.get_current_joint_values()
import numpy
start_angle = angles[5]
print("current angles=", start_angle)
for r in numpy.arange(start_angle, start_angle - 3.14 * 2, -1.0):
rospy.loginfo(angles)
angles[5] = r
rospy.loginfo("rotate (%f)" % (r))
msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(
0.2)
msg.trajectory.points = [
JointTrajectoryPoint(positions=angles,
time_from_start=rospy.Duration(1))
]
client.send_goal(msg)
client.wait_for_result()
class PickAndPlace:
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self, traj, time, positions, velocities=None):
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions = copy.deepcopy(positions)
if velocities is not None:
point.velocities = copy.deepcopy(velocities)
point.time_from_start = rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self, q_traj, t_traj):
assert (len(q_traj) == len(t_traj))
#Insert current position to beginning.
if t_traj[0] > 1.0e-2:
t_traj.insert(0, 0.0)
q_traj.insert(0, self.Q(arm=arm))
self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj)
#self.traj= self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj) #, dq_traj
#print traj
#self.sub_jpc.publish(self.ToROSTrajectory(self.JointNames(), q_traj, t_traj, dq_traj))
def QTrajToDQTraj(self, q_traj, t_traj):
dof = len(q_traj[0])
#Modeling the trajectory with spline.
splines = [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj = []
for t in t_traj:
dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
#print dq_traj
return dq_traj
def JointNames(self):
#arm= 0
return self.joint_names[0]
def ROSGetJTP(self, q, t, dq=None):
jp = trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions = q
jp.time_from_start = rospy.Duration(t)
if dq is not None: jp.velocities = dq
return jp
def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None):
assert (len(q_traj) == len(t_traj))
if dq_traj is not None: (len(dq_traj) == len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names = joint_names
if dq_traj is not None:
self.traj.points = [
self.ROSGetJTP(q, t, dq)
for q, t, dq in zip(q_traj, t_traj, dq_traj)
]
else:
self.traj.points = [
self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj)
]
self.traj.header.stamp = rospy.Time.now()
#print self.traj
return self.traj
def SmoothQTraj(self, q_traj):
if len(q_traj) == 0: return
q_prev = np.array(q_traj[0])
q_offset = np.array([0] * len(q_prev))
for q in q_traj:
q_diff = np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d] < -math.pi: q_offset[d] += 1
elif q_diff[d] > math.pi: q_offset[d] -= 1
q_prev = copy.deepcopy(q)
q[:] = q + q_offset * 2.0 * math.pi
def cts(self, start_pose, end_pose, maxtries, exe_signal=False):
waypoints = []
fraction = 0.0
attempts = 0
#maxtries_z=300
waypoints.append(start_pose)
waypoints.append(end_pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.01, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
signal = False
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal: self.arm.execute(plan)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = True
return plan, end_joint_state, signal
# shaking function:
# freq : shaking freqence
# times : shaking time per action
def shaking(self, start_joint_state, end_joint_state, freq, times):
q_traj = [start_joint_state]
t_traj = [0.0]
for i in range(times):
q_traj.append(end_joint_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
q_traj.append(start_joint_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
def setupSence(self):
r_tool_size = [0.03, 0.02, 0.18]
l_tool_size = [0.03, 0.02, 0.18]
'''
#sim table
table_pose=PoseStamped()
table_pose.header.frame_id=self.reference_frame
table_pose.pose.position.x=0.75
table_pose.pose.position.y=0.0
table_pose.pose.position.z=self.table_ground+self.table_size[2]/2.0
table_pose.pose.orientation.w=1.0
self.scene.add_box(self.table_id,table_pose,self.table_size)
'''
#real scene table
table_pose = PoseStamped()
table_pose.header.frame_id = self.reference_frame
table_pose.pose.position.x = -0.09
table_pose.pose.position.y = 0.62
table_pose.pose.position.z = self.table_ground + self.table_size[
2] / 2.0
table_pose.pose.orientation.w = 1.0
self.scene.add_box(self.table_id, table_pose, self.table_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = self.arm_end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.057
l_p.pose.position.z = 0.09
l_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p,
l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = self.arm_end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.057
r_p.pose.position.z = 0.09
r_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p,
r_tool_size)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene = PlanningSceneInterface()
pub_traj = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.gripperCtrl = rospy.ServiceProxy(
"/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
#self.m2j=rospy.Publisher("/two_finger/motoman_control/move_to_joint",JointAnglesDuration,queue_size=1,latch=True)
self.colors = dict()
rospy.sleep(1)
self.arm = MoveGroupCommander('arm')
cartesian = rospy.get_param('~cartesian', True)
self.arm_end_effector_link = self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names = [[]]
self.joint_names[0] = rospy.get_param('controller_joint_names')
self.traj = trajectory_msgs.msg.JointTrajectory()
self.sub_jpc = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#scene planning
self.l_id = 'l_tool'
self.r_id = 'r_tool'
self.table_id = 'table'
self.target_id = 'target_object'
self.f_target_id = 'receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.scene.remove_world_object(self.f_target_id)
self.table_ground = 0.13
self.table_size = [0.9, 0.6, 0.018]
self.setupSence()
joint_names = [
'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
]
joint_names = rospy.get_param('controller_joint_names')
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = joint_names
target_size = [0.058, 0.058, 0.19]
f_target_size = [0.2, 0.2, 0.04]
#final target
f_target_pose = PoseStamped()
f_target_pose.header.frame_id = self.reference_frame
f_target_pose.pose.position.x = -0.184 + 0.27
f_target_pose.pose.position.y = 0.62 + 0.1
f_target_pose.pose.position.z = self.table_ground + self.table_size[
2] + f_target_size[2] / 2.0
f_target_pose.pose.orientation.x = 0
f_target_pose.pose.orientation.y = 0
f_target_pose.pose.orientation.z = 0
f_target_pose.pose.orientation.w = 1
self.scene.add_box(self.f_target_id, f_target_pose, f_target_size)
#pouring pose
#set color
self.setColor(self.table_id, 0.8, 0, 0, 1.0)
self.setColor(self.f_target_id, 0.8, 0.4, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.setColor(self.target_id, 0, 1, 0)
self.sendColors()
self.gripperCtrl(255)
#pre pose
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#last pose
j_ori_state = [
1.4624803066253662, 0.6915526390075684, 0.2589389979839325,
-1.9777415990829468, -0.16644451022148132, 1.0748193264007568,
0.25650566816329956
]
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
pour_state = self.arm.get_current_joint_values()
pour_state[6] = 0.25650566816329956
self.arm.set_joint_value_target(pour_state)
self.arm.go()
rospy.sleep(3)
self.gripperCtrl(0)
rospy.sleep(1.5)
#parameters
rotate_ori = 0.174 #10
#rotate_ori=0.348 #30
shake_times = 0
freq = 2.75
amp = 0.15
rotate = rotate_ori * 4
shaking_signal = True
#dfile=open("cab_a0.11_f2.75_d30_p-times3_bt1.txt","a")
#i=0
degree = 10.0
while shaking_signal:
pour_state[6] -= 1.57 + rotate
self.arm.set_joint_value_target(pour_state)
self.arm.go()
rospy.sleep(1)
start_shaking_pose = self.arm.get_current_pose(
self.arm_end_effector_link).pose
shift_pose = deepcopy(start_shaking_pose)
#shift_x_pose=deepcopy(start_shaking_pose)
#shift_z_pose=deepcopy(start_shaking_pose)
shift_pose.position.x += amp * math.sin(rotate)
shift_pose.position.z += amp * math.cos(rotate)
if cartesian:
plan, end_joint_state, signal = self.cts(
start_shaking_pose, shift_pose, 300)
shaking_state = self.arm.get_current_joint_values()
self.shaking(shaking_state, end_joint_state, freq, 5)
#rospy.sleep(1)
pour_state[6] = 0.25650566816329956
#rospy.loginfo("shake_times: "+str(shake_times)+ ",rotate_ori: "+str(rotate_ori)+ ",incre: "+str(incre)+ ". ")
#rotate_ori+=0.0174 #1 degree
#amp+=0.01
#freq+=0.1
shake_times += 1
#area_ratio= rospy.wait_for_message('/color_area_ratio',Float64)
#rospy.loginfo("shake_times: "+str(shake_times)+ ", degree: "+str(degree)+ ", amp: "+str(amp)+ ", ratio: "+str(area_ratio.data))
#dfile.write("%f %f\r\n" % (degree,area_ratio.data))
#degree+=1
if shake_times == 20:
shaking_signal = False
#dfile.close()
rospy.sleep(3.5)
pour_state[6] = 0.25650566816329956
self.arm.set_joint_value_target(pour_state)
self.arm.go()
rospy.sleep(2.5)
self.gripperCtrl(255)
#remove and shut down
#self.scene.remove_attached_object(self.arm_end_effector_link,'l_tool')
#self.scene.remove_attached_object(self.arm_end_effector_link,'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
speed = Twist()
r = rospy.Rate(4)
## 매니퓰레이터 변수 선언
group_name = "ur3_manipulator"
move_group = MoveGroupCommander(group_name)
FIXED_FRAME = 'world'
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', DisplayTrajectory, queue_size=20)
#통합제어
joint_goal = move_group.get_current_joint_values()
#변수 선언
mobile_joints = [-pi / 3, 0.5]
joint_goal_list = [
pi * 90 / 180, pi * -130 / 180, pi * 111 / 180, pi * -68 / 180,
pi * -90 / 180, 0
] #home pose
#모바일 제어
goal_x_dist = mobile_joints[1] #meter
angle_to_goal = mobile_joints[0]
goal_distance = 0
r.sleep()
if theta - angle_to_goal > 0:
node_name = "commander_example"
rospy.init_node( node_name, anonymous=True )
group = MoveGroupCommander("arm")
gripper_group = MoveGroupCommander("gripper")
group.set_planning_time( 600.0 )
# Getting Initial Pose & RPY
pose_init = group.get_current_pose()
rospy.loginfo( "Get Initial Pose\n{}".format( pose_init ) )
# add gripper joint status
gripper_status_init = gripper_group.get_current_joint_values()
rospy.loginfo( "Get Initial Posesssssss\n{}".format(gripper_status_init) )
rpy_init = group.get_current_rpy()
rospy.loginfo( "Get Initial RPY:{}".format( rpy_init ) )
# Pose 1
rospy.loginfo( "Starting Pose 1")
pose_target_1 = [ 0.12, 0.0, 0.1, 0.0, math.pi/2.0, 0.0 ] # [ x, y, z, r, p, y ]
group.set_pose_target( pose_target_1 )
group.go()
gripper_target_1 = [ 0.01, 0]
gripper_group.set_joint_value_target(gripper_target_1)
gripper_group.go()
1.0374444266562388, 1.5704395720709645, -2.698212672994671
]
#def default_grip(arm):
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
eef = MoveGroupCommander("endeffector")
rospy.sleep(1)
#Start State Gripper
group_variable_values = eef.get_current_joint_values()
group_variable_values[0] = 0.0
eef.set_joint_value_target(group_variable_values)
plan2 = eef.plan()
arm.execute(plan2)
# clean the scene
scene.remove_world_object("pole")
scene.remove_world_object("table")
scene.remove_world_object("part")
# publish a demo scene
k = PoseStamped()
k.header.frame_id = robot.get_planning_frame()
k.pose.position.x = 0.0
k.pose.position.y = 0.0
k.pose.position.z = -0.05
class PickAndPlace:
def setColor(self, name, r, g, b, a=0.9):
color = ObjectColor()
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
self.colors[name] = color
def sendColors(self):
p = PlanningScene()
p.is_diff = True
for color in self.colors.values():
p.object_colors.append(color)
self.scene_pub.publish(p)
def add_point(self, traj, time, positions, velocities=None):
point = trajectory_msgs.msg.JointTrajectoryPoint()
point.positions = copy.deepcopy(positions)
if velocities is not None:
point.velocities = copy.deepcopy(velocities)
point.time_from_start = rospy.Duration(time)
traj.points.append(point)
def FollowQTraj(self, q_traj, t_traj):
assert (len(q_traj) == len(t_traj))
#Insert current position to beginning.
if t_traj[0] > 1.0e-2:
t_traj.insert(0, 0.0)
q_traj.insert(0, self.Q(arm=arm))
self.dq_traj = self.QTrajToDQTraj(q_traj, t_traj)
def QTrajToDQTraj(self, q_traj, t_traj):
dof = len(q_traj[0])
#Modeling the trajectory with spline.
splines = [TCubicHermiteSpline() for d in range(dof)]
for d in range(len(splines)):
data_d = [[t, q[d]] for q, t in zip(q_traj, t_traj)]
splines[d].Initialize(data_d,
tan_method=splines[d].CARDINAL,
c=0.0,
m=0.0)
#NOTE: We don't have to make spline models as we just want velocities at key points.
# They can be obtained by computing tan_method, which will be more efficient. with_tan=True
dq_traj = []
for t in t_traj:
dq = [splines[d].Evaluate(t, with_tan=True)[1] for d in range(dof)]
dq_traj.append(dq)
#print dq_traj
return dq_traj
def JointNames(self):
#0arm= 0
return self.joint_names[0]
def ROSGetJTP(self, q, t, dq=None):
jp = trajectory_msgs.msg.JointTrajectoryPoint()
jp.positions = q
jp.time_from_start = rospy.Duration(t)
if dq is not None: jp.velocities = dq
return jp
def ToROSTrajectory(self, joint_names, q_traj, t_traj, dq_traj=None):
assert (len(q_traj) == len(t_traj))
if dq_traj is not None: (len(dq_traj) == len(t_traj))
#traj= trajectory_msgs.msg.JointTrajectory()
self.traj.joint_names = joint_names
if dq_traj is not None:
self.traj.points = [
self.ROSGetJTP(q, t, dq)
for q, t, dq in zip(q_traj, t_traj, dq_traj)
]
else:
self.traj.points = [
self.ROSGetJTP(q, t) for q, t in zip(q_traj, t_traj)
]
self.traj.header.stamp = rospy.Time.now()
#print self.traj
return self.traj
def SmoothQTraj(self, q_traj):
if len(q_traj) == 0: return
q_prev = np.array(q_traj[0])
q_offset = np.array([0] * len(q_prev))
for q in q_traj:
q_diff = np.array(q) - q_prev
for d in range(len(q_prev)):
if q_diff[d] < -math.pi: q_offset[d] += 1
elif q_diff[d] > math.pi: q_offset[d] -= 1
q_prev = copy.deepcopy(q)
q[:] = q + q_offset * 2.0 * math.pi
def add_target(self, target_pose, target_size, frame, x, y, o1, o2, o3,
o4):
target_pose.header.frame_id = frame
target_pose.pose.position.x = x
target_pose.pose.position.y = y
target_pose.pose.position.z = self.table_ground + self.table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.x = o1
target_pose.pose.orientation.y = o2
target_pose.pose.orientation.z = o3
target_pose.pose.orientation.w = o4
#self.scene.add_box(f_target_id,f_target_pose,f_target_size)
def cts(self, start_pose, end_pose, maxtries, exe_signal=False):
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
for i in range(2, len(plan.joint_trajectory.points)):
q_traj.append(
plan.joint_trajectory.points[i].positions)
t_traj.append(t_traj[-1] + 0.08)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = 1
return signal, end_joint_state
#move and rotate
def cts_rotate(self,
start_pose,
end_pose,
angle_r,
maxtries,
exe_signal=False):
angle = angle_r * 3.14 / 180.0
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0.0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
per_angle = angle / (len(plan.joint_trajectory.points) - 2)
for i in range(2, len(plan.joint_trajectory.points)):
joint_inc_list = [
j
for j in plan.joint_trajectory.points[i].positions
]
#plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
joint_inc_list[6] -= per_angle * (i - 1)
q_traj.append(joint_inc_list)
t_traj.append(t_traj[-1] + 0.07)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
end_joint_state = plan.joint_trajectory.points[-1].positions
signal = 1
return signal, end_joint_state
def cts_rotate_delta(self,
start_pose,
end_pose,
angle_pre,
angle_r,
maxtries,
exe_signal=False):
angle = (angle_r - angle_pre) * 3.14 / 180.0
waypoints = []
fraction = 0.0
attempts = 0
waypoints.append(start_pose.pose)
waypoints.append(end_pose.pose)
while fraction != 1 and attempts < maxtries:
(plan, fraction) = self.arm.compute_cartesian_path(
waypoints, 0.005, 0.0, True)
attempts += 1
if (attempts % maxtries == 0 and fraction != 1):
rospy.loginfo("path planning failed with " +
str(fraction * 100) + "% success.")
return 0, 0.0
continue
elif fraction == 1:
rospy.loginfo("path compute successfully with " +
str(attempts) + " attempts.")
if exe_signal:
end_joint_state = plan.joint_trajectory.points[
-1].positions
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
per_angle = angle / (len(plan.joint_trajectory.points) - 2)
for i in range(2, len(plan.joint_trajectory.points)):
joint_inc_list = [
j
for j in plan.joint_trajectory.points[i].positions
]
#~ plan.joint_trajectory.points[i].positions[6]-=per_angle*(i-1)
#~ if i==len(plan.joint_trajectory.points)-1:
#~ joint_inc_list[6]-=angle
#~ q_traj.append(joint_inc_list)
#~ t_traj.append(1.5)
joint_inc_list[6] -= per_angle * (i - 1) + (
angle_pre * 3.14 / 180.0)
q_traj.append(joint_inc_list)
t_traj.append(t_traj[-1] + 0.1)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
signal = 1
return signal, end_joint_state
# shaking function:
# freq : shaking freqence
# times : shaking time per action
def shaking(self, initial_state, end_joint_state, freq, times):
q_traj = [initial_state]
t_traj = [0.0]
for i in range(times):
q_traj.append(end_joint_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
q_traj.append(initial_state)
t_traj.append(t_traj[-1] + 0.5 / freq)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(6)
def shake_a(self, pre_p_angle, r_angle, amp):
start_shake_pose = self.arm.get_current_pose(
self.arm_end_effector_link) # for trajectory of shaking
start_joint_state = self.arm.get_current_joint_values(
) # for state[6] to rotate
shift_pose = deepcopy(start_shake_pose)
r_angle = (r_angle / 180.0) * 3.14
pre_p_angle = (pre_p_angle / 180.0) * 3.14
shift_pose.pose.position.x += amp * math.sin(r_angle) * math.cos(
pre_p_angle) # in verticle direction
shift_pose.pose.position.y -= amp * math.sin(r_angle) * math.sin(
pre_p_angle)
shift_pose.pose.position.z += amp * math.cos(r_angle) #...
signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
return signal, end_joint_state
def shake_b(self, pre_p_angle, r_angle, amp):
start_shake_pose = self.arm.get_current_pose(
self.arm_end_effector_link) # for trajectory of shaking
start_joint_state = self.arm.get_current_joint_values(
) # for state[6] to rotate
shift_pose = deepcopy(start_shake_pose)
r_angle = (r_angle / 180.0) * 3.14
pre_p_angle = (pre_p_angle / 180.0) * 3.14
shift_pose.pose.position.x -= amp * math.cos(r_angle) * math.cos(
pre_p_angle) # in verticle direction
shift_pose.pose.position.y += amp * math.cos(r_angle) * math.sin(
pre_p_angle)
shift_pose.pose.position.z += amp * math.sin(r_angle) #...
signal, end_joint_state = self.cts(start_shake_pose, shift_pose, 300)
return signal, end_joint_state
def setupSence(self):
r_tool_size = [0.03, 0.02, 0.18]
l_tool_size = [0.03, 0.02, 0.18]
#real scene table
table_pose = PoseStamped()
table_pose.header.frame_id = self.reference_frame
table_pose.pose.position.x = -0.052
table_pose.pose.position.y = 0.65
table_pose.pose.position.z = self.table_ground + self.table_size[
2] / 2.0
table_pose.pose.orientation.w = 1.0
self.scene.add_box(self.table_id, table_pose, self.table_size)
#left gripper
l_p = PoseStamped()
l_p.header.frame_id = self.arm_end_effector_link
l_p.pose.position.x = 0.00
l_p.pose.position.y = 0.057
l_p.pose.position.z = 0.09
l_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.l_id, l_p,
l_tool_size)
#right gripper
r_p = PoseStamped()
r_p.header.frame_id = self.arm_end_effector_link
r_p.pose.position.x = 0.00
r_p.pose.position.y = -0.057
r_p.pose.position.z = 0.09
r_p.pose.orientation.w = 1
self.scene.attach_box(self.arm_end_effector_link, self.r_id, r_p,
r_tool_size)
#Params: pose of bottle, grasp_radius, grasp_height, grasp_theta
def get_grasp_pose(self, pose, r, theta):
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = pose.pose.position.x + r * math.sin(theta)
g_p.pose.position.y = pose.pose.position.y - r * math.cos(theta)
g_p.pose.position.z = pose.pose.position.z
g_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
g_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
math.sin(0.5 * theta))
g_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
g_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
math.cos(0.5 * theta))
return g_p
def get_pour_pose(self, pose, h, r, theta):
p_p = PoseStamped()
p_p.header.frame_id = self.arm_end_effector_link
p_p.pose.position.x = pose.pose.position.x - r * math.cos(theta)
p_p.pose.position.y = pose.pose.position.y + r * math.sin(theta)
p_p.pose.position.z = pose.pose.position.z + h
theta *= -1
p_p.pose.orientation.w = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
p_p.pose.orientation.x = -0.5 * (math.cos(0.5 * theta) +
math.sin(0.5 * theta))
p_p.pose.orientation.y = 0.5 * (math.cos(0.5 * theta) -
math.sin(0.5 * theta))
p_p.pose.orientation.z = 0.5 * (math.sin(0.5 * theta) +
math.cos(0.5 * theta))
return p_p
def pour_rotate(self, initial_pose, angle_pre, angle_r, r, maxtries):
angle_pre = (angle_pre / 180.0) * 3.14
angle_r_1 = (angle_r / 180.0) * 3.14
cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
final_pose = deepcopy(initial_pose)
final_pose.pose.position.x += r * (
1 - math.cos(angle_r_1)) * math.cos(angle_pre)
final_pose.pose.position.y += r * (
1 - math.cos(angle_r_1)) * math.sin(angle_pre)
final_pose.pose.position.z += r * math.sin(angle_r_1)
#~ signal,e_j_s=self.cts_rotate_delta(cur_pose,final_pose,angle_r_pre,angle_r,maxtries,True)
signal, e_j_s = self.cts_rotate(cur_pose, final_pose, angle_r,
maxtries, True)
return signal
def p_r_trail(self, angle_pre, angle_r, r, maxtries):
angle_pre = (angle_pre / 180.0) * 3.14
angle_r_1 = (angle_r / 180.0) * 3.14
initial_pose = self.arm.get_current_pose(self.arm_end_effector_link)
final_pose = deepcopy(initial_pose)
final_pose.pose.position.x += r * (
1 - math.cos(angle_r_1)) * math.cos(angle_pre)
final_pose.pose.position.y += r * (
1 - math.cos(angle_r_1)) * math.sin(angle_pre)
final_pose.pose.position.z += r * math.sin(angle_r_1)
signal, e_j_s = self.cts_rotate(initial_pose, final_pose, angle_r,
maxtries, True)
return signal
def move_back(self, back_pose):
current_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, end_j = self.cts(current_pose, back_pose, 300, True)
return signal
def pg_g_pp(self, pose, r):
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
p_i_radian = np.arctan(abs(pose.pose.position.x /
pose.pose.position.y))
p_i_angle = p_i_radian * 180.0 / 3.14
pick_list = [
p_i_angle, 5.0, 25.0, 45.0, 65.0, 15.0, 35.0, 55.0, 75.0, 10.0,
20.0, 30.0, 40.0, 50.0, 60.0
]
for i in range(len(pick_list)):
theta = (pick_list[i] / 180.0) * 3.14
if pose.pose.position.x > 0:
theta *= -1.0
grasp_pose = self.get_grasp_pose(pose, r, theta)
#pick up
signal, e_j_s = self.cts(start_pose, grasp_pose, 300, True)
if signal == 0: continue
break
rospy.sleep(1)
self.gripperCtrl(0)
rospy.sleep(2)
#move to ori_pose
current_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e_j_s = self.cts(current_pose, start_pose, 300, True)
rospy.sleep(2)
rospy.loginfo("Grasping done.")
return start_pose, grasp_pose
def pp_ps_old(self, target_pose, pour_angle, r_pour, h_pour):
for i in range(len(pour_angle)):
maxtries = 300
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
theta = (pour_angle[i] / 180.0) * 3.14
pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta)
#move to pose
signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0,
maxtries, True)
if signal_1 == 0: continue
pre_pour_angle = pour_angle[i]
rospy.loginfo("Ready for pouring.")
return pre_pour_angle
def pp_ps(self, target_pose, pour_angle, r_pour, h_pour):
maxtries = 300
start_pose = self.arm.get_current_pose(self.arm_end_effector_link)
theta = (pour_angle / 180.0) * 3.14
pour_pose = self.get_pour_pose(target_pose, h_pour, r_pour, theta)
#move to pose
signal_1, e_j_s = self.cts_rotate(start_pose, pour_pose, 90.0,
maxtries, True)
return signal_1
def go_back(self, ori_pose, pre_grasp_pose):
cur_pose = self.arm.get_current_pose(self.arm_end_effector_link)
signal, e1 = self.cts(cur_pose, ori_pose, 300, True)
rospy.loginfo("back to pre_grasp pose, ready for placing bottle..")
rospy.sleep(1)
signal_1, e2 = self.cts(ori_pose, pre_grasp_pose, 300, True)
rospy.loginfo("back to grasp pose, open gripper..")
rospy.sleep(1)
self.gripperCtrl(255)
rospy.sleep(1)
signal_2, e3 = self.cts(pre_grasp_pose, ori_pose, 300, True)
rospy.loginfo("back to pre_grasp pose, ready for next kind of source.")
def rotate_back(self, joint_state):
q_traj = [self.arm.get_current_joint_values()]
t_traj = [0.0]
q_traj.append(joint_state)
t_traj.append(t_traj[-1] + 4)
self.FollowQTraj(q_traj, t_traj)
self.sub_jpc.publish(
self.ToROSTrajectory(self.JointNames(), q_traj, t_traj,
self.dq_traj))
rospy.sleep(5)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.scene = PlanningSceneInterface()
pub_traj = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#pub_ratio_sig= rospy.Publisher('/enable_source_change',Int64,queue_size=1)
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
self.gripperCtrl = rospy.ServiceProxy(
"/two_finger/gripper/gotoPositionUntilTouch", SetPosition)
self.colors = dict()
rospy.sleep(1)
self.robot = moveit_commander.robot.RobotCommander()
self.arm = MoveGroupCommander('arm')
self.arm.allow_replanning(True)
cartesian = rospy.get_param('~cartesian', True)
self.arm_end_effector_link = self.arm.get_end_effector_link()
self.arm.set_goal_position_tolerance(0.005)
self.arm.set_goal_orientation_tolerance(0.025)
self.arm.allow_replanning(True)
self.reference_frame = 'base_link'
self.arm.set_pose_reference_frame(self.reference_frame)
self.arm.set_planning_time(5)
#shaking
self.joint_names = [[]]
self.joint_names[0] = rospy.get_param('controller_joint_names')
self.traj = trajectory_msgs.msg.JointTrajectory()
self.sub_jpc = rospy.Publisher('/joint_path_command',
trajectory_msgs.msg.JointTrajectory,
queue_size=10)
#scene planning
self.l_id = 'l_tool'
self.r_id = 'r_tool'
self.table_id = 'table'
self.target1_id = 'target1_object'
self.target2_id = 'target2_object'
#~ self.target3_id='target3_object'
#~ self.target4_id='target4_object'
self.f_target_id = 'receive_container'
self.scene.remove_world_object(self.l_id)
self.scene.remove_world_object(self.r_id)
self.scene.remove_world_object(self.table_id)
self.scene.remove_world_object(self.target1_id)
self.scene.remove_world_object(self.target2_id)
#~ self.scene.remove_world_object(self.target3_id)
#~ self.scene.remove_world_object(self.target4_id)
self.scene.remove_world_object(self.f_target_id)
#self.scene.remove_attached_object(self.arm_end_effector_link,self.target_id)
self.table_ground = 0.13
self.table_size = [0.9, 0.6, 0.018]
self.setupSence()
target1_size = [0.035, 0.035, 0.19]
target2_size = target1_size
#~ target3_size=target1_size
#~ target4_size=target1_size
self.f_target_size = [0.2, 0.2, 0.04]
f_target_pose = PoseStamped()
pre_pour_pose = PoseStamped()
target1_pose = PoseStamped()
target2_pose = PoseStamped()
#~ target3_pose=PoseStamped()
#~ target4_pose=PoseStamped()
joint_names = [
'joint_' + jkey for jkey in ('s', 'l', 'e', 'u', 'r', 'b', 't')
]
joint_names = rospy.get_param('controller_joint_names')
traj = trajectory_msgs.msg.JointTrajectory()
traj.joint_names = joint_names
#final target
l_gripper = 0.18
#self.add_target(f_target_pose,self.f_target_size,self.reference_frame,-0.184+0.27,0.62+0.1,0,0,0,1)
#~ self.add_target(f_target_pose,self.f_target_size,self.reference_frame,0.24,0.6-l_gripper,0,0,0,1)
#~ self.scene.add_box(self.f_target_id,f_target_pose,self.f_target_size)
#self.add_target(pre_pour_pose,self.reference_frame,x,y,0,0,0,1)
#target localization
msg1 = rospy.wait_for_message('/aruco_simple/pose', Pose)
rospy.sleep(1)
msg2 = rospy.wait_for_message('/aruco_simple/pose2', Pose)
rospy.sleep(1)
self.add_target(f_target_pose, self.f_target_size,
self.reference_frame, -msg2.position.y - 0.257,
-msg2.position.x + 0.81 - 0.1 - l_gripper, 0, 0, 0, 1)
self.scene.add_box(self.f_target_id, f_target_pose, self.f_target_size)
self.add_target(target1_pose, target1_size, self.reference_frame,
-msg1.position.y - 0.257,
-msg1.position.x + 0.81 - 0.065, 0, 0, 0, 1)
self.scene.add_box(self.target1_id, target1_pose, target1_size)
#~ self.add_target(target2_pose,target2_size,self.reference_frame,-msg2.position.y-0.257,-msg2.position.x+0.81-0.065,0,0,0,1)
#~ self.scene.add_box(self.target2_id,target2_pose,target2_size)
#~ self.add_target(target3_pose,target3_size,self.reference_frame,-0.01,0.76-0.01,0,0,0,1)
#~ self.scene.add_box(self.target3_id,target3_pose,target3_size)
#~ self.add_target(target4_pose,target4_size,self.reference_frame,0.25,0.80,0,0,0,1)
#~ self.scene.add_box(self.target4_id,target4_pose,target4_size)
#attach_pose
g_p = PoseStamped()
g_p.header.frame_id = self.arm_end_effector_link
g_p.pose.position.x = 0.00
g_p.pose.position.y = -0.00
g_p.pose.position.z = 0.025
g_p.pose.orientation.w = 0.707
g_p.pose.orientation.x = 0
g_p.pose.orientation.y = -0.707
g_p.pose.orientation.z = 0
#set color
self.setColor(self.target1_id, 0.8, 0, 0, 1.0)
self.setColor(self.target2_id, 0.8, 0, 0, 1.0)
#~ self.setColor(self.target3_id,0.8,0,0,1.0)
#self.setColor(self.target4_id,0.8,0,0,1.0)
self.setColor(self.f_target_id, 0.8, 0.3, 0, 1.0)
self.setColor('r_tool', 0.8, 0, 0)
self.setColor('l_tool', 0.8, 0, 0)
self.sendColors()
self.gripperCtrl(255)
rospy.sleep(3)
self.arm.set_named_target("initial_arm")
self.arm.go()
rospy.sleep(3)
#j_ori_state=[-1.899937629699707, -0.5684762597084045, 0.46537330746650696, 2.3229329586029053, -0.057941947132349014, -1.2867668867111206, 0.2628822326660156]
#j_ori_state=[-2.161055326461792, -0.6802523136138916, -1.7733728885650635, -2.3315746784210205, -0.5292841196060181, 1.4411976337432861, -2.2327845096588135]
j_ori_state = [
-1.2628753185272217, -0.442996621131897, -0.1326361745595932,
2.333048105239868, -0.15598002076148987, -1.2167049646377563,
3.1414425373077393
]
#signal= True
self.arm.set_joint_value_target(j_ori_state)
self.arm.go()
rospy.sleep(3)
#parameter setup
tar_num = 1
maxtries = 300
r_grasp = 0.16
#~ topic_name=["/ratio_carrot","/ratio_lettuce","ratio_pepper"]
#~ save_data=["carrot.txt","pepper.txt","cucumber.txt"]
#ratio_table=[0.03,0.025,0.04]
for i in range(0, tar_num):
#grasp target
rospy.loginfo("Choosing source...")
#~ dfile=open(save_data[i],"a")
if i == 0:
target_pose = target1_pose
target_id = self.target1_id
target_size = target1_size
#~ amp=0.13
#~ freq=2.75
#~ r_angle=40.0
elif i == 1:
target_pose = target2_pose
target_id = self.target2_id
target_size = target2_size
#~ amp=0.15
#~ freq=2.5
#~ r_angle=45.0
elif i == 2:
target_pose = target3_pose
target_id = self.target3_id
target_size = target3_size
amp = 0.15
freq = 2.75
r_angle = 40.0
elif i == 3:
target_pose = target4_pose
target_id = self.target4_id
target_size = target4_size
amp = 0.1
freq = 2.75
r_angle = 45.0
r_pour = 0.13
r_bottle = 0.1
h_pour = 0.1
pour_angle = [0.0, -15.0, 15.0, -30.0, 30.0, -45.0, 45.0]
#~ tip_angle=[10.0,20.0,30.0,40.0,50.0,60.0,70.0,80.0,90.0]
#~ tip_angle=[15.0,30.0,45.0,60.0,75.0,90.0]
tip_angle = [20.0, 30.0, 40.0, 50.0, 60.0, 70.00]
shake_freq = [2.0, 2.25, 2.50, 2.75]
shake_amp = [0.05, 0.075, 0.1, 0.125, 0.15]
shake_per_times = 5
shake_times_tgt = 2
rospy.loginfo("Params loaded.")
rospy.loginfo("Current Source: " + str(i + 1) + " ")
#grasp and back
ori_pose, g_pose = self.pg_g_pp(target_pose, r_grasp)
ori_joint_state = self.arm.get_current_joint_values()
#move to target position for pouring
for m in range(len(pour_angle)):
for j in range(len(tip_angle)):
for k in range(len(shake_freq)):
dfile = open(
"pepper_tipangle_" + str(tip_angle[j]) +
"_shaking_freq_" + str(shake_freq[k]) + "_1", "a")
for n in range(len(shake_amp)):
pp_ps_sgn = self.pp_ps(f_target_pose,
pour_angle[m], r_pour,
h_pour)
if pp_ps_sgn == 0:
rospy.loginfo(
"pre-Pouring angle not correct, back for replanning..."
)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
initial_pose = self.arm.get_current_pose(
self.arm_end_effector_link)
pour_signal = self.pour_rotate(
initial_pose, pour_angle[m], tip_angle[j],
r_bottle, maxtries)
if pour_signal != 1:
rospy.loginfo(
"Pouring angle not correct, back for replanning..."
)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
continue
amp = shake_amp[n]
freq = shake_freq[k]
rospy.loginfo("shaking degree : " +
str(tip_angle[j]) +
", shaking amp : " + str(amp) +
", shaking freq : " + str(freq))
shake_times = 0
signal = True
start_joint_state = self.arm.get_current_joint_values(
)
signal, end_joint_state = self.shake_a(
pour_angle[m], tip_angle[j], amp)
while signal == 1:
#~ area_ratio= rospy.wait_for_message('/ratio_carrot',Float64)
#if (area_ratio>ratio_table[i]) or (shake_times>shake_times_tgt):
if (shake_times < shake_times_tgt):
self.shaking(start_joint_state,
end_joint_state, freq,
shake_per_times)
rospy.sleep(2)
shake_times += 1
rospy.loginfo("shaking times : " +
str(shake_times) + " .")
area_ratio = rospy.wait_for_message(
'/ratio_carrot', Float64)
if shake_times == shake_times_tgt:
dfile.write(
"%f %f \r\n" %
(shake_amp[n], area_ratio.data))
self.rotate_back(ori_joint_state)
rospy.sleep(2)
else:
signal = 0
self.rotate_back(ori_joint_state)
rospy.sleep(2)
self.rotate_back(ori_joint_state)
rospy.sleep(2)
#remove and shut down
self.scene.remove_attached_object(self.arm_end_effector_link, 'l_tool')
self.scene.remove_attached_object(self.arm_end_effector_link, 'r_tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('TCP_Move', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('xarm6')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('link_base')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.1)
arm.set_max_velocity_scaling_factor(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 角度弧度转换
j1 = 90.0 / 180 * math.pi
j2 = -18.6 / 180 * math.pi
j3 = -28.1 / 180 * math.pi
j4 = 1.0 / 180 * math.pi
j5 = 47.6 / 180 * math.pi
j6 = -0.9 / 180 * math.pi
# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）
joint_positions = [j1, j2, j3, j4, j5, j6]
arm.set_joint_value_target(joint_positions)
arm.go()
rospy.sleep(1)
# 向下按压门把手
current_pose = arm.get_current_joint_values()
current_pose[4] += (20.0 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
#推开门
current_pose = arm.get_current_joint_values()
current_pose[0] -= (42.0 / 180.0) * math.pi
current_pose[1] += (2.0 / 180.0) * math.pi
current_pose[2] -= (11.4 / 180.0) * math.pi
arm.set_joint_value_target(current_pose)
arm.go()
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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)):
if robot_joints[5] < 0:
robot_joints[5] = robot_joints[5] + math.radians(90) # wrist3
print "---- robot_joints wrist3: deg, rad", math.degrees(
robot_joints[5]), robot_joints[5]
elif robot_joints[5] > 0:
robot_joints[5] = robot_joints[5] - math.radians(90)
