def InitializeMoveitCommander():
#First initialize moveit_commander
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
#Instantiate a RobotCommander object. This object is an interface to the robot as a whole.
robot = moveit_commander.RobotCommander()
#Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(1)
print "============ Starting tutorial "
#Instantiate a PlanningSceneInterface object. This object is an interface to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
#Instantiate a MoveGroupCommander object. This object is an interface to one group of joints. In this case the group is the joints in the left arm. This interface can be used to plan and execute motions on the left arm.
global group_both_arms, group_left_arm, group_right_arm
group_both_arms = MoveGroupCommander("both_arms")
group_both_arms.set_goal_position_tolerance(0.01)
group_both_arms.set_goal_orientation_tolerance(0.01)
group_left_arm = MoveGroupCommander("left_arm")
group_left_arm.set_goal_position_tolerance(0.01)
group_left_arm.set_goal_orientation_tolerance(0.01)
group_right_arm = MoveGroupCommander("right_arm")
group_right_arm.set_goal_position_tolerance(0.01)
group_right_arm.set_goal_orientation_tolerance(0.01)
#We create this DisplayTrajectory publisher which is used below to publish trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)
# Obtain current poses of left and right end-effectors: [x,y,z,roll,pitch,yaw].T
P_left_pose = group_left_arm.get_current_pose()
P_right_pose = group_right_arm.get_current_pose()
P_left_euler = group_left_arm.get_current_rpy()
P_right_euler = group_right_arm.get_current_rpy()
global P_left_current, P_right_current
P_left_current = np.array([[P_left_pose.pose.position.x],
[P_left_pose.pose.position.y],
[P_left_pose.pose.position.z],
[P_left_euler[0]], [P_left_euler[1]],
[P_left_euler[2]]])
P_right_current = np.array([[P_right_pose.pose.position.x],
[P_right_pose.pose.position.y],
[P_right_pose.pose.position.z],
[P_right_euler[0]], [P_right_euler[1]],
[P_right_euler[1]]])
print "\nCurrent pose of left and right EE: \n", np.transpose(
P_left_current), "\n", np.transpose(P_right_current)
def __init__(self):
group = MoveGroupCommander("arm")
#group.set_orientation_tolerance([0.3,0.3,0,3])
p = PoseStamped()
p.header.frame_id = "/katana_base_link"
p.pose.position.x = 0.4287
#p.pose.position.y = -0.0847
p.pose.position.y = 0.0
p.pose.position.z = 0.4492
q = quaternion_from_euler(2, 0, 0)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
print "Planning frame: ", group.get_planning_frame()
print "Pose reference frame: ", group.get_pose_reference_frame()
#group.set_pose_reference_frame("katana_base_link")
print "RPy target: 0,0,0"
#group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
#group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
group.set_pose_target(p, "katana_gripper_tool_frame")
group.go()
print "Current rpy: ", group.get_current_rpy(
"katana_gripper_tool_frame")
def __init__(self):
group = MoveGroupCommander("arm")
#group.set_orientation_tolerance([0.3,0.3,0,3])
p = PoseStamped()
p.header.frame_id = "/katana_base_link"
p.pose.position.x = 0.4287
#p.pose.position.y = -0.0847
p.pose.position.y = 0.0
p.pose.position.z = 0.4492
q = quaternion_from_euler(2, 0, 0)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
print "Planning frame: " ,group.get_planning_frame()
print "Pose reference frame: ",group.get_pose_reference_frame()
#group.set_pose_reference_frame("katana_base_link")
print "RPy target: 0,0,0"
#group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
#group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
group.set_pose_target(p, "katana_gripper_tool_frame")
group.go()
print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")
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
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()
from moveit_msgs.msg import RobotTrajectory, Grasp
if __name__ == '__main__':
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = MoveGroupCommander("arm_1")
rospy.sleep(1)
currentrf = robot.get_pose_reference_frame()
robot.set_pose_reference_frame(currentrf)
newpose = robot.get_current_pose().pose
a = robot.get_current_pose().pose
newposerpy = robot.get_current_rpy()
newpose.position.x = newpose.position.x - delta_x
newpose.position.x = newpose.position.y - delta_y
newpose.position.x = newpose.position.z - delta_z
robot.set_pose_target(newpose)
planned = robot.plan()
robot.execute(planned)
rospy.sleep(2)
newpose = robot.get_current_pose().pose
robot.set_start_state_to_current_state()
robot.set_pose_target(a)
b = robot.compute_cartesian_path([newpose, a], 0.01, 10000)
