def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('Three_box')
control = arm_base_control()
sce = moveit_python.PlanningSceneInterface('odom')
sce.clear()
rospy.sleep(1)
control.add_three_box_obstacle()
start = time.clock()
startime = datetime.datetime.now()
point_list = []
for i in range(11):
point_list.append((i * 0.2 - 1, 3, 0.1))
control.print_pointlist(point_list)
finish = time.clock()
finshtime = datetime.datetime.now()
print('time:', finish - start, (finshtime - startime).seconds)
def __init__(self, use_moveit=False):
self.use_moveit = use_moveit
self.baxter = URDF.from_parameter_server(key='robot_description')
self.kdl_tree = kdl_tree_from_urdf_model(self.baxter)
self.base_link = self.baxter.get_root()
self.right_limb_interface = baxter_interface.Limb('right')
self.left_limb_interface = baxter_interface.Limb('left')
self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state", GetLinkState)
self.get_model_state = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState)
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
self.set_model_config = rospy.ServiceProxy('/gazebo/set_model_configuration', SetModelConfiguration)
# Listen to collision information
# self.collision_getter = InfoGetter()
# self.collision_topic = "/hug_collision"
# Verify robot is enabled
print("Getting robot state... ")
self._rs = baxter_interface.RobotEnable()
self._init_state = self._rs.state().enabled
if not self._init_state:
print("Enabling robot... ")
self._rs.enable()
if self.use_moveit:
# Moveit group setup
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_python.PlanningSceneInterface(self.robot.get_planning_frame())
# self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "right_arm"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
# ######################## Create Hugging target #############################
# humanoid hugging target # remember to make sure target_line correct + - y
self.target_pos_start = np.asarray([0.5, 0, -0.93]) # robot /base frame, z = -0.93 w.r.t /world frame
self.target_line_start = np.empty([22, 3], float)
for i in range(11):
self.target_line_start[i] = self.target_pos_start + [0, -0.0, 1.8] - (asarray([0, -0.0, 1.8]) - asarray([0, -0.0, 0.3]))/10*i
self.target_line_start[i+11] = self.target_pos_start + [0, -0.5, 1.3] + (asarray([0, 0.5, 1.3]) - asarray([0, -0.5, 1.3]))/10*i
self.target_line = self.target_line_start
# Build line point graph for interaction mesh
# reset right arm
start_point_right = [-0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0]
t01 = threading.Thread(target=resetarm_job, args=(self.right_limb_interface, start_point_right))
t01.start()
# reset left arm
start_point_left = [0.3, 1.0, 0.0, 0.5, 0.0, 0.027, 0.0]
t02 = threading.Thread(target=resetarm_job, args=(self.left_limb_interface, start_point_left))
t02.start()
t02.join()
t01.join()
right_limb_pose, _ = limbPose(self.kdl_tree, self.base_link, self.right_limb_interface, 'right')
left_limb_pose, _ = limbPose(self.kdl_tree, self.base_link, self.left_limb_interface, 'left')
graph_points = np.concatenate((right_limb_pose[5:], left_limb_pose[5:], self.target_line_start), 0)
self.triangulation = Delaunay(graph_points)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('Three_box')
control = arm_base_control()
sce = moveit_python.PlanningSceneInterface('odom')
sce.clear()
rospy.sleep(1)
point_list = []
point_list = control.straight_line_sample((-1, 1), (3, 1))
control.print_pointlist(point_list)
def __init__(self, limb):
self._baxter = URDF.from_parameter_server(key='robot_description')
self._kdl_tree = kdl_tree_from_urdf_model(self._baxter)
self._right_limb_interface = baxter_interface.Limb('right')
self._base_link = self._baxter.get_root()
self._tip_link = limb + '_gripper'
self.solver = KDLKinematics(self._baxter, self._base_link,
self._tip_link)
self.rs = RobotState()
self.rs.joint_state.name = [
'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0',
'right_w1', 'right_w2'
]
self.rs.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
# Moveit group setup
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_python.PlanningSceneInterface(
self.robot.get_planning_frame())
# self.scene = moveit_commander.PlanningSceneInterface()
self.group_name = "right_arm"
self.group = moveit_commander.MoveGroupCommander(self.group_name)
# service
self.set_model_config = rospy.ServiceProxy(
'/gazebo/set_model_configuration', SetModelConfiguration)
self.get_link_state = rospy.ServiceProxy("/gazebo/get_link_state",
GetLinkState)
self.sv_srv = rospy.ServiceProxy('/check_state_validity',
GetStateValidity)
# human target
self.target_pos_start = np.asarray(
[0.5, 0, -0.93]) # robot /base frame, z = -0.93 w.r.t /world frame
self.target_line_start = np.empty([22, 3], float)
for i in range(11):
self.target_line_start[i] = self.target_pos_start + [
0, -0.0, 1.8
] - (np.asarray([0, -0.0, 1.8]) -
np.asarray([0, -0.0, 0.5])) / 10 * i
self.target_line_start[i + 11] = self.target_pos_start + [
0, -0.5, 1.3
] + (np.asarray([0, 0.5, 1.3]) -
np.asarray([0, -0.5, 1.3])) / 10 * i
self.target_line = self.target_line_start
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('dynamic_obstacle_demo')
scene = Scene_obstacle()
control = arm_base_control()
sce = moveit_python.PlanningSceneInterface('odom')
sce.clear()
# scene.scene.remove_world_object()
rospy.sleep(1)
point_list = scene.straight_line_sample((-1,1), (3,1))
point_list_ob = scene.straight_line_sample((-1,1), (3,1), height = 0.05)
scene.print_list_visualize(point_list_ob)
control.print_pointlist(point_list)
def __init__(self):
rospy.init_node('general_printing')
name = ['1', '2']
for na in name:
control = arm_base_control(name=na)
sce = moveit_python.PlanningSceneInterface('odom')
sce.clear()
rospy.sleep(1)
point_list = []
point_list += control.straight_line_sample((0, 1), (1, 1))
# control.print_list_visualize(point_list)
control.print_pointlist(point_list)
def __init__(self):
rospy.init_node('general_printing')
for name in range(1, 8):
control = arm_base_control()
sce = moveit_python.PlanningSceneInterface('odom')
sce.clear()
rospy.sleep(5)
point_list = []
point_list += control.straight_line_sample((0, 1), (1, 1))
point_list += control.straight_line_sample((1, 1), (1, 2))
point_list += control.straight_line_sample((1, 2), (3, 1))
point_list += control.straight_line_sample((3, 1), (1, 0))
point_list += control.straight_line_sample((1, 0), (1, 1))
# control.print_list_visualize(point_list)
control.print_pointlist(point_list, name='f**k' + str(name))
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('general_printing')
control = arm_base_control()
sce = moveit_python.PlanningSceneInterface('odom')
sce.clear()
rospy.sleep(1)
point_list = []
point_list += control.get_circle_point((-1, 1), 1)
point_list += control.straight_line_sample((0, 1), (1, 1))
point_list += control.straight_line_sample((1, 1), (1, 2))
point_list += control.straight_line_sample((1, 2), (3, 1))
point_list += control.straight_line_sample((3, 1), (1, 0))
point_list += control.straight_line_sample((1, 0), (1, 1))
# control.print_list_visualize(point_list)
control.print_pointlist(point_list)
def __init__(self, name=None):
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
# Init moveit group
self.group = moveit_commander.MoveGroupCommander('robot')
self.arm_group = moveit_commander.MoveGroupCommander('arm')
self.base_group = moveit_commander.MoveGroupCommander('base')
self.scene = moveit_commander.PlanningSceneInterface()
self.sce = moveit_python.PlanningSceneInterface('odom')
self.pub_co = rospy.Publisher('collision_object',
CollisionObject,
queue_size=100)
self.msg_print = SetBoolRequest()
self.request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
self.pub_co = rospy.Publisher('collision_object',
CollisionObject,
queue_size=100)
sub_waypoint_status = rospy.Subscriber('execute_trajectory/status',
GoalStatusArray,
self.waypoint_execution_cb)
sub_movegroup_status = rospy.Subscriber('execute_trajectory/status',
GoalStatusArray,
self.move_group_execution_cb)
# sub_movegroup_status = rospy.Subscriber('move_group/status', GoalStatusArray, self.move_group_execution_cb)
rospy.Subscriber("joint_states", JointState, self.further_ob_printing)
#########################################################################
rospy.Subscriber("/joint_states", JointState, self.joint_callback)
rospy.Subscriber("/joint_states", JointState, self.eef_callback)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
self.base_position_x = []
self.base_position_y = []
self.base_position_z = []
self.eef_position_x = []
self.eef_position_y = []
self.eef_position_z = []
self.position_time = []
self.name = name
self.js_0 = []
self.js_1 = []
self.js_2 = []
self.js_time = []
self.startime = datetime.datetime.now()
self.mkdir('experiment_data' + '/' + name)
#############################################################################
msg_print = SetBoolRequest()
msg_print.data = True
self.re_position_x = []
self.re_position_y = []
self.waypoint_execution_status = 0
self.move_group_execution_status = 0
self.further_printing_number = 0
self.pre_further_printing_number = 0
# initial printing number
self._printing_number = 0
self._further_printing_number = 0
self.future_printing_status = False
self.current_printing_pose = None
self.previous_printing_pose = None
self.target_list = None
self.group.allow_looking(1)
self.group.allow_replanning(1)
self.group.set_planning_time(10)
# Initialize time record
self.travel_time = 0
self.planning_time = 0
self.printing_time = 0
# Initialize extruder
extruder_publisher = rospy.Publisher('set_extruder_rate',
Float32,
queue_size=20)
msg_extrude = Float32()
msg_extrude = 5.0
extruder_publisher.publish(msg_extrude)
self.pub_rviz_marker = rospy.Publisher('/visualization_marker',
Marker,
queue_size=100)
self.remove_all_rviz_marker()
self.printing_number_rviz = 0
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('enclosure_demo')
control = arm_base_control()
sce = moveit_python.PlanningSceneInterface('odom')
sce.clear()
rospy.sleep(0.5)
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "odom"
box_pose.pose.position.x = -0.5
box_pose.pose.position.y = 2.5
box_pose.pose.position.z = 0.2
box_one = "box_1"
control.scene.add_box(box_one, box_pose, size=(0.5, 0.5, 0.5))
rospy.sleep(0.5)
# scene.scene.remove_world_object()
rospy.sleep(1)
point_list = []
point_list += control.straight_line_sample((-1, 1), (2, 1))
point_list += control.straight_line_sample((2, 1), (2, 3))
point_list += control.straight_line_sample((1, 3), (0, 3))
point_list += control.straight_line_sample((1, 3), (0, 2))
point_list_0 = control.straight_line_sample((0, 2), (0, 1))
control.group.set_position_target(
[1, 2, 0.1], control.group.get_end_effector_link())
control.group.go(wait=True)
control.print_list_visualize(point_list)
control.print_list_visualize(point_list_0, name='future_ob')
while not rospy.is_shutdown():
# Check for enclosure
rospy.sleep(0.05)
control.base_group.set_position_target(
[0, 0, 0.05], control.base_group.get_end_effector_link())
result = control.base_group.plan()
control.base_group.clear_pose_targets()
if len(result.joint_trajectory.points) == 0:
print('Check enclosure failed')
# Remove future obstacle
current_future_ob_list = sce.getKnownCollisionObjects()
for point_name in current_future_ob_list:
result = re.match('future_ob', point_name)
if result:
sce.removeCollisionObject(point_name)
rospy.sleep(0.05)
random_pose = control.group.get_current_pose(
control.group.get_end_effector_link())
(roll, pitch, yaw) = euler_from_quaternion([
random_pose.pose.orientation.x,
random_pose.pose.orientation.y,
random_pose.pose.orientation.z,
random_pose.pose.orientation.w
])
[random_pose.pose.orientation.x, \
random_pose.pose.orientation.y, \
random_pose.pose.orientation.z, \
random_pose.pose.orientation.w] = quaternion_from_euler(roll, pitch, yaw - pi/2)
(random_pose.pose.position.x, random_pose.pose.position.y,
random_pose.pose.position.z) = (0, 2, 0.1)
control.group.set_pose_target(random_pose)
control.group.go(wait=True)
control.group.clear_pose_targets()
# control.group.set_position_target([0, 2, 0.1], control.group.get_end_effector_link())
# control.group.go(wait = True)
control.print_list_visualize(point_list_0, name='future_ob')
rospy.sleep(0.05)
else:
print('Check enclosure successful')
break
#!/usr/bin/python
import rospy
import moveit_python
from moveit_msgs.msg import CollisionObject
from geometry_msgs.msg import Pose, Point, Quaternion
from shape_msgs.msg import SolidPrimitive
from std_msgs.msg import Header
rospy.init_node("moveit_table_scene")
scene = moveit_python.PlanningSceneInterface("base_link")
#pub = rospy.Publisher("/move_group/monitored_planning_scene", CollisionObject, queue_size=1)
table = CollisionObject()
# Header
h = Header()
h.stamp = rospy.Time.now()
# Shape
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = [0.808, 1.616, 2.424]
# Pose
position = Point(*[1.44, 0.0, 0.03])
orient = Quaternion(*[-0.510232, 0.49503, 0.515101, 0.478832])
# Create Collision Object
scene.addSolidPrimitive("table", box, Pose(*[position, orient]))
def __init__(self, name_space):
moveit_commander.roscpp_initialize(sys.argv)
# current_robot_ns = rospy.get_namespace()
# current_robot_ns=current_robot_ns[1:-1]
current_robot_ns = name_space
self.robot = moveit_commander.RobotCommander(
current_robot_ns + '/robot_description', current_robot_ns)
# Init moveit group
self.group = moveit_commander.MoveGroupCommander(
'robot', current_robot_ns + '/robot_description', current_robot_ns)
self.arm_group = moveit_commander.MoveGroupCommander(
'arm', current_robot_ns + '/robot_description', current_robot_ns)
self.base_group = moveit_commander.MoveGroupCommander(
'base', current_robot_ns + '/robot_description', current_robot_ns)
self.scene = moveit_commander.PlanningSceneInterface()
self.sce = moveit_python.PlanningSceneInterface(
'odom', current_robot_ns)
self.pub_co = rospy.Publisher('/' + current_robot_ns +
'/collision_object',
CollisionObject,
queue_size=100)
self.msg_print = SetBoolRequest()
self.request_fk = rospy.ServiceProxy(
'/' + current_robot_ns + '/compute_fk', GetPositionFK)
self.pub_co = rospy.Publisher('/' + current_robot_ns +
'/collision_object',
CollisionObject,
queue_size=10)
sub_waypoint_status = rospy.Subscriber(
'/' + current_robot_ns + '/execute_trajectory/status',
GoalStatusArray, self.waypoint_execution_cb)
sub_movegroup_status = rospy.Subscriber(
'/' + current_robot_ns + '/move_group/status', GoalStatusArray,
self.move_group_execution_cb)
sub_movegroup_status = rospy.Subscriber(
'/' + current_robot_ns + '/move_group/status', GoalStatusArray,
self.move_group_execution_cb)
rospy.Subscriber('/' + current_robot_ns + "/joint_states", JointState,
self.further_ob_printing)
# Initialize extruder
print_request = rospy.ServiceProxy(
'/' + current_robot_ns + '/set_extruder_printing', SetBool)
self.extruder_publisher = rospy.Publisher('/' + current_robot_ns +
'/set_extruder_rate',
Float32,
queue_size=20)
# switch the extruder ON, control via set_extruder_rate
msg_print = SetBoolRequest()
msg_print.data = True
print_request(msg_print)
self.msg_extrude = Float32()
self.waypoint_execution_status = 0
self.move_group_execution_status = 0
self.further_printing_number = 0
self.pre_further_printing_number = 0
# initial printing number
self._printing_number = 0
self._further_printing_number = 0
self.future_printing_status = False
self.current_printing_pose = None
self.previous_printing_pose = None
self.target_list = None
self.group.allow_looking(1)
self.group.allow_replanning(1)
self.group.set_planning_time(10)
# Initialize extruder
extruder_publisher = rospy.Publisher('set_extruder_rate',
Float32,
queue_size=20)
msg_extrude = Float32()
msg_extrude = 5.0
extruder_publisher.publish(msg_extrude)
self.pub_rviz_marker = rospy.Publisher('/visualization_marker',
Marker,
queue_size=100)
self.remove_all_rviz_marker()
self.printing_number_rviz = 0
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('arm_base_printing')
self.robot = moveit_commander.RobotCommander()
# Initialize moveit scene interface (woeld surrounding the robot)
# self.scene = moveit_commander.PlanningSceneInterface()
self.scene = moveit_python.PlanningSceneInterface('odom')
self.scene.clear()
self.group = moveit_commander.MoveGroupCommander('robot')
self.base_group = moveit_commander.MoveGroupCommander('robot')
self.arm_group = moveit_commander.MoveGroupCommander('arm')
self.msg_print = SetBoolRequest()
self.request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK)
self.pub_co = rospy.Publisher('collision_object',
CollisionObject,
queue_size=10)
sub_waypoint_status = rospy.Subscriber('execute_trajectory/status',
GoalStatusArray,
self.waypoint_execution_cb)
# check_inside = rospy.Subscriber("joint_states", JointState, self.check_inside_callback)
self.waypoint_execution_status = 0
self.group.allow_looking(1)
self.group.allow_replanning(1)
self.group.set_planning_time(10)
# Initialize extruder
extruder_publisher = rospy.Publisher('set_extruder_rate',
Float32,
queue_size=20)
msg_extrude = Float32()
msg_extrude = 5.0
extruder_publisher.publish(msg_extrude)
self.printing_number = 0
self.further_printing_number = 0
# self.add_three_box_obstacle()
point_list = []
# for i in range(11):
# point_list.append((i * 0.2 -1, 3, 0.1))
# print(point_list)
point_list = self.straight_line_sample((-1, 1), (3, 1))
point_list_ob = self.straight_line_sample((-1, 1), (3, 1), height=0.05)
# # print(point_list)
# self.print_list_visualize(point_list_ob)
# self.print_pointlist(point_list)
# point_list = self.get_circle_point((2,2), 1)
# point_list_ob = self.get_circle_point((2,2), 1, height = 0.05)
# self.print_future_visualize(point_list_ob[3:8])
self.print_pointlist(point_list)
def pick_and_place(grasp_pose, object_dims):
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
# interface to the robot
robot = moveit_commander.RobotCommander()
# interface to the world surrounding the robot.
scene = moveit_python.PlanningSceneInterface("base_link")
# interface to a group of joints.
group = moveit_commander.MoveGroupCommander("manipulator")
# publisher to RVIZ for visualization
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory, queue_size=10)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(1)
# name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
# name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## MAIN ##
pregrasp_pose = compute_pregrasp(grasp_pose) # compute pre-grasp
move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher) # move to pregrasp
move_to_pose(grasp_pose, group, robot, display_trajectory_publisher) # move to grasp
""" To Do Close Gripper """
#close_gripper() To Do
# attach object to ur5 in order to have successful motion planning
attach_object(object_dims,pregrasp_pose, grasp_pose, scene, "tool0", "mybox", 0.01)
move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher)
# move ur5 to a neutral position
up_pose = Pose()
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
up_pose.orientation.x = quaternion[0]
up_pose.orientation.y = quaternion[1]
up_pose.orientation.z = quaternion[2]
up_pose.orientation.w = quaternion[3]
up_pose.position.x = 0
up_pose.position.y = .2
up_pose.position.z = 0.8
print "MOVING TO NEUTRAL UP POSITION"
move_to_pose(up_pose, group, robot, display_trajectory_publisher)
# X and Y coordinates of where object should be placed
PLACE_X = 0.5
PLACE_Y = -0.3
print "MOVING TO PRE RELEASE POSE"
# even though we are releasing the object, still use grasp poses
# change the x and y coordinates of the pregrasp pose
pregrasp_pose.position.x = PLACE_X
pregrasp_pose.position.y = PLACE_Y
move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher)
print "MOVING TO RELEASE POSE"
# change the x and y coordinates of the grasp pose
pregrasp_pose.position.z=pregrasp_pose.position.z-0.1
move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher)
""" To Do Open Gripper """
# "remove" the object from the robot
scene.removeAttachedObject("mybox")
#open_gripper() To Do
print "MOVING TO NEUTRAL UP POSITION"
# move back up to a neutral position awaiting next command
move_to_pose(up_pose, group, robot, display_trajectory_publisher)
print "SUCCESS"
print "============ STOPPING"
moveit_commander.roscpp_shutdown()
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from math import pi
from math import cos, sin
from moveit_msgs.msg import CollisionObject
from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle
import geometry_msgs.msg
from geometry_msgs.msg import *
import moveit_python
# There two obstacle move between (-1,1) and (3, 1)
if __name__ == '__main__':
rospy.init_node('dynamic_obstacle')
# scene = moveit_python.PlanningSceneInterface()
scene = moveit_python.PlanningSceneInterface('odom')
scene.clear()
box1_x = -1
box1_y = 1.5
box1_z = 0.25
box2_x = 3
box2_y = 0.5
box2_z = 0.25
direction_1 = 1
direction_2 = -1
while not rospy.is_shutdown():
#!/usr/bin/env python
# This code is used for checking precision of extrinsic callibration
import moveit_python
import moveit_commander
import rospy
from geometry_msgs.msg import *
if __name__ == "__main__":
rospy.init_node('insert_spike')
robot = moveit_commander.RobotCommander()
scene = moveit_python.PlanningSceneInterface('ra_base')
scene.addCylinder("spike1", 0.306, 0.0063, -0.25, -0.68235, 0.153)
def pick_and_place():
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_wrist_to_pregrasp', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_python.PlanningSceneInterface("base_link")
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("manipulator")
#group = moveit_python.MoveGroupCommander("manipulator")
## 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,
queue_size=10)
## 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 "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
#print "============ Robot Groups:"
#print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
#print "============ Printing robot state"
#print robot.get_current_state()
print "============"
## Wait for test_pr2_gripper_grasp_planner_cluster to publish selected grasp
#rospy.Subscriber('selected_grasp', Pose, selected_grasp_callback)
#rospy.spin() # keeps python from exiting until this node is stopped
grasp_pose, object_dimensions = get_selected_grasp(
) #get best pre-grasp pose
#print "object dimensions:"
#print object_dimensions.x, object_dimensions.y, object_dimensions.z
#compute the pregrasp
pregrasp_pose = compute_pregrasp(grasp_pose)
move_to_pose(pregrasp_pose, group, robot,
display_trajectory_publisher) #move to pregrasp
move_to_pose(grasp_pose, group, robot,
display_trajectory_publisher) #move to grasp
""" To Do Close Gripper """
#close_gripper() To Do
#"attach object to ur5 in order to have successful motion planning"
attach_object(object_dimensions, pregrasp_pose, grasp_pose, scene, "tool0",
"mybox", 0.01)
move_to_pose(pregrasp_pose, group, robot,
display_trajectory_publisher) #move back to pregrasp pose
#move ur5 to a neutral position
up_pose = Pose()
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
up_pose.orientation.x = quaternion[0]
up_pose.orientation.y = quaternion[1]
up_pose.orientation.z = quaternion[2]
up_pose.orientation.w = quaternion[3]
up_pose.position.x = 0
up_pose.position.y = .2
up_pose.position.z = 0.8
print "MOVING TO NEUTRAL UP POSITION"
move_to_pose(up_pose, group, robot, display_trajectory_publisher)
# X and Y coordinates of where object should be placed
PLACE_X = 0.5
PLACE_Y = -0.3
print "MOVING TO PRE RELEASE POSE"
# even though we are releasing the object, still use grasp poses
#change the x and y coordinates of the pregrasp pose
pregrasp_pose.position.x = PLACE_X
pregrasp_pose.position.y = PLACE_Y
move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher)
print "MOVING TO RELEASE POSE"
#change the x and y coordinates of the grasp pose
pregrasp_pose.position.z = pregrasp_pose.position.z - 0.1
move_to_pose(pregrasp_pose, group, robot, display_trajectory_publisher)
# "remove" the object from the robot
scene.removeAttachedObject("mybox")
""" To Do Open Gripper """
#open_gripper() To Do
print "MOVING TO NEUTRAL UP POSITION"
#move back up to a neutral position awaiting next command
move_to_pose(up_pose, group, robot, display_trajectory_publisher)
print "SUCCESS"
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
