def move_to_coord(trans, rot, keep_oreint=False):
#coordinates are in baxter's torso!
global arm
goal = PoseStamped()
goal.header.frame_id = "base"
# x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]
# Orientation as a quaternion
goal.pose.orientation.x = rot[0]
goal.pose.orientation.y = rot[1]
goal.pose.orientation.z = rot[2]
goal.pose.orientation.w = rot[3]
# Set the goal state to the pose you just defined
arm.set_pose_target(goal)
# Set the start state for the arm
arm.set_start_state_to_current_state()
if keep_oreint:
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = 'left'+"_gripper";
orien_const.header.frame_id = "base";
#constrain it to be the same as my goal state. Seems reasonable.
orien_const.orientation.x = rot[0];
orien_const.orientation.y = rot[1];
orien_const.orientation.z = rot[2];
orien_const.orientation.w = rot[3];
orien_const.absolute_x_axis_tolerance = 0.02;
orien_const.absolute_y_axis_tolerance = 0.02;
orien_const.absolute_z_axis_tolerance = 0.02;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
print(consts)
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
#arm.execute(arm_plan)
arm.go(arm_plan,wait=True)
def planMoveToPosHoldOrientation(pos=None, orientation=None, arm=None):
arm = arm or left_arm
if arm is left_arm:
pos = pos or left_arm_default_state[0]
orientation = orientation or left_arm_default_state[1]
if arm is right_arm:
pos = pos or right_arm_default_state[0]
orientation = orientation or right_arm_default_state[1]
#Second goal pose -----------------------------------------------------
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = pos[0]
goal_2.pose.position.y = pos[1]
goal_2.pose.position.z = pos[2]
#Orientation as a quaternion
orientation = orientation/np.linalg.norm(orientation)
goal_2.pose.orientation.x = orientation[0]
goal_2.pose.orientation.y = orientation[1]
goal_2.pose.orientation.z = orientation[2]
goal_2.pose.orientation.w = orientation[3]
#Set the goal state to the pose you just defined
arm.set_pose_target(goal_2)
#Set the start state for the left arm
arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = orientation[0]
orien_const.orientation.y = orientation[1]
orien_const.orientation.z = orientation[2]
orien_const.orientation.w = orientation[3]
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
#Plan a path
return (arm, arm.plan())
def planPath(positions, orientation=None, arm=None, holdOrientation=False, step=0.01, threshold=1000):
arm = arm or left_arm
if arm is left_arm:
positions = (left_arm_default_state[0],) if positions is None else positions
orientation = left_arm_default_state[1] if orientation is None else orientation
if arm is right_arm:
positions = (right_arm_default_state[0],) if positions is None else positions
orientation = right_arm_default_state[1] if orientation is None else orientation
# Compute path
waypoints = []
# start with the current pose
#waypoints.append(arm.get_current_pose().pose)
wpose = Pose()
orientation = orientation/np.linalg.norm(orientation)
wpose.orientation.x = orientation[0]
wpose.orientation.y = orientation[1]
wpose.orientation.z = orientation[2]
wpose.orientation.w = orientation[3]
for pos in positions:
wpose.position.x = pos[0]
wpose.position.y = pos[1]
wpose.position.z = pos[2]
waypoints.append(copy.deepcopy(wpose))
if holdOrientation:
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = orientation[0]
orien_const.orientation.y = orientation[1]
orien_const.orientation.z = orientation[2]
orien_const.orientation.w = orientation[3]
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
#Plan a path
(plan, fraction) = arm.compute_cartesian_path(waypoints, step, threshold)
return (arm, plan, fraction)
def move_to_coord(trans, rot, arm, which_arm='left', keep_oreint=False):
goal = PoseStamped()
goal.header.frame_id = "base"
# x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]
# Orientation as a quaternion
goal.pose.orientation.x = rot[0]
goal.pose.orientation.y = rot[1]
goal.pose.orientation.z = rot[2]
goal.pose.orientation.w = rot[3]
# Set the goal state to the pose you just defined
arm.set_pose_target(goal)
# Set the start state for the arm
arm.set_start_state_to_current_state()
if keep_oreint:
# Create a path constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = which_arm+"_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
arm.set_path_constraints(consts)
# Plan a path
arm_plan = arm.plan()
# Execute the plan
arm.execute(arm_plan)
def callback(message):
global last_x
global last_y
global last_z
global error
try:
if message.transforms[0].child_frame_id == 'ar_marker_23':
#Read the position of artag
(trans,rot) = listener.lookupTransform('/base', '/ar_marker_23', rospy.Time(0))
#Execute only when the difference of the current position and the last position exceed the threshold
if ((abs(trans[0]-last_x) < threshold) and (abs(trans[1]-last_y) < threshold) and (abs(trans[2]-last_z) < threshold)):
pass
else:
last_x = trans[0]
last_y = trans[1]
last_z = trans[2]
print trans
#Goal position
goal = PoseStamped()
goal.header.frame_id = "base"
#x, y, and z position
goal.pose.position.x = trans[0]
goal.pose.position.y = trans[1]
goal.pose.position.z = trans[2]-0.1
#Orientation as a quaternion: default orientation
goal.pose.orientation.x = 0.5
goal.pose.orientation.y = 0.5
goal.pose.orientation.z = 0.5
goal.pose.orientation.w = -0.5
#Set the goal state
right_arm.set_pose_target(goal)
#Set the start state
right_arm.set_start_state_to_current_state()
#Create a orientation constraint for the arm
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.x = 0.5;
orien_const.orientation.y = 0.5;
orien_const.orientation.z = 0.5;
orien_const.orientation.w = -0.5;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
right_arm.execute(right_plan)
else:
pass
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print 'exception'
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the left gripper
left_gripper = baxter_gripper.Gripper('left')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
left_gripper.calibrate()
rospy.sleep(2.0)
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
left_arm = moveit_commander.MoveGroupCommander('left_arm')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
left_arm.set_planner_id('RRTConnectkConfigDefault')
left_arm.set_planning_time(10)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(10)
#First goal pose ------------------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.2
goal_1.pose.position.y = 0.6
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_1)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 1 (path constraints are never enforced during this motion): ')
left_arm.execute(left_plan)
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
print('Done!')
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = 0.4
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_2)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 2: ')
left_arm.execute(left_plan)
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.0
goal_3.pose.position.y = 0.7
goal_3.pose.position.z = 0.0
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_3)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "left_gripper";
orien_const.header.frame_id = "base";
orien_const.orientation.y = -1.0;
orien_const.absolute_x_axis_tolerance = 0.1;
orien_const.absolute_y_axis_tolerance = 0.1;
orien_const.absolute_z_axis_tolerance = 0.1;
orien_const.weight = 1.0;
consts = Constraints()
consts.orientation_constraints = [orien_const]
left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 3: ')
left_arm.execute(left_plan)
#Fourth goal pose -----------------------------------------------------
rospy.sleep(2.0)
#Close the left gripper
#print('Closing...')
#left_gripper.close(block=True)
#rospy.sleep(0.5)
#Open the left gripper
#print('Opening...')
#left_gripper.open(block=True)
#rospy.sleep(1.0)
#print('Done!')
goal_4 = PoseStamped()
goal_4.header.frame_id = "base"
#x, y, and z position
goal_4.pose.position.x = 0.4
goal_4.pose.position.y = 0.7
goal_4.pose.position.z = 0.3
#Orientation as a quaternion
goal_4.pose.orientation.x = 0.0
goal_4.pose.orientation.y = -1.0
goal_4.pose.orientation.z = 0.0
goal_4.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
left_arm.set_pose_target(goal_4)
#Set the start state for the left arm
left_arm.set_start_state_to_current_state()
#Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
#orien_const = OrientationConstraint()
#orien_const.link_name = "left_gripper";
#orien_const.header.frame_id = "base";
#orien_const.orientation.y = -1.0;
#orien_const.absolute_x_axis_tolerance = 0.1;
#orien_const.absolute_y_axis_tolerance = 0.1;
#orien_const.absolute_z_axis_tolerance = 0.1;
#orien_const.weight = 1.0;
#consts = Constraints()
#consts.orientation_constraints = [orien_const]
#left_arm.set_path_constraints(consts)
#Plan a path
left_plan = left_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the left arm to goal pose 4: ')
left_arm.execute(left_plan)
rospy.sleep(2.0)
#Close the left gripper
print('Closing...')
left_gripper.close(block=True)
rospy.sleep(0.5)
#Open the left gripper
print('Opening...')
left_gripper.open(block=True)
rospy.sleep(1.0)
print('Done!')
def right_arm_go_to_pose_goal(self):
# 设置动作对象变量,此处为arm
right_arm = self.right_arm
# 获取当前末端执行器位置姿态
pose_goal = right_arm.get_current_pose().pose
# 限制末端夹具运动
right_joint_const = JointConstraint()
right_joint_const.joint_name = "gripper_r_joint_r"
if Rightfinger > -55 :
right_joint_const.position = 0.024
else:
right_joint_const.position = 0
right_joint_const.weight = 1.0
# 限制1轴转动
right_joint1_const = JointConstraint()
right_joint1_const.joint_name = "yumi_joint_1_r"
right_joint1_const.position = 0
right_joint1_const.tolerance_above = 120
right_joint1_const.tolerance_below = 0
right_joint1_const.weight = 1.0
# 限制2轴转动
right_joint2_const = JointConstraint()
right_joint2_const.joint_name = "yumi_joint_2_r"
right_joint2_const.position = 0
right_joint2_const.tolerance_above = 0
right_joint2_const.tolerance_below = 150
right_joint2_const.weight = 1.0
# 限制3轴转动
right_joint3_const = JointConstraint()
right_joint3_const.joint_name = "yumi_joint_3_r"
right_joint3_const.position = 0
right_joint3_const.tolerance_above = 35
right_joint3_const.tolerance_below = 55
right_joint3_const.weight = 1.0
# 限制4轴转动
right_joint4_const = JointConstraint()
right_joint4_const.joint_name = "yumi_joint_4_r"
right_joint4_const.position = 0
right_joint4_const.tolerance_above = 60
right_joint4_const.tolerance_below = 75
right_joint4_const.weight = 1.0
# 限制5轴转动
right_joint5_const = JointConstraint()
right_joint5_const.joint_name = "yumi_joint_5_r"
right_joint5_const.position = 40
right_joint5_const.tolerance_above = 50
right_joint5_const.tolerance_below = 20
right_joint5_const.weight = 1.0
# 限制6轴转动
right_joint6_const = JointConstraint()
right_joint6_const.joint_name = "yumi_joint_6_r"
right_joint6_const.position = 0
right_joint6_const.tolerance_above = 10
right_joint6_const.tolerance_below = 35
right_joint6_const.weight = 1.0
# 限制7轴转动
right_joint7_const = JointConstraint()
right_joint7_const.joint_name = "yumi_joint_7_r"
right_joint7_const.position = -10
right_joint7_const.tolerance_above = 0
right_joint7_const.tolerance_below = 160
right_joint7_const.weight = 1.0
# 限制末端位移
right_position_const = PositionConstraint()
right_position_const.header = Header()
right_position_const.link_name = "gripper_r_joint_r"
right_position_const.target_point_offset.x = 0.5
right_position_const.target_point_offset.y = -0.5
right_position_const.target_point_offset.z = 1.0
right_position_const.weight = 1.0
# 添加末端姿态约束:
right_orientation_const = OrientationConstraint()
right_orientation_const.header = Header()
right_orientation_const.orientation = pose_goal.orientation
right_orientation_const.link_name = "gripper_r_joint_r"
right_orientation_const.absolute_x_axis_tolerance = 0.50
right_orientation_const.absolute_y_axis_tolerance = 0.25
right_orientation_const.absolute_z_axis_tolerance = 0.50
right_orientation_const.weight = 100
# 施加全约束
consts = Constraints()
consts.joint_constraints = [right_joint_const]
consts.orientation_constraints = [right_orientation_const]
# consts.position_constraints = [right_position_const]
right_arm.set_path_constraints(consts)
# 设置动作对象目标位置姿态
pose_goal.orientation.x = Right_Qux
pose_goal.orientation.y = Right_Quy
pose_goal.orientation.z = Right_Quz
pose_goal.orientation.w = Right_Quw
pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
right_arm.set_pose_target(pose_goal)
print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = 0.1644
# pose_goal.orientation.y = 0.3111
# pose_goal.orientation.z = 0.9086
# pose_goal.orientation.w = 0.2247
# pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053
# pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12
# pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# # 设置动作对象目标位置姿态
# pose_goal.orientation.x = pose_goal.orientation.x
# pose_goal.orientation.y = pose_goal.orientation.y
# pose_goal.orientation.z = pose_goal.orientation.z
# pose_goal.orientation.w = pose_goal.orientation.w
# pose_goal.position.x = pose_goal.position.x
# pose_goal.position.y = pose_goal.position.y - 0.01
# pose_goal.position.z = pose_goal.position.z
# right_arm.set_pose_target(pose_goal)
# print "End effector pose %s" % pose_goal
# 规划和输出动作
traj = right_arm.plan()
right_arm.execute(traj, wait=False)
# 动作完成后清除目标信息
right_arm.clear_pose_targets()
# 清除路径约束
right_arm.clear_path_constraints()
# 确保没有剩余未完成动作在执行
right_arm.stop()
def main():
#Initialize moveit_commander
moveit_commander.roscpp_initialize(sys.argv)
#Start a node
rospy.init_node('moveit_node')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right')
#Calibrate the gripper (other commands won't work unless you do this first)
print('Calibrating...')
right_gripper.calibrate()
rospy.sleep(2.0)
#Initialize both arms
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# left_arm = moveit_commander.MoveGroupCommander('left_arm')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# left_arm.set_planner_id('RRTConnectkConfigDefault')
# left_arm.set_planning_time(10)
right_arm.set_planner_id('RRTConnectkConfigDefault')
right_arm.set_planning_time(10)
#First goal pose ------------------------------------------------------
goal_1 = PoseStamped()
goal_1.header.frame_id = "base"
#x, y, and z position
goal_1.pose.position.x = 0.2
goal_1.pose.position.y = -0.5
goal_1.pose.position.z = 0.2
#Orientation as a quaternion
goal_1.pose.orientation.x = 0.0
goal_1.pose.orientation.y = -1.0
goal_1.pose.orientation.z = 0.0
goal_1.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_1)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input(
'Press <Enter> to move the right arm to goal pose 1 (path constraints are never enforced during this motion): '
)
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
#Second goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_2 = PoseStamped()
goal_2.header.frame_id = "base"
#x, y, and z position
goal_2.pose.position.x = 0.6
goal_2.pose.position.y = -0.3
goal_2.pose.position.z = 0.0
#Orientation as a quaternion
goal_2.pose.orientation.x = 0.0
goal_2.pose.orientation.y = -1.0
goal_2.pose.orientation.z = 0.0
goal_2.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_2)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the right arm to goal pose 2: ')
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
#Third goal pose -----------------------------------------------------
rospy.sleep(2.0)
goal_3 = PoseStamped()
goal_3.header.frame_id = "base"
#x, y, and z position
goal_3.pose.position.x = 0.6
goal_3.pose.position.y = -0.1
goal_3.pose.position.z = 0.1
#Orientation as a quaternion
goal_3.pose.orientation.x = 0.0
goal_3.pose.orientation.y = -1.0
goal_3.pose.orientation.z = 0.0
goal_3.pose.orientation.w = 0.0
#Set the goal state to the pose you just defined
right_arm.set_pose_target(goal_3)
#Set the start state for the right arm
right_arm.set_start_state_to_current_state()
# #Create a path constraint for the arm
# #UNCOMMENT TO ENABLE ORIENTATION CONSTRAINTS
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper"
orien_const.header.frame_id = "base"
orien_const.orientation.y = -1.0
orien_const.absolute_x_axis_tolerance = 0.1
orien_const.absolute_y_axis_tolerance = 0.1
orien_const.absolute_z_axis_tolerance = 0.1
orien_const.weight = 1.0
consts = Constraints()
consts.orientation_constraints = [orien_const]
right_arm.set_path_constraints(consts)
#Plan a path
right_plan = right_arm.plan()
#Execute the plan
raw_input('Press <Enter> to move the right arm to goal pose 3: ')
right_arm.execute(right_plan)
#Close the right gripper
print('Closing...')
right_gripper.close()
rospy.sleep(1.0)
#Open the right gripper
print('Opening...')
right_gripper.open()
rospy.sleep(1.0)
print('Done!')
