本文整理汇总了Python中moveit_commander.PlanningSceneInterface.attach_box方法的典型用法代码示例。如果您正苦于以下问题:Python PlanningSceneInterface.attach_box方法的具体用法?Python PlanningSceneInterface.attach_box怎么用?Python PlanningSceneInterface.attach_box使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类moveit_commander.PlanningSceneInterface
的用法示例。
在下文中一共展示了PlanningSceneInterface.attach_box方法的6个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: MoveGroupCommandInterpreter
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import attach_box [as 别名]
#.........这里部分代码省略.........
if cmd == "id":
return (MoveGroupInfoLevel.INFO, g.get_name())
if cmd == "help":
return (MoveGroupInfoLevel.INFO, self.get_help())
if cmd == "vars":
known = g.get_remembered_joint_values()
return (MoveGroupInfoLevel.INFO, "[" + " ".join(known.keys()) + "]")
if cmd == "joints":
joints = g.get_joints()
return (MoveGroupInfoLevel.INFO, "\n" + "\n".join([str(i) + " = " + joints[i] for i in range(len(joints))]) + "\n")
if cmd == "show":
return self.command_show(g)
if cmd == "current":
return self.command_current(g)
if cmd == "ground":
pose = PoseStamped()
pose.pose.position.x = 0
pose.pose.position.y = 0
# offset such that the box is 0.1 mm below ground (to prevent collision with the robot itself)
pose.pose.position.z = -0.0501
pose.pose.orientation.x = 0
pose.pose.orientation.y = 0
pose.pose.orientation.z = 0
pose.pose.orientation.w = 1
pose.header.stamp = rospy.get_rostime()
pose.header.frame_id = self._robot.get_root_link()
self._planning_scene_interface.attach_box(self._robot.get_root_link(), "ground", pose, (3, 3, 0.1))
return (MoveGroupInfoLevel.INFO, "Added ground")
if cmd == "eef":
if len(g.get_end_effector_link()) > 0:
return (MoveGroupInfoLevel.INFO, g.get_end_effector_link())
else:
return (MoveGroupInfoLevel.INFO, "There is no end effector defined")
if cmd == "database":
if self._db_host == None:
return (MoveGroupInfoLevel.INFO, "Not connected to a database")
else:
return (MoveGroupInfoLevel.INFO, "Connected to " + self._db_host + ":" + str(self._db_port))
if cmd == "plan":
if self._last_plan != None:
return (MoveGroupInfoLevel.INFO, str(self._last_plan))
else:
return (MoveGroupInfoLevel.INFO, "No previous plan")
if cmd == "constrain":
g.clear_path_constraints()
return (MoveGroupInfoLevel.SUCCESS, "Cleared path constraints")
if cmd == "tol" or cmd == "tolerance":
return (MoveGroupInfoLevel.INFO, "Joint = %0.6g, Position = %0.6g, Orientation = %0.6g" % g.get_goal_tolerance())
if cmd == "time":
return (MoveGroupInfoLevel.INFO, str(g.get_planning_time()))
if cmd == "execute":
if self._last_plan != None:
if g.execute(self._last_plan):
示例2: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import attach_box [as 别名]
#.........这里部分代码省略.........
obj_id.append(model_name)
ops.pose = self.pwh.pose[self.pwh.name.index(model_name)]
obj_pose.append(deepcopy(ops))
obj_size.append([0.05, 0.05, 0.15])
# obj_id[obj_id.index('custom_1')] = 'target'
obj_size[obj_id.index('custom_2')] = [0.05, 0.05, 0.10]
obj_size[obj_id.index('custom_3')] = [0.05, 0.05, 0.05]
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
self.setColor(obj_id[i], 1, 0.623, 0, 1.0)
### Make the target purple and table green ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_table')], 0.3, 1, 0.3, 1.0)
self.scene.remove_attached_object(GRIPPER_FRAME)
# Send the colors to the planning scene
self.sendColors()
else:
if self.flag == 0:
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link', 'r_wrist_roll_link', 'r_upper_arm_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
self.flag +=1
time.sleep(0.5)
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def bl(self):
blist = ['target','custom_2','custom_3', 'custom_table']
self.blist = []
for name in obj_id:
if name not in blist:
self.blist.append(obj_id.index(name))
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index(name)])
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
return self.blist
def drange(self, start, stop, step):
r = start
while r < stop:
yield r
r += step
示例3: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import attach_box [as 别名]
#.........这里部分代码省略.........
M3[2,3] = target_pose.pose.position.z
M4 = transformations.euler_matrix(0, 1.57, 0)
M4[0,3] = 0.0 # offset about x
M4[1,3] = 0.0 # about y
M4[2,3] = 0.18 # about z
T2 = np.dot(M3, M4)
grasping = deepcopy(target_pose)
grasping.pose.position.x = T2[0,3]
grasping.pose.position.y = T2[1,3]
grasping.pose.position.z = T2[2,3]
quat2 = transformations.quaternion_from_matrix(T2)
grasping.pose.orientation.x = quat2[0]
grasping.pose.orientation.y = quat2[1]
grasping.pose.orientation.z = quat2[2]
grasping.pose.orientation.w = quat2[3]
grasping.header.frame_id = 'gazebo_world'
self.plan_exec(grasping)
#Close the gripper
print "========== Waiting for gazebo to catch up =========="
self.close_gripper()
#################### ATTACH OBJECT ######################
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links)
# counter to let the planning scene know when to remove the object
obj_att = 1
#self.scene.remove_world_object(target_id)
#################### POST-GRASP RETREAT #########################
M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
M5[0,3] = initial_pose.pose.position.x
M5[1,3] = initial_pose.pose.position.y
M5[2,3] = initial_pose.pose.position.z
M6 = transformations.euler_matrix(0, 1.57, 0)
M6[0,3] = 0.0 # offset about x
M6[1,3] = 0.0 # about y
M6[2,3] = 0.3 # about z
T3 = np.dot(M5, M6)
post_grasping = deepcopy(initial_pose)
post_grasping.pose.position.x = T3[0,3]
post_grasping.pose.position.y = T3[1,3]
post_grasping.pose.position.z = T3[2,3]
quat3 = transformations.quaternion_from_matrix(T3)
post_grasping.pose.orientation.x = quat3[0]
post_grasping.pose.orientation.y = quat3[1]
post_grasping.pose.orientation.z = quat3[2]
post_grasping.pose.orientation.w = quat3[3]
post_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(post_grasping)
示例4: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import attach_box [as 别名]
#.........这里部分代码省略.........
obj_size[obj_id.index('custom_table')] = [1.5, 0.8, 0.03]
### REMOVE OBJECT FROM PREVIOUS RUNS ###
for i in range(0, len(obj_id)):
self.scene.remove_world_object(obj_id[i])
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, obj_id[obj_id.index('target')])
next_call = time.time()
while True:
next_call = next_call+1
if self.aro is None:
for i in range(0, len(obj_id)):
### CREATE THE SCENE ###
self.scene.add_box(obj_id[i], obj_pose[i], obj_size[i])
### Make the target purple and obstacles orange ###
self.setColor(obj_id[obj_id.index('target')], 0.6, 0, 1, 1.0)
self.setColor(obj_id[obj_id.index('custom_2')], 1, 0.623, 0, 1.0)
self.setColor(obj_id[obj_id.index('custom_3')], 1, 0.623, 0, 1.0)
self.setColor(obj_id[obj_id.index('custom_1_0')], 1, 0.623, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
###### ATTACH ITEM TO GRIPPER ######
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, obj_id[self.aro], obj_pose[self.aro], obj_size[self.aro], touch_links)
### REMOVE SPECIFIC OBJECT AFTER IT HAS BEEN ATTACHED TO GRIPPER ###
self.scene.remove_world_object(obj_id[self.aro])
time.sleep(next_call - time.time())
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def drange(self, start, stop, step):
r = start
while r < stop:
yield r
r += step
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
示例5: __init__
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import attach_box [as 别名]
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the MoveIt! commander for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander('right_gripper')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Remove leftover objects from a previous run
scene.remove_attached_object(end_effector_link, 'tool')
scene.remove_world_object('table')
scene.remove_world_object('box1')
scene.remove_world_object('box2')
scene.remove_world_object('target')
# Set the height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table
table_size = [0.2, 0.7, 0.01]
# Set the length, width and height of the object to attach
tool_size = [0.3, 0.02, 0.02]
# Create a pose for the tool relative to the end-effector
p = PoseStamped()
p.header.frame_id = end_effector_link
scene.attach_mesh
# Place the end of the object within the grasp of the gripper
p.pose.position.x = tool_size[0] / 2.0 - 0.025
p.pose.position.y = 0.0
p.pose.position.z = 0.0
# Align the object with the gripper (straight out)
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
# Attach the tool to the end-effector
scene.attach_box(end_effector_link, 'tool', p, tool_size)
# Add a floating table top
table_pose = PoseStamped()
table_pose.header.frame_id = 'base_footprint'
table_pose.pose.position.x = 0.35
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', table_pose, table_size)
# Update the current state
right_arm.set_start_state_to_current_state()
# Move the arm with the attached object to the 'straight_forward' position
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Return the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(2)
scene.remove_attached_object(end_effector_link, 'tool')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
示例6: simple_function
# 需要导入模块: from moveit_commander import PlanningSceneInterface [as 别名]
# 或者: from moveit_commander.PlanningSceneInterface import attach_box [as 别名]
def simple_function():
rc = RobotCommander()
mgc = MoveGroupCommander("manipulator")
# print(rc.get_group_names())
# print(rc.get_group('manipulator'))
# exit()
eef_step = 0.01
jump_threshold = 2
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rc.get_current_state()
rospy.logwarn(rc.get_current_state())
sss.wait_for_input()
#rate = rospy.Rate(0.1) # 10hz
rate = rospy.Rate(1) # 10hz
rospy.sleep(1)
theSub = rospy.Subscriber('/attached_collision_object', AttachedCollisionObject, attCollObjCb, queue_size = 1)
while not rospy.is_shutdown():
approach_pose = PoseStamped()
approach_pose.header.frame_id = "table"
approach_pose.header.stamp = rospy.Time(0)
approach_pose.pose.position.x = 0.146
approach_pose.pose.position.y = 0.622
approach_pose.pose.position.z = 0.01
quat = tf.transformations.quaternion_from_euler(0, 0, 1.57/2)
approach_pose.pose.orientation.x = quat[0]
approach_pose.pose.orientation.y = quat[1]
approach_pose.pose.orientation.z = quat[2]
approach_pose.pose.orientation.w = quat[3]
# mgc.set_start_state_to_current_state()
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed before adding collision objects")
# else:
# rospy.logwarn("Planning succeeded before adding collision objects")
#
# rospy.logwarn("waiting for input to add dummy box")
# sss.wait_for_input()
#
box_dummy_pose = PoseStamped()
box_dummy_pose.header.frame_id = "table"
box_dummy_pose.pose.position.x = 0.147
box_dummy_pose.pose.position.y = 0.636
box_dummy_pose.pose.position.z = 0
psi.add_box("dummy_box", box_dummy_pose, (0.18,0.09,0.26))# #size x,y,z x is always to the left viewing the robot from the PC
# rospy.logwarn("I have added the box")
# rospy.sleep(1)
# rospy.logwarn("waiting for input to try planning with dummy box")
# sss.wait_for_input()
#
# (traj_approach,frac_approach) = mgc.compute_cartesian_path([approach_pose.pose], eef_step, jump_threshold, True)
# if(frac_approach != 1):
# rospy.logwarn("Planning did not succeed after adding collision objects (dummy box)")
# else:
# rospy.logwarn("Planning succeeded after adding collision objects (dummy box)")
#
rospy.logwarn("waiting for input to add end effector box box")
sss.wait_for_input()
# end effector collision object
link_attached_to_ef = "ee_link"
mb_ef_collisonobj = "metal_bracket"
mb_ef_pose = PoseStamped()
mb_ef_pose.header.frame_id = link_attached_to_ef
mb_ef_pose.pose.position.x = 0.065/2.0
mb_ef_pose.pose.position.y = 0.0
mb_ef_pose.pose.position.z = 0.0
mb_ef_size = (0.065,0.06,0.06)
psi.attach_box(link_attached_to_ef, mb_ef_collisonobj, mb_ef_pose, mb_ef_size, [link_attached_to_ef, "wrist_3_link"])
raw_input("0 hi")
mgc.attach_object("dummy_box", link_attached_to_ef, [link_attached_to_ef, "wrist_3_link"])
rospy.logwarn("I have added the attached box to the end effector")
rospy.sleep(1)
raw_input("1 hi")
rospy.logwarn(rc.get_current_state())
# mgc.attach_object(object_name, link_name, touch_links)
mgc.set_start_state_to_current_state()
rospy.logwarn(rc.get_current_state())
raw_input("2 hi")
rospy.logwarn("waiting for input to try planning with end effector box")
#.........这里部分代码省略.........