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)
