def setup_environment(lower_arm, upper_arm, botharms): # add object to Planning Scene rospy.loginfo( "Planning Scene Settings") scene = PlanningSceneInterface() rospy.sleep(2) # Waiting for PlanningSceneInterface scene.remove_world_object() # upper tool box_pose = PoseStamped() box_pose.header.frame_id = upper_arm.get_planning_frame() pos = upper_arm.get_current_pose() rot_o = 90.0/180.0*math.pi rot_a = 0.0/180.0*math.pi rot_t = -90.0/180.0*math.pi q = tf.transformations.quaternion_from_euler(rot_o, rot_a, rot_t, 'szyx') # setting origin of upper tool box_pose.pose.position.x = pos.pose.position.x box_pose.pose.position.y = pos.pose.position.y box_pose.pose.position.z = pos.pose.position.z box_pose.pose.orientation.x = q[0] box_pose.pose.orientation.y = q[1] box_pose.pose.orientation.z = q[2] box_pose.pose.orientation.w = q[3] rospack = rospkg.RosPack() resourcepath = rospack.get_path('khi_robot_sample')+"/config/work/" meshpath = resourcepath + "upper_tool_001.stl" scene.attach_mesh('upper_link_j4', 'upper_tool', box_pose, meshpath, (1,1,1),['upper_link_j3','upper_link_j4']) rospy.sleep(1) rospy.loginfo( "Planning Scene Settings Finish")
####################### CLOSE RIGHT HAND ############################### scene.remove_world_object("right_shoe") rospy.sleep(1) res = req(2,2) # 2 -> Right _hand 2-> Grasp rospy.sleep(1) ####################### ATTACH RIGHT SHOE TO THE ROBOT ############################### pose.pose.position.x = 0.475 pose.pose.position.y = -0.1500 pose.pose.position.z = 1.035 pose.pose.orientation.w = 0.99144 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0.13053 scene.attach_mesh('r_eef', 'right_shoe', pose, roslib.packages.get_pkg_dir('hermes_virtual_robot')+'/common/files/stl/right_shoe.stl',links_off) rospy.sleep(1) ####################### POSICION ARRIBA ZAPATO ############################### pose.pose.position.x = 0.4493 pose.pose.position.y = -0.3628 pose.pose.position.z = 1.1937 pose.pose.orientation.w = 0.68058 pose.pose.orientation.x = -0.21466 pose.pose.orientation.y = 0.65539 pose.pose.orientation.z = 0.24739 pose.header.frame_id = robot.get_planning_frame() pose.header.stamp = rospy.Time.now()
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('MotionSequence') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface() # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying left wam finger poses self.left_wam_finger_1_pub = rospy.Publisher('left_wam_finger_1', PoseStamped) self.left_wam_finger_2_pub = rospy.Publisher('left_wam_finger_2', PoseStamped) self.left_wam_finger_3_pub = rospy.Publisher('left_wam_finger_3', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Define move group comander for each moveit group left_wam = moveit_commander.MoveGroupCommander('left_wam') right_wam = moveit_commander.MoveGroupCommander('right_wam') left_wam_finger_1 = moveit_commander.MoveGroupCommander('left_wam_finger_1') left_wam_finger_2 = moveit_commander.MoveGroupCommander('left_wam_finger_2') left_wam_finger_3 = moveit_commander.MoveGroupCommander('left_wam_finger_3') right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1') right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2') right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3') left_wam.set_planner_id("PRMstarkConfigDefault") right_wam.set_planner_id("PRMstarkConfigDefault") #left_wam_finger_1.set_planner_id("RRTstarkConfigDefault") #left_wam_finger_2.set_planner_id("RRTstarkConfigDefault") #left_wam_finger_3.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_1.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_2.set_planner_id("RRTstarkConfigDefault") #right_wam_finger_3.set_planner_id("RRTstarkConfigDefault") # Get the name of the end-effector link left_end_effector_link = left_wam.get_end_effector_link() right_end_effector_link = right_wam.get_end_effector_link() # Display the name of the end_effector link rospy.loginfo("The end effector link of left wam is: " + str(left_end_effector_link)) rospy.loginfo("The end effector link of right wam is: " + str(right_end_effector_link)) # Allow some leeway in position (meters) and orientation (radians) right_wam.set_goal_position_tolerance(0.01) right_wam.set_goal_orientation_tolerance(0.05) left_wam.set_goal_position_tolerance(0.01) left_wam.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_wam.allow_replanning(True) left_wam.allow_replanning(True) # Allow 5 seconds per planning attempt right_wam.set_planning_time(15) left_wam.set_planning_time(25) # Allow replanning to increase the odds of a solution right_wam.allow_replanning(True) left_wam.allow_replanning(True) # Set the reference frame for wam arms left_wam.set_pose_reference_frame(REFERENCE_FRAME) right_wam.set_pose_reference_frame(REFERENCE_FRAME) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 5 # Give the scene a chance to catch up rospy.sleep(2) # Give each of the scene objects a unique name table_id = 'table' bowl_id = 'bowl' pitcher_id = 'pitcher' spoon_id = 'spoon' # Remove leftover objects from a previous run scene.remove_world_object(table_id) scene.remove_world_object(bowl_id) scene.remove_world_object(pitcher_id) scene.remove_world_object(spoon_id) # Remove leftover objects from a previous run scene.remove_attached_object(right_end_effector_link, 'spoon') #right_wam.set_named_target('right_wam_start') #right_wam.go() #rospy.sleep(2) # Closing the hand first # Closing the hand #right_wam_finger_1.set_named_target("right_wam_finger_1_grasp") #right_wam_finger_2.set_named_target("right_wam_finger_2_grasp") #right_wam_finger_3.set_named_target("right_wam_finger_3_grasp") #right_wam_finger_1.execute(right_wam_finger_1.plan()) #rospy.sleep(5) #right_wam_finger_2.execute(right_wam_finger_2.plan()) #rospy.sleep(5) #right_wam_finger_3.execute(right_wam_finger_3.plan()) #rospy.sleep(5) # Create a pose for the tool relative to the end-effector p = PoseStamped() p.header.frame_id = right_end_effector_link # Place the end of the object within the grasp of the gripper p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.02 # Align the object with the gripper (straight out) p.pose.orientation.x = -0.5 p.pose.orientation.y = 0.5 p.pose.orientation.z = -0.5 p.pose.orientation.w = 0.5 # Attach the tool to the end-effector # Set the height of the table off the ground table_ground = 0.5762625 # Set the length, width and height of the table and boxes table_size = [0.90128, 0.381, 0.0238125] table_pose = PoseStamped() table_pose.header.frame_id = REFERENCE_FRAME table_pose.pose.position.x = 0 table_pose.pose.position.y = 0.847725 table_pose.pose.position.z = table_ground scene.add_box(table_id, table_pose, table_size) # Set the height of the bowl bowl_ground = 0.57816875 bowl_pose = PoseStamped() bowl_pose.header.frame_id = REFERENCE_FRAME bowl_pose.pose.position.x = 0.015 bowl_pose.pose.position.y = 0.847725 bowl_pose.pose.position.z = bowl_ground scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl') # Set the height of the pitcher #pitcher_ground = 0.57816875 #pitcher_pose = PoseStamped() #pitcher_pose.header.frame_id = REFERENCE_FRAME #pitcher_pose.pose.position.x = 0.25 #pitcher_pose.pose.position.y = 0.847725 #pitcher_pose.pose.position.z = pitcher_ground #pitcher_pose.pose.orientation.w = -0.5 #pitcher_pose.pose.orientation.z = 0.707 #scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl') # Make the table red and the boxes orange self.setColor(table_id, 0.8, 0.4, 0, 1.0) self.setColor(bowl_id, 0, 0.4, 0.8, 1.0) #self.setColor(pitcher_id, 0.9, 0.9, 0, 1.0) self.sendColors() rospy.sleep(2) start = input("Start left_wam planning ? ") # Set the support surface name to the table object #left_wam.set_support_surface_name(table_id) #right_wam.set_support_surface_name(table_id) # Set the target pose. target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.40363476287 target_pose.pose.position.y = 0.847725 target_pose.pose.position.z = 0.721472317843 target_pose.pose.orientation.x = 0.707 target_pose.pose.orientation.y = 0 target_pose.pose.orientation.z = -0.707 target_pose.pose.orientation.w = 0 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(target_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) left_wam.shift_pose_target(5, 0, left_end_effector_link) start = input("Left wam starting position ") # Pause for a second # Closing the hand left_wam_finger_1.set_named_target("left_wam_finger_1_grasp") left_wam_finger_2.set_named_target("left_wam_finger_2_grasp") left_wam_finger_3.set_named_target("left_wam_finger_3_grasp") left_wam_finger_1.execute(left_wam_finger_1.plan()) #rospy.sleep(3) left_wam_finger_2.execute(left_wam_finger_2.plan()) #rospy.sleep(3) left_wam_finger_3.execute(left_wam_finger_3.plan()) #rospy.sleep(3) start = input("Left wam hand closing ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose) intermidiate_pose = deepcopy(end_pose) intermidiate_pose.position.z = intermidiate_pose.position.z + 0.05 plan = self.StraightPath(end_pose, intermidiate_pose, left_wam) left_wam.set_start_state_to_current_state() left_wam.execute(plan) start = input("Hold up the Pitcher ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link).pose) intermidiate_pose = deepcopy(end_pose) intermidiate_pose.position.x = intermidiate_pose.position.x - 0.1 plan = self.StraightPath(end_pose, intermidiate_pose, left_wam) left_wam.set_start_state_to_current_state() left_wam.execute(plan) start = input("left_wam into pouring position ") end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link)) back_pose = deepcopy(end_pose) end_pose.pose.orientation.x = 0.97773401145 end_pose.pose.orientation.y = 0 end_pose.pose.orientation.z = -0.209726592658 end_pose.pose.orientation.w = 0 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(end_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) start = input("Pour the water ") # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(back_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) end_pose = deepcopy(left_wam.get_current_pose(left_end_effector_link)) end_pose.pose.position.x = end_pose.pose.position.x + 0.1 # Set the start state to the current state left_wam.set_start_state_to_current_state() # Set the goal pose of the end effector to the stored pose left_wam.set_pose_target(end_pose, left_end_effector_link) left_wam.execute(left_wam.plan()) start = input("Left_wam back to prep position") target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = -0.600350195463908 target_pose.pose.position.y = 0.80576308041 target_pose.pose.position.z = 0.794775212132 target_pose.pose.orientation.x = 3.01203251908e-05 target_pose.pose.orientation.y = 0.705562870053 target_pose.pose.orientation.z = 4.55236739937e-05 target_pose.pose.orientation.w = 0.708647326547 start = input("Start right_wam planning ? ") right_wam.set_start_state_to_current_state() right_wam.set_pose_target(target_pose, right_end_effector_link) right_wam.execute(right_wam.plan()) start = input("right_wam into position. ") start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose.position.x = intermidiate_pose.position.x + 0.6 plan = self.StraightPath(start_pose, intermidiate_pose, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) rospy.sleep(3) # Closing the hand right_wam_finger_1.set_named_target("right_wam_finger_1_grasp") right_wam_finger_2.set_named_target("right_wam_finger_2_grasp") right_wam_finger_3.set_named_target("right_wam_finger_3_grasp") right_wam_finger_1.execute(right_wam_finger_1.plan()) #rospy.sleep(3) right_wam_finger_2.execute(right_wam_finger_2.plan()) #rospy.sleep(3) right_wam_finger_3.execute(right_wam_finger_3.plan()) rospy.sleep(1) scene.attach_mesh(right_end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl') #create a circle path circles = input("How many circles you want the wam to mix ? ") start_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) plan = self.CircularPath(start_pose, circles, right_wam) #execute the circle path right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("Mix the oatmeal ") #put the right_wam back to preparation pose end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose1 = deepcopy(end_pose) intermidiate_pose1.position.z = intermidiate_pose1.position.z + 0.1 plan = self.StraightPath(end_pose, intermidiate_pose1, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("wait for the execution of straight path ") end_pose = deepcopy(right_wam.get_current_pose(right_end_effector_link).pose) intermidiate_pose2 = deepcopy(end_pose) intermidiate_pose2.position.x = intermidiate_pose2.position.x - 0.25 plan = self.StraightPath(end_pose, intermidiate_pose2, right_wam) right_wam.set_start_state_to_current_state() right_wam.execute(plan) pause = input("right_wam back into prep position ") #left_wam.shift_pose_target(5, 0, left_end_effector_link) #left_wam.go() #rospy.sleep(2) #left_wam.shift_pose_target(0, -0.05, left_end_effector_link) #left_wam.go() #rospy.sleep(2) # Initialize the grasp pose to the target pose #grasp_pose = target_pose # Generate a list of grasps #grasps = self.make_grasps(grasp_pose, [pitcher_id]) # Publish the grasp poses so they can be viewed in RViz #for grasp in grasps: # self.left_wam_finger_1_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) # self.left_wam_finger_2_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) # self.left_wam_finger_3_pub.publish(grasp.grasp_pose) # rospy.sleep(0.2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('AttachMesh') # Construct the initial scene object scene = PlanningSceneInterface() # Pause for the scene to get ready rospy.sleep(1) # Initialize the MoveIt! commander for the right wam and fingers right_wam = moveit_commander.MoveGroupCommander('right_wam') right_wam_finger_1 = moveit_commander.MoveGroupCommander('right_wam_finger_1') right_wam_finger_2 = moveit_commander.MoveGroupCommander('right_wam_finger_2') right_wam_finger_3 = moveit_commander.MoveGroupCommander('right_wam_finger_3') # Get the name of the end-effector link end_effector_link = right_wam.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_wam.set_goal_position_tolerance(0.01) right_wam.set_goal_orientation_tolerance(0.05) # Allow replanning to increase the odds of a solution right_wam.allow_replanning(True) # Allow 5 seconds per planning attempt right_wam.set_planning_time(5) # Remove leftover objects from a previous run scene.remove_attached_object(end_effector_link, 'spoon') right_wam.set_named_target('right_wam_start') right_wam.go() rospy.sleep(2) # Closing the hand first # Closing the hand right_wam_finger_1.set_named_target("right_wam_finger_1_grasp") right_wam_finger_2.set_named_target("right_wam_finger_2_grasp") right_wam_finger_3.set_named_target("right_wam_finger_3_grasp") right_wam_finger_1.execute(right_wam_finger_1.plan()) rospy.sleep(5) right_wam_finger_2.execute(right_wam_finger_2.plan()) rospy.sleep(5) right_wam_finger_3.execute(right_wam_finger_3.plan()) rospy.sleep(5) # 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 # Place the end of the object within the grasp of the gripper p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.02 # Align the object with the gripper (straight out) p.pose.orientation.x = -0.5 p.pose.orientation.y = 0.5 p.pose.orientation.z = -0.5 p.pose.orientation.w = 0.5 scene.attach_mesh(end_effector_link, 'spoon', p, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/spoon.stl') moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
def init(): global marker_array_pub, marker_pub, tf_broadcaster, tf_listener global move_group, turntable global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir rospy.init_node('acquisition') camera_mesh = rospy.get_param('~camera_mesh', None) camera_orientation = rospy.get_param('~camera_orientation', None) camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0]) camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1]) min_distance = rospy.get_param('~min_distance', 0.2) max_distance = rospy.get_param('~max_distance', min_distance) max_velocity = rospy.get_param('~max_velocity', 0.1) num_positions = rospy.get_param('~num_positions', 15) num_spins = rospy.get_param('~num_spins', 8) object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2])) photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0]) photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0]) ptpip = rospy.get_param('~ptpip', None) reach = rospy.get_param('~reach', 0.85) simulation = '/gazebo' in get_node_names() test = rospy.get_param('~test', True) turntable_pos = rospy.get_param( '~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05]) turntable_radius = rospy.get_param('~turntable_radius', 0.2) wall_thickness = rospy.get_param('~wall_thickness', 0.04) marker_array_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1, latch=True) marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=1, latch=True) tf_broadcaster = tf.TransformBroadcaster() tf_listener = tf.TransformListener() move_group = MoveGroupCommander('manipulator') move_group.set_max_acceleration_scaling_factor( 1.0 if simulation else max_velocity) move_group.set_max_velocity_scaling_factor( 1.0 if simulation else max_velocity) robot = RobotCommander() scene = PlanningSceneInterface(synchronous=True) try: st_control = load_source( 'st_control', os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts', 'iai_scanning_table', 'st_control.py')) turntable = st_control.ElmoUdp() if turntable.check_device(): turntable.configure() turntable.reset_encoder() turntable.start_controller() turntable.set_speed_deg(30) except Exception as e: print(e) sys.exit('Could not connect to turntable.') if simulation: move_home() elif not test: working_dir = rospy.get_param('~working_dir', None) if working_dir is None: sys.exit('Working directory not specified.') elif not camera.init( os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'out', working_dir, 'images'), ptpip): sys.exit('Could not initialize camera.') # add ground plane ps = PoseStamped() ps.header.frame_id = robot.get_planning_frame() scene.add_plane('ground', ps) # add photobox ps.pose.position.x = photobox_pos[ 0] + photobox_size[0] / 2 + wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_left', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[ 0] - photobox_size[0] / 2 - wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_right', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[ 1] - photobox_size[1] / 2 - wall_thickness / 2 ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_back', ps, (photobox_size[0], wall_thickness, photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] - wall_thickness / 2 scene.add_box('box_ground', ps, (photobox_size[0], photobox_size[1], wall_thickness)) # add turntable turntable_height = turntable_pos[2] - photobox_pos[2] ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = photobox_pos[2] + turntable_height / 2 scene.add_cylinder('turntable', ps, turntable_height, turntable_radius) # add object on turntable ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = turntable_pos[2] + object_size[2] / 2 scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2) # add cable mounts scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount') scene.remove_attached_object('forearm_link', 'forearm_cable_mount') scene.remove_world_object('upper_arm_cable_mount') scene.remove_world_object('forearm_cable_mount') if ptpip is None and not simulation: size = [0.08, 0.08, 0.08] ps.header.frame_id = 'upper_arm_link' ps.pose.position.x = -0.13 ps.pose.position.y = -0.095 ps.pose.position.z = 0.135 scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size) ps.header.frame_id = 'forearm_link' ps.pose.position.x = -0.275 ps.pose.position.y = -0.08 ps.pose.position.z = 0.02 scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size) # add camera eef_link = move_group.get_end_effector_link() ps = PoseStamped() ps.header.frame_id = eef_link scene.remove_attached_object(eef_link, 'camera_0') scene.remove_attached_object(eef_link, 'camera_1') scene.remove_world_object('camera_0') scene.remove_world_object('camera_1') ps.pose.position.y = camera_pos[1] ps.pose.position.z = camera_pos[2] if camera_mesh: ps.pose.position.x = camera_pos[0] quaternion = tf.transformations.quaternion_from_euler( math.radians(camera_orientation[0]), math.radians(camera_orientation[1]), math.radians(camera_orientation[2])) ps.pose.orientation.x = quaternion[0] ps.pose.orientation.y = quaternion[1] ps.pose.orientation.z = quaternion[2] ps.pose.orientation.w = quaternion[3] scene.attach_mesh(eef_link, 'camera_0', ps, os.path.expanduser(camera_mesh), camera_size) if not simulation: scene.attach_mesh(eef_link, 'camera_1', ps, os.path.expanduser(camera_mesh), numpy.array(camera_size) * 1.5) vertices = scene.get_attached_objects( ['camera_0'])['camera_0'].object.meshes[0].vertices camera_size[0] = max(vertice.x for vertice in vertices) - min( vertice.x for vertice in vertices) camera_size[1] = max(vertice.y for vertice in vertices) - min( vertice.y for vertice in vertices) camera_size[2] = max(vertice.z for vertice in vertices) - min( vertice.z for vertice in vertices) else: ps.pose.position.x = camera_pos[0] + camera_size[0] / 2 scene.attach_box(eef_link, 'camera_0', ps, camera_size)