本文整理汇总了Python中moveit_commander.PlanningSceneInterface类的典型用法代码示例。如果您正苦于以下问题:Python PlanningSceneInterface类的具体用法?Python PlanningSceneInterface怎么用?Python PlanningSceneInterface使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了PlanningSceneInterface类的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: __init__
def __init__(self):
# Retrieve params:
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'lego_block')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.016)
self._arm_group = rospy.get_param('~arm', 'arm_move_group')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.01)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
# Create planning scene where we will add the objects etc.
self._scene = PlanningSceneInterface()
# Create robot commander: interface to comand the manipulator programmatically (get the planning_frame for exemple
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene (remove the old objects:
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
# TODO get the position of the detected object
self._pose_object_grasp = self._add_object_grasp(self._grasp_object_name)
rospy.sleep(1.0)
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Pick object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
示例2: __init__
def __init__(self, name):
# stuff for grasp planning
self.tf_listener = tf.TransformListener()
self.tf_broadcaster = tf.TransformBroadcaster()
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_objects = RecognizedObjectArray()
#rospy.Subscriber("object_array", RecognizedObjectArray, self.objects_callback)
self.sub = rospy.Subscriber("/recognized_object_array", RecognizedObjectArray, self.objects_callback)
self.grasp_publisher = rospy.Publisher("generated_grasps", PoseArray)
rospy.loginfo("Connecting to pickup AS")
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
#pickup_ac.wait_for_server() # needed?
rospy.loginfo("Connecting to grasp generator AS")
self.grasps_ac = SimpleActionClient('/grasp_generator_server/generate', GenerateBlockGraspsAction)
#grasps_ac.wait_for_server() # needed?
#planning scene for motion planning
self.scene = PlanningSceneInterface()
# blocking action server
self.grasp_obj_as = ActionServer(name, GraspObjectAction, self.goal_callback, self.cancel_callback, False)
self.feedback = GraspObjectFeedback()
self.result = GraspObjectResult()
self.current_goal = None
self.grasp_obj_as.start()
示例3: __init__
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
示例4: __init__
def __init__(self):
self.node_name = "PickAndPlaceServer"
rospy.loginfo("Initalizing PickAndPlaceServer...")
self.sg = SphericalGrasps()
# Get the object size
self.object_height = 0.1
self.object_width = 0.05
self.object_depth = 0.05
self.pick_pose = rospy.get_param('~pickup_marker_pose')
self.place_pose = rospy.get_param('~place_marker_pose')
rospy.loginfo("%s: Waiting for pickup action server...", self.node_name)
self.pickup_ac = SimpleActionClient('/pickup', PickupAction)
connected = self.pickup_ac.wait_for_server(rospy.Duration(3000))
if not connected:
rospy.logerr("%s: Could not connect to pickup action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to pickup action server", self.node_name)
rospy.loginfo("%s: Waiting for place action server...", self.node_name)
self.place_ac = SimpleActionClient('/place', PlaceAction)
if not self.place_ac.wait_for_server(rospy.Duration(3000)):
rospy.logerr("%s: Could not connect to place action server", self.node_name)
exit()
rospy.loginfo("%s: Connected to place action server", self.node_name)
self.scene = PlanningSceneInterface()
rospy.loginfo("Connecting to /get_planning_scene service")
self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
self.scene_srv.wait_for_service()
rospy.loginfo("Connected.")
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
# Get the links of the end effector exclude from collisions
self.links_to_allow_contact = rospy.get_param('~links_to_allow_contact', None)
if self.links_to_allow_contact is None:
rospy.logwarn("Didn't find any links to allow contacts... at param ~links_to_allow_contact")
else:
rospy.loginfo("Found links to allow contacts: " + str(self.links_to_allow_contact))
self.pick_as = SimpleActionServer(self.pick_pose, PickUpPoseAction,
execute_cb=self.pick_cb, auto_start=False)
self.pick_as.start()
self.place_as = SimpleActionServer(self.place_pose, PickUpPoseAction,
execute_cb=self.place_cb, auto_start=False)
self.place_as.start()
示例5: __init__
def __init__(self, name):
# stuff for grasp planning
rospy.loginfo("Getting a TransformListener...")
self.tf_listener = tf.TransformListener()
rospy.loginfo("Getting a TransformBroadcaster...")
self.tf_broadcaster = tf.TransformBroadcaster()
rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_clusters = None
rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)
if DEBUG_MODE:
self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)
rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
self.grasps_ac.wait_for_server()
rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
self.depth_service.wait_for_service()
rospy.loginfo("Getting a PlanningSceneInterface instance...")
self.scene = PlanningSceneInterface()
# blocking action server
rospy.loginfo("Creating Action Server '" + name + "'...")
self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
self.as_feedback = ObjectManipulationFeedback()
self.as_result = ObjectManipulationActionResult()
self.current_goal = None
# Take care of left and right arm grasped stuff
self.right_hand_object = None
self.left_hand_object = None
self.current_side = 'right'
rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
self.grasp_obj_as.start()
示例6: __init__
def __init__(self):
rospy.init_node('moveit_web',disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states',JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text':'ready to plan','ready':True}
示例7: __init__
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('scene_generator')
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
示例8: __init__
def __init__(self):
smach.State.__init__(self,
outcomes=['succeeded','failed'],
input_keys=[],
output_keys=['new_box'])
# initialize tf listener
self.listener = tf.TransformListener()
### Create a handle for the Move Group Commander
self.mgc = MoveGroupCommander("manipulator")
### Create a handle for the Planning Scene Interface
self.psi = PlanningSceneInterface()
#
### initialize service for gripper on universal arm
self.io_srv = rospy.ServiceProxy('set_io', SetIO)
self.eef_step = 0.01
self.jump_threshold = 2
rospy.logwarn("Initializing Grasp")
rospy.sleep(1)
示例9: main
def main():
rospy.init_node('moveit_py_place', anonymous=True)
#right_arm.set_planner_id("KPIECEkConfigDefault");
scene = PlanningSceneInterface()
robot = RobotCommander()
#group = MoveGroupCommander("head")
right_arm = MoveGroupCommander("right_arm")
#right_arm.set_planner_id("KPIECEkConfigDefault");
rospy.logwarn("cleaning world")
GRIPPER_FRAME = 'gripper_bracket_f2'
#scene.remove_world_object("table")
scene.remove_world_object("part")
scene.remove_attached_object(GRIPPER_FRAME, "part")
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.67
p.pose.position.y = -0.
p.pose.position.z = 0.75
scene.add_box("part", p, (0.07, 0.01, 0.2))
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part")
n_attempts += 1
print "Attempts pickup: ", n_attempts
rospy.sleep(0.2)
#robot.right_arm.pick("part")
#right_arm.go()
rospy.sleep(5)
示例10: roscpp_initialize
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint
if __name__ == "__main__":
roscpp_initialize(sys.argv)
rospy.init_node("moveit_py_demo", anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
rospy.sleep(1)
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
rospy.logwarn("cleaning world")
# right_arm.set_named_target("r_start")
# right_arm.go()
# right_gripper.set_named_target("open")
# right_gripper.go()
rospy.sleep(3)
示例11: roscpp_initialize
#!/usr/bin/env python
import sys
import rospy
from moveit_commander import RobotCommander, PlanningSceneInterface, roscpp_initialize, roscpp_shutdown, MoveGroupCommander
from geometry_msgs.msg import PoseStamped
import moveit_msgs.msg
import geometry_msgs.msg
import moveit_commander
if __name__=='__main__':
roscpp_initialize(sys.argv)
rospy.init_node('move_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
group = MoveGroupCommander("manipulator")
# specify the planner
group.set_planner_id("RRTkConfigDefault")
rospy.sleep(3)
## 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)
示例12: __init__
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('SceneSetup')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Set the reference frame for pose targets
reference_frame = 'rack1'
table_id = 'table'
bowl_id = 'bowl'
pitcher_id = 'pitcher'
# 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)
# 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 = [1.0128, 0.481, 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
# 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
bowl_pose.pose.position.y = 0.847725
bowl_pose.pose.position.z = bowl_ground
# 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.4
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
# 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, 0.4, 0.8, 1.0)
self.sendColors()
scene.add_box(table_id, table_pose, table_size)
scene.add_mesh(bowl_id, bowl_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/bowl.stl')
#scene.add_mesh(pitcher_id, pitcher_pose, '/home/yzheng/catkin_ws/src/manipulation_scenarios/ycb_object_models/models/stl/pitcher.stl')
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
示例13: Planner
class Planner(object):
move_group = None
goals = None
jspub = None
namespace = None
# These will eventually go to model objects
robot_data = {
'group_name': 'right_arm_and_torso',
'eef_link': 'r_wrist_joint_link'
}
# Current state of the 'session' (right now, only one)
current_scene = None
status = None
link_poses = None
def __init__(self):
rospy.init_node('moveit_web',disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states',JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text':'ready to plan','ready':True}
def get_scene(self):
return self.current_scene
def set_scene(self, scene):
self.current_scene = scene
psw = PlanningSceneWorld()
for co_json in scene['objects']:
# TODO: Fix orientation by using proper quaternions on the client
pose = self._make_pose(co_json['pose'])
# TODO: Decide what to do with STL vs. Collada. The client has a Collada
# loader but the PlanningSceneInterface can only deal with STL.
# TODO: Proper mapping between filenames and URLs
# filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
co = self.ps.make_mesh(co_json['name'], pose, filename)
psw.collision_objects.append(co)
self.psw_pub.publish(psw)
def get_link_poses(self):
if self.link_poses is None:
self.link_poses = self._move_group.get_link_poses_compressed()
return self.link_poses
# Create link back to socket.io namespace to allow emitting information
def set_socket(self, namespace):
self.namespace = namespace
def emit(self, event, data=None):
if self.namespace:
self.namespace.emit(event, data)
def emit_new_goal(self, pose):
self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])
def set_random_goal(self):
goal_pose = self.move_group.get_random_pose()
# goal_pose = self.move_group.get_random_pose('base_footprint')
self.emit_new_goal(goal_pose)
def _make_pose(self, json_pose):
pose = PoseStamped()
pose.header.frame_id = "odom_combined"
pp = json_pose['position']
pose.pose.position.x = pp['x']
pose.pose.position.y = pp['y']
pose.pose.position.z = pp['z']
# TODO: Orientation is not working. See about
# properly using Quaternions everywhere
pp = json_pose['orientation']
pose.pose.orientation.x = pp['x']
pose.pose.orientation.y = pp['y']
pose.pose.orientation.z = pp['z']
pose.pose.orientation.w = pp['w']
return pose
def plan_to_poses(self, poses):
goal_pose = self._make_pose(poses[0])
self.move_group.set_pose_target(goal_pose)
# self.move_group.set_pose_target(goal_pose,'base_footprint')
#.........这里部分代码省略.........
示例14: __init__
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~arm', 'arm')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.6)
self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.4)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True)
self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
rospy.sleep(1.0)
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
self._pose_coke_can = self._add_coke_can(self._grasp_object_name)
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y + 0.02
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
# Create grasp generator 'generate' action client:
self._grasps_ac = SimpleActionClient('/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown('Grasp generator action client not available!')
return
# Create move group 'pickup' action client:
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
# Create move group 'place' action client:
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(5.0)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
# Pick Coke can object:
while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Pick up successfully')
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name, self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(1.0)
rospy.loginfo('Place successfully')
示例15: PlanningSceneInterface
import copy
import random
from moveit_simple_grasps.msg import GenerateGraspsAction, GenerateGraspsGoal, GenerateGraspsResult
moveit_error_dict = {}
for name in MoveItErrorCodes.__dict__.keys():
if not name[:1] == '_':
code = MoveItErrorCodes.__dict__[name]
moveit_error_dict[code] = name
if __name__=='__main__':
rospy.init_node("part2_attacher")
scene = PlanningSceneInterface()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = '/base_link'
p.pose.position.x = 0.6
p.pose.position.y = 0.0
p.pose.position.z = 0.45
p.pose.orientation.w = 1.0
#scene.add_box("table", p, (0.5, 1.5, 0.9))
p.pose.position.x = 0.45
p.pose.position.y = -0.1
p.pose.position.z = 1.0