def __init__(self): """ this class controls and performs mvt of gripper """ self.giskard = GiskardWrapper() rospy.logout("--> Set kitchen/world in Giskard") rospy.logout("- Set pose kitchen") kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world") kitchen_pose.header.frame_id = "map" # setup kitchen rospy.logout("- Get urdf") self.urdf = rospy.get_param("kitchen_description") self.parsed_urdf = URDF.from_xml_string(self.urdf) self.config_file = None rospy.logout("- clear urdf and add kitchen urdf") self.giskard.clear_world() self.giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states") # setup gripper as service rospy.logout("- Set right and left Gripper service proxy") self.l_gripper_service = rospy.ServiceProxy('/l_gripper_simulator/set_joint_states', SetJointState) self.r_gripper_service = rospy.ServiceProxy('/r_gripper_simulator/set_joint_states', SetJointState) # setup gripper as actionlib # self.rgripper = actionlib.SimpleActionClient('r_gripper/gripper_command', GripperCommandAction) # self.lgripper = actionlib.SimpleActionClient('l_gripper/gripper_command', GripperCommandAction) # self.rgripper.wait_for_server() # self.lgripper.wait_for_server() rospy.logout("--> Gripper are ready for every task.")
def __init__(self, config_file): with open(get_ros_pkg_path(u'giskardpy') + u'/config/' + config_file) as f: config = yaml.load(f) rospy.set_param(u'~', config) rospy.set_param(u'~path_to_data_folder', u'tmp_data/') rospy.set_param(u'~enable_gui', False) self.sub_result = rospy.Subscriber(u'/giskardpy/command/result', MoveActionResult, self.cb, queue_size=100) self.tree = grow_tree() self.loop_once() # rospy.sleep(1) self.wrapper = GiskardWrapper(ns=u'tests') self.results = Queue(100) self.default_root = self.get_robot().get_root() self.map = u'map' self.simple_base_pose_pub = rospy.Publisher(u'/move_base_simple/goal', PoseStamped, queue_size=10) self.set_base = rospy.ServiceProxy(u'/base_simulator/set_joint_states', SetJointState) self.tick_rate = 10 def create_publisher(topic): p = rospy.Publisher(topic, JointState, queue_size=10) rospy.sleep(.2) return p self.joint_state_publisher = KeyDefaultDict(create_publisher)
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, MoveGripperAction, execute_cb=self.execute_cb, auto_start=False) self._giskard_wrapper = GiskardWrapper() self._as.start()
def joint_space_client(): angle = 0.0 giskard = GiskardWrapper() rospy.sleep(0.5) # Creates the SimpleActionClient, passing the type of the action # (MoveAction) to the constructor. # Creates a goal to send to the action server. # Waits until the action server has started up and started # listening for goals. angle += 0.8 rospy.loginfo("%f", angle) gaya_pose = {'r_shoulder_pan_joint': -1.7125, 'r_shoulder_lift_joint': -0.25672, 'r_upper_arm_roll_joint': -1.46335, 'r_elbow_flex_joint': -2.12216, 'r_forearm_roll_joint': 1.76632, 'r_wrist_flex_joint': -0.10001, 'r_wrist_roll_joint': 0.05106, 'l_shoulder_pan_joint': 1.9652, 'l_shoulder_lift_joint': - 0.26499, 'l_upper_arm_roll_joint': 1.3837, 'l_elbow_flex_joint': - 2.1224, 'l_forearm_roll_joint': 16.99, 'l_wrist_flex_joint': - 0.10001, 'l_wrist_roll_joint': 0} giskard.allow_all_collisions() giskard.set_joint_goal(gaya_pose) giskard.plan_and_execute() return giskard.get_result()
def __init__(self, collision_distance=0.05, mode_rotation=None): """ Proxy class working with giskard for manipulating object with a gripper :param collision_distance: distance to keep from objects None or < 0 allow all collisions :type collision_distance:float :param mode_rotation: Rotation of tip depending on robot_pose :type mode_rotation: dict(mode: [x, y, z, w]) """ self.mode_rotation_ = mode_rotation self.collision_distance_ = collision_distance self.giskard_wrapper_ = GiskardWrapper()
def __init__(self, name): self._action_name = name self.dummy_object_ = u'dummy_plan_object' self.gripper_frame_ = u'hand_palm_link' self.root_frame_ = u'odom' self._as = actionlib.SimpleActionServer(self._action_name, MakePlanAction, execute_cb=self.execute_cb, auto_start=False) self._giskard_wrapper = GiskardWrapper() self.mode_rotation_ = self.get_mode_rotation() self._as.start() rospy.loginfo("{} is ready and waiting for orders.".format(self._action_name))
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, PlaceAction, execute_cb=self.execute_cb, auto_start=False) self._giskard_wrapper = GiskardWrapper() self._gripper = Gripper(apply_force_action_server=u'/hsrb/gripper_controller/apply_force', follow_joint_trajectory_server=u'/hsrb/gripper_controller/follow_joint_trajectory') self._manipulator = Manipulator(mode_rotation=self.get_mode_rotation()) self._as.start() if tfwrapper.tfBuffer is None: tfwrapper.init() rospy.loginfo("{} is ready and waiting for orders.".format(self._action_name))
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, TakePoseAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() self._giskard_wrapper = GiskardWrapper() self._robot = hsrb_interface.Robot() self._whole_body = self._robot.get('whole_body') rospy.loginfo("{} is ready and waiting for orders.".format( self._action_name))
def __init__(self, apply_force_action_server, follow_joint_trajectory_server): self._gripper_apply_force_client = actionlib.SimpleActionClient( apply_force_action_server, GripperApplyEffortAction) self._gripper_apply_force_client.wait_for_server() self._gripper_controller = actionlib.SimpleActionClient( follow_joint_trajectory_server, FollowJointTrajectoryAction) self._gripper_controller.wait_for_server() self._obj_in_gripper_pub = rospy.Publisher("object_in_gripper", ObjectInGripper, queue_size=10) self._giskard_wrapper = GiskardWrapper()
def __init__(self): self.stage = 0 self.giskard = GiskardWrapper() self.giskard.allow_all_collisions() self.pose0 = {'torso_lift_joint': 0.20} self.pose3 = { 'r_shoulder_pan_joint': 0.30, 'r_shoulder_lift_joint': 0.0, 'r_upper_arm_roll_joint': -1.95, 'r_elbow_flex_joint': -0.55, # 'r_forearm_roll_joint': 1.76632, # 'r_wrist_flex_joint': -0.10001, 'r_wrist_roll_joint': -1.55, 'l_shoulder_pan_joint': 0.25, 'l_shoulder_lift_joint': 1.05, 'l_upper_arm_roll_joint': 1.75, 'l_elbow_flex_joint': -1.75, # 'l_forearm_roll_joint': 16.99, # 'l_wrist_flex_joint': - 0.10001, 'l_wrist_roll_joint': 0. } # self.default_pose = {'r_shoulder_pan_joint': -1.7125, # 'r_shoulder_lift_joint': -0.25672, # 'r_upper_arm_roll_joint': -1.46335, # 'r_elbow_flex_joint': -2.12216, # 'r_forearm_roll_joint': 1.76632, # 'r_wrist_flex_joint': -0.10001, # 'r_wrist_roll_joint': 0.05106, # # 'l_shoulder_pan_joint': 1.9652, # 'l_shoulder_lift_joint': - 0.26499, # 'l_upper_arm_roll_joint': 1.3837, # 'l_elbow_flex_joint': - 2.1224, # 'l_forearm_roll_joint': 16.99, # 'l_wrist_flex_joint': - 0.10001, # 'l_wrist_roll_joint': 0} self.vel_pub = rospy.Publisher('/base_controller/command', Twist, queue_size=1) # self.DemoCommandsSub = rospy.Subscriber("/DemoCommands", # String, # self.DemoCommandsCallback, # queue_size = 1) self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) # print "start waiting" # rospy.wait_for_service('SimulationCommands') # print "ready" self.run_demo()
def __init__(self): description = get_param('robot_description') self.giskard_wrapper = GiskardWrapper() self.free_joints = {} self.joint_list = [ ] # for maintaining the original order of the joints self.dependent_joints = get_param("dependent_joints", {}) self.use_mimic = get_param('~use_mimic_tags', True) self.use_small = get_param('~use_smallest_joint_limits', True) self.zeros = get_param("zeros") #self.pub_def_positions = get_param("publish_default_positions", True) #self.pub_def_vels = get_param("publish_default_velocities", False) #self.pub_def_efforts = get_param("publish_default_efforts", False) msg = rospy.wait_for_message(u'/whole_body_controller/state', JointTrajectoryControllerState) self.giskard_joints = msg.joint_names robot = minidom.parseString(description) if robot.getElementsByTagName('COLLADA'): self.init_collada(robot) else: self.init_urdf(robot)
def __init__(self, root_tips, suffix=u''): """ :param root_tips: list containing root->tip tuple for each interactive marker. :type root_tips: list :param suffix: the marker will be called 'eef_control{}'.format(suffix) :type suffix: str """ self.enable_self_collision = rospy.get_param(u'~enable_self_collision', True) self.giskard = GiskardWrapper() if len(root_tips) > 0: self.roots, self.tips = zip(*root_tips) else: self.roots = [] self.tips = [] self.suffix = suffix self.markers = {} # marker server self.server = InteractiveMarkerServer(u'eef_control{}'.format( self.suffix)) self.menu_handler = MenuHandler() for root, tip in zip(self.roots, self.tips): int_marker = self.make_6dof_marker( InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip) self.server.insert( int_marker, self.process_feedback(self.server, int_marker.name, root, tip, self.giskard, self.enable_self_collision)) self.menu_handler.apply(self.server, int_marker.name) self.server.applyChanges()
def __init__(self): print("MotionTaskWithConstraint is initialized !") self._giskard = GiskardWrapper() rospy.logout("- Set pose kitchen") kitchen_pose = tf_wrapper.lookup_pose("map", "iai_kitchen/world") kitchen_pose.header.frame_id = "map" # setup kitchen rospy.logout("- Get urdf") self.urdf = rospy.get_param("kitchen_description") self.parsed_urdf = URDF.from_xml_string(self.urdf) self.config_file = None rospy.logout("- clear urdf and add kitchen urdf") self._giskard.clear_world() self._giskard.add_urdf(name="kitchen", urdf=self.urdf, pose=kitchen_pose, js_topic="kitchen/cram_joint_states")
class MoveGripperServer: _feedback = MoveGripperFeedback() _result = MoveGripperResult() _root = u'odom' def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, MoveGripperAction, execute_cb=self.execute_cb, auto_start=False) self._giskard_wrapper = GiskardWrapper() self._as.start() def execute_cb(self, goal): rospy.loginfo("Move gripper: {}".format(goal)) self._result.error_code = self._result.FAILED self._giskard_wrapper.interrupt() self._giskard_wrapper.set_cart_goal(self._root, u'hand_palm_link', goal.goal_pose) self._giskard_wrapper.plan_and_execute(wait=False) result = self._giskard_wrapper.get_result(rospy.Duration(60)) if result and result.SUCCESS in result.error_code: self._result.error_code = self._result.SUCCESS self._as.set_succeeded(self._result)
def set_cart_goal_for_basis(x, y, z): giskard = GiskardWrapper() # take base_footprint poseStamp = lookup_pose("odom_combined", "base_footprint") # set goal pose poseStamp.pose.position.x = x poseStamp.pose.position.y = y poseStamp.pose.position.z = z giskard.set_cart_goal("odom_combined", "base_footprint", poseStamp) giskard.avoid_all_collisions() giskard.plan_and_execute()
def __init__(self): rospy.set_param(u'~enable_gui', False) rospy.set_param(u'~interactive_marker_chains', []) rospy.set_param(u'~map_frame', u'map') rospy.set_param(u'~joint_convergence_threshold', 0.002) rospy.set_param(u'~wiggle_precision_threshold', 7) rospy.set_param(u'~sample_period', 0.1) rospy.set_param(u'~default_joint_vel_limit', 10) rospy.set_param(u'~default_collision_avoidance_distance', 0.05) rospy.set_param(u'~fill_velocity_values', False) rospy.set_param(u'~nWSR', u'None') rospy.set_param(u'~root_link', u'base_footprint') rospy.set_param(u'~enable_collision_marker', True) rospy.set_param(u'~enable_self_collision', False) rospy.set_param(u'~path_to_data_folder', u'../data/pr2/') rospy.set_param(u'~collision_time_threshold', 15) rospy.set_param(u'~max_traj_length', 30) self.sub_result = rospy.Subscriber(u'/qp_controller/command/result', MoveActionResult, self.cb, queue_size=100) self.pm = giskard_pm() self.pm.start_plugins() self.wrapper = GiskardWrapper(ns=u'tests') self.results = Queue(100) self.robot = self.pm._plugins[u'fk'].get_robot() self.controlled_joints = self.pm._plugins[ u'controlled joints'].controlled_joints self.joint_limits = { joint_name: self.robot.get_joint_lower_upper_limit(joint_name) for joint_name in self.controlled_joints if self.robot.is_joint_controllable(joint_name) } self.world = self.pm._plugins[u'bullet'].world # type: PyBulletWorld self.default_root = u'base_link' self.r_tip = u'r_gripper_tool_frame' self.l_tip = u'l_gripper_tool_frame' self.map = u'map' self.simple_base_pose_pub = rospy.Publisher(u'/move_base_simple/goal', PoseStamped, queue_size=10) rospy.sleep(1)
def __init__(self): self.stage = 0 self.giskard = GiskardWrapper() self.giskard.allow_all_collisions() self.pose0 = {'torso_lift_joint': 0.15} self.vel_pub = rospy.Publisher('/base_controller/command', Twist, queue_size=1) # self.DemoCommandsSub = rospy.Subscriber("/DemoCommands", # String, # self.DemoCommandsCallback, # queue_size = 1) self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) # print "start waiting" # rospy.wait_for_service('SimulationCommands') # print "ready" self.run_demo()
def __init__(self): # initialize object print("HSR_MOVE is actived") # Set Mobility checker self.check_mobility = Mobility() self.interface = GiskardWrapper() self.pose_stamped = PoseStamped() self.states_pre_grasp = { "wrist_roll_joint": 0.0, "wrist_flex_joint": 0.0, "arm_roll_joint": 0.0, "arm_flex_joint": 0.0, "arm_lift_joint": 0.0 } self.place_states = { "wrist_roll_joint": 0.0, "wrist_flex_joint": -1.57, "arm_roll_joint": 0.0, "arm_flex_joint": 0.0, "arm_lift_joint": 0.0 } self.primary_states = { "arm_lift_joint": 0.0, "arm_flex_joint": -0.026, "arm_roll_joint": -1.57, "wrist_flex_joint": -1.57, "wrist_roll_joint": 0.0 } self.end_states = { "arm_lift_joint": 0.0, "arm_flex_joint": 0.0, "wrist_flex_joint": -1.0, "arm_roll_joint": 1.57 } self.perceive_states = { "arm_lift_joint": 0.0, "arm_flex_joint": 0.0, "wrist_flex_joint": -1.70, "arm_roll_joint": 1.57 }
def set_cart_goal_test(): giskard = GiskardWrapper() poseStamp = lookup_pose("odom_combined", "iai_kitchen/kitchen_island_left_lower_drawer_handle") h_g = np_to_kdl(np.array([[-1, 0, 0, 0], [0, 0, 1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])) pose = pose_to_kdl(poseStamp.pose) pose = pose * h_g poseStamp.pose = kdl_to_pose(pose) giskard.set_cart_goal("odom_combined", "r_gripper_tool_frame", poseStamp) giskard.allow_all_collisions() giskard.plan_and_execute()
def __init__(self, enabled=True, knowrob=None, avoid_self_collisinon=True, tip='camera_link', root='ur5_shoulder_link'): self.move_time_limit = 25 self.enabled = enabled self.knowrob = knowrob self.giskard = GiskardWrapper() self.tip = tip self.root = root self.trans_p_gain = 0.5 self.rot_p_gain = 0.5 self.trans_max_speed = 0.1 self.rot_max_speed = 0.3 self.self_collision_min_dist = 0.03 self.avoid_self_collision = avoid_self_collisinon # TODO get this from param server of topic self.joint_names = ['ur5_shoulder_pan_joint', 'ur5_shoulder_lift_joint', 'ur5_elbow_joint', 'ur5_wrist_1_joint', 'ur5_wrist_2_joint', 'ur5_wrist_3_joint', ]
def __init__(self, enabled=True, knowrob=None, avoid_self_collisinon=True, tip='camera_link', root='iiwa_link_0'): self.move_time_limit = 25 self.enabled = enabled self.knowrob = knowrob self.giskard = GiskardWrapper() self.tip = tip self.root = root self.trans_p_gain = 0.5 self.rot_p_gain = 0.5 self.trans_max_speed = 0.1 self.rot_max_speed = 0.3 self.self_collision_min_dist = 0.03 self.avoid_self_collision = avoid_self_collisinon # TODO get this from param server of topic self.joint_names = ['iiwa_joint_1', 'iiwa_joint_2', 'iiwa_joint_3', 'iiwa_joint_4', 'iiwa_joint_5', 'iiwa_joint_6', 'iiwa_joint_7']
def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, OpenAction, execute_cb=self.execute_cb, auto_start=False) self._gripper = Gripper( apply_force_action_server=u'/hsrb/gripper_controller/apply_force', follow_joint_trajectory_server= u'/hsrb/gripper_controller/follow_joint_trajectory') self._manipulator = Manipulator(collision_distance=0.01) self._giskard_wrapper = GiskardWrapper() self._as.start() rospy.loginfo("{} is ready and waiting for orders.".format( self._action_name))
class Demo: def __init__(self): self.stage = 0 self.giskard = GiskardWrapper() self.giskard.allow_all_collisions() self.pose0 = {'torso_lift_joint': 0.15} self.vel_pub = rospy.Publisher('/base_controller/command', Twist, queue_size=1) # self.DemoCommandsSub = rospy.Subscriber("/DemoCommands", # String, # self.DemoCommandsCallback, # queue_size = 1) self.tfBuffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tfBuffer) # print "start waiting" # rospy.wait_for_service('SimulationCommands') # print "ready" self.run_demo() def run_demo(self): try: print "stage 0" self.giskard.set_joint_goal(self.pose0) # self.giskard.add_cmd() l_tip = u'r_gripper_tool_frame' self.giskard.plan_and_execute() print "stage 1" rospy.sleep(0.5) self.stage = -1 except KeyboardInterrupt: print('interrupted!')
class GiskardTestWrapper(object): def __init__(self, config_file): with open(get_ros_pkg_path(u'giskardpy') + u'/config/' + config_file) as f: config = yaml.load(f) rospy.set_param('~', config) rospy.set_param('~path_to_data_folder', u'tmp_data/') rospy.set_param('~enable_gui', False) rospy.set_param('~plugins/PlotTrajectory/enabled', True) self.sub_result = rospy.Subscriber('~command/result', MoveActionResult, self.cb, queue_size=100) self.cancel_goal = rospy.Publisher('~command/cancel', GoalID, queue_size=100) self.tree = grow_tree() self.loop_once() # rospy.sleep(1) self.wrapper = GiskardWrapper(node_name=u'tests') self.results = Queue(100) self.default_root = self.get_robot().get_root() self.map = u'map' self.simple_base_pose_pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10) self.set_base = rospy.ServiceProxy('/base_simulator/set_joint_states', SetJointState) self.tick_rate = 10 def create_publisher(topic): p = rospy.Publisher(topic, JointState, queue_size=10) rospy.sleep(.2) return p self.joint_state_publisher = KeyDefaultDict(create_publisher) # rospy.sleep(1) def wait_for_synced(self): sleeper = rospy.Rate(self.tick_rate) self.loop_once() # while self.tree.tip().name != u'has goal': # self.loop_once() # sleeper.sleep() # self.loop_once() # while self.tree.tip().name != u'has goal': # self.loop_once() # sleeper.sleep() def get_robot(self): """ :rtype: Robot """ return self.get_god_map().get_data(robot) def get_god_map(self): """ :rtype: giskardpy.god_map.GodMap """ return Blackboard().god_map def cb(self, msg): """ :type msg: MoveActionResult """ self.results.put(msg.result) def loop_once(self): self.tree.tick() def get_controlled_joint_names(self): """ :rtype: dict """ return self.get_robot().controlled_joints def get_controllable_links(self): return self.get_robot().get_controlled_links() def get_current_joint_state(self): """ :rtype: JointState """ return rospy.wait_for_message('joint_states', JointState) def tear_down(self): rospy.sleep(1) logging.loginfo(u'stopping plugins') def set_object_joint_state(self, object_name, joint_state): self.wrapper.set_object_joint_state(object_name, joint_state) rospy.sleep(0.5) self.wait_for_synced() current_js = self.get_world().get_object(object_name).joint_state for joint_name, state in joint_state.items(): np.testing.assert_almost_equal(current_js[joint_name].position, state, 2) def set_kitchen_js(self, joint_state, object_name=u'kitchen'): self.set_object_joint_state(object_name, joint_state) # # JOINT GOAL STUFF ################################################################################################# # def compare_joint_state(self, current_js, goal_js, decimal=2): """ :type current_js: dict :type goal_js: dict :type decimal: int """ joint_names = set(current_js.keys()).intersection(set(goal_js.keys())) for joint_name in joint_names: goal = goal_js[joint_name] current = current_js[joint_name] if self.get_robot().is_joint_continuous(joint_name): np.testing.assert_almost_equal(shortest_angular_distance( goal, current), 0, decimal=decimal) else: np.testing.assert_almost_equal( current, goal, decimal, err_msg='{} at {} insteand of {}'.format( joint_name, current, goal)) def set_joint_goal(self, js, weight=None): """ :rtype js: dict """ self.wrapper.set_joint_goal(js, weight=weight) def check_joint_state(self, expected, decimal=2): current_joint_state = to_joint_state_position_dict( self.get_current_joint_state()) self.compare_joint_state(current_joint_state, expected, decimal=decimal) def send_and_check_joint_goal(self, goal, weight=None, decimal=2, expected_error_codes=None): """ :type goal: dict """ self.set_joint_goal(goal, weight=weight) self.send_and_check_goal(expected_error_codes=expected_error_codes) if expected_error_codes == [MoveResult.SUCCESS]: self.check_joint_state(goal, decimal=decimal) # # CART GOAL STUFF ################################################################################################## # def teleport_base(self, goal_pose): goal_pose = transform_pose(self.default_root, goal_pose) js = { u'odom_x_joint': goal_pose.pose.position.x, u'odom_y_joint': goal_pose.pose.position.y, u'odom_z_joint': rotation_from_matrix( quaternion_matrix([ goal_pose.pose.orientation.x, goal_pose.pose.orientation.y, goal_pose.pose.orientation.z, goal_pose.pose.orientation.w ]))[0] } goal = SetJointStateRequest() goal.state = position_dict_to_joint_states(js) self.set_base.call(goal) self.loop_once() rospy.sleep(0.5) self.loop_once() def keep_position(self, tip, root=None): if root is None: root = self.default_root goal = PoseStamped() goal.header.frame_id = tip goal.pose.orientation.w = 1 self.set_cart_goal(goal, tip, root) def keep_orientation(self, tip, root=None): goal = PoseStamped() goal.header.frame_id = tip goal.pose.orientation.w = 1 self.set_rotation_goal(goal, tip, root) def align_planes(self, tip_link, tip_normal, root_link=None, root_normal=None, weight=None): self.wrapper.align_planes(tip_link, tip_normal, root_link, root_normal, weight) def set_rotation_goal(self, goal_pose, tip_link, root_link=None, max_velocity=None): if not root_link: root_link = self.default_root self.wrapper.set_rotation_goal(root_link, tip_link, goal_pose, max_velocity=max_velocity) def set_translation_goal(self, goal_pose, tip_link, root_link=None, max_velocity=None): if not root_link: root_link = self.default_root self.wrapper.set_translation_goal(root_link, tip_link, goal_pose, max_velocity=max_velocity) def set_cart_goal(self, goal_pose, tip_link, root_link=None, weight=None, linear_velocity=None, angular_velocity=None): if not root_link: root_link = self.default_root if weight is not None: self.wrapper.set_cart_goal(root_link, tip_link, goal_pose, weight=weight, max_linear_velocity=linear_velocity, max_angular_velocity=angular_velocity) else: self.wrapper.set_cart_goal(root_link, tip_link, goal_pose, max_linear_velocity=linear_velocity, max_angular_velocity=angular_velocity) def set_and_check_cart_goal(self, goal_pose, tip_link, root_link=None, weight=None, linear_velocity=None, angular_velocity=None, expected_error_codes=None): goal_pose_in_map = transform_pose(u'map', goal_pose) self.set_cart_goal(goal_pose, tip_link, root_link, weight, linear_velocity=linear_velocity, angular_velocity=angular_velocity) self.loop_once() self.send_and_check_goal(expected_error_codes) self.loop_once() if expected_error_codes is None: self.check_cart_goal(tip_link, goal_pose_in_map) def check_cart_goal(self, tip_link, goal_pose): goal_in_base = transform_pose(u'map', goal_pose) current_pose = lookup_pose(u'map', tip_link) np.testing.assert_array_almost_equal( msg_to_list(goal_in_base.pose.position), msg_to_list(current_pose.pose.position), decimal=2) try: np.testing.assert_array_almost_equal( msg_to_list(goal_in_base.pose.orientation), msg_to_list(current_pose.pose.orientation), decimal=2) except AssertionError: np.testing.assert_array_almost_equal( msg_to_list(goal_in_base.pose.orientation), -np.array(msg_to_list(current_pose.pose.orientation)), decimal=2) # # GENERAL GOAL STUFF ############################################################################################### # def interrupt(self): self.cancel_goal.publish(GoalID()) def check_reachability(self, expected_error_codes=None): self.send_and_check_goal( expected_error_codes=expected_error_codes, goal_type=MoveGoal.PLAN_AND_CHECK_REACHABILITY) def get_as(self): return Blackboard().get(u'~command') def send_goal(self, goal=None, goal_type=MoveGoal.PLAN_AND_EXECUTE, wait=True): """ :rtype: MoveResult """ if goal is None: goal = MoveActionGoal() goal.goal = self.wrapper._get_goal() goal.goal.type = goal_type i = 0 self.loop_once() t1 = Thread( target=self.get_as()._as.action_server.internal_goal_callback, args=(goal, )) self.loop_once() t1.start() sleeper = rospy.Rate(self.tick_rate) while self.results.empty(): self.loop_once() sleeper.sleep() i += 1 t1.join() self.loop_once() result = self.results.get() return result def send_goal_and_dont_wait(self, goal=None, goal_type=MoveGoal.PLAN_AND_EXECUTE, stop_after=20): if goal is None: goal = MoveActionGoal() goal.goal = self.wrapper._get_goal() goal.goal.type = goal_type i = 0 self.loop_once() t1 = Thread( target=self.get_as()._as.action_server.internal_goal_callback, args=(goal, )) self.loop_once() t1.start() sleeper = rospy.Rate(self.tick_rate) while self.results.empty(): self.loop_once() sleeper.sleep() i += 1 if i > stop_after: self.interrupt() t1.join() self.loop_once() result = self.results.get() return result def send_and_check_goal(self, expected_error_codes=None, goal_type=MoveGoal.PLAN_AND_EXECUTE, goal=None): r = self.send_goal(goal=goal, goal_type=goal_type) for i in range(len(r.error_codes)): error_code = r.error_codes[i] error_message = r.error_messages[i] if expected_error_codes is None: expected_error_code = MoveResult.SUCCESS else: expected_error_code = expected_error_codes[i] assert error_code == expected_error_code, \ u'in goal {}; got: {}, expected: {} | error_massage: {}'.format(i, move_result_error_code(error_code), move_result_error_code( expected_error_code), error_message) return r.trajectory def add_waypoint(self): self.wrapper.add_cmd() def add_json_goal(self, constraint_type, **kwargs): self.wrapper.set_json_goal(constraint_type, **kwargs) def get_result_trajectory_position(self): trajectory = self.get_god_map().unsafe_get_data(identifier.trajectory) trajectory2 = [] for t, p in trajectory._points.items(): trajectory2.append( {joint_name: js.position for joint_name, js in p.items()}) return trajectory2 def get_result_trajectory_velocity(self): trajectory = self.get_god_map().get_data(identifier.trajectory) trajectory2 = [] for t, p in trajectory._points.items(): trajectory2.append( {joint_name: js.velocity for joint_name, js in p.items()}) return trajectory2 def are_joint_limits_violated(self): controllable_joints = self.get_robot().get_movable_joints() trajectory_pos = self.get_result_trajectory_position() trajectory_vel = self.get_result_trajectory_velocity() for joint in controllable_joints: joint_limits = self.get_robot().get_joint_limits(joint) vel_limit = self.get_robot().get_joint_velocity_limit(joint) trajectory_pos_joint = [p[joint] for p in trajectory_pos] trajectory_vel_joint = [p[joint] for p in trajectory_vel] if any( round(p, 7) < joint_limits[0] and round(p, 7) > joint_limits[1] for p in trajectory_pos_joint): return True if any( round(p, 7) < vel_limit and round(p, 7) > vel_limit for p in trajectory_vel_joint): return True return False # # BULLET WORLD ##################################################################################################### # def get_world(self): """ :rtype: PyBulletWorld """ return self.get_god_map().get_data(world) def clear_world(self): assert self.wrapper.clear_world( ).error_codes == UpdateWorldResponse.SUCCESS assert len(self.get_world().get_object_names()) == 0 assert len(self.wrapper.get_object_names().object_names) == 0 # assert len(self.get_robot().get_attached_objects()) == 0 # assert self.get_world().has_object(u'plane') def remove_object(self, name, expected_response=UpdateWorldResponse.SUCCESS): r = self.wrapper.remove_object(name) assert r.error_codes == expected_response, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(expected_response)) assert not self.get_world().has_object(name) assert not name in self.wrapper.get_object_names().object_names def detach_object(self, name, expected_response=UpdateWorldResponse.SUCCESS): if expected_response == UpdateWorldResponse.SUCCESS: p = self.get_robot().get_fk_pose(self.get_robot().get_root(), name) p = transform_pose(u'map', p) assert name in self.wrapper.get_attached_objects().object_names, \ 'there is no attached object named {}'.format(name) r = self.wrapper.detach_object(name) assert r.error_codes == expected_response, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(expected_response)) if expected_response == UpdateWorldResponse.SUCCESS: assert self.get_world().has_object(name) assert not name in self.wrapper.get_attached_objects( ).object_names, 'the object was not detached' compare_poses(self.get_world().get_object(name).base_pose, p.pose, decimal=2) def add_box(self, name=u'box', size=(1, 1, 1), pose=None, expected_response=UpdateWorldResponse.SUCCESS): r = self.wrapper.add_box(name, size, pose=pose) assert r.error_codes == expected_response, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(expected_response)) p = transform_pose(u'map', pose) o_p = self.get_world().get_object(name).base_pose assert self.get_world().has_object(name) compare_poses(p.pose, o_p) assert name in self.wrapper.get_object_names().object_names compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose) def add_sphere(self, name=u'sphere', size=1, pose=None): r = self.wrapper.add_sphere(name=name, size=size, pose=pose) assert r.error_codes == UpdateWorldResponse.SUCCESS, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(UpdateWorldResponse.SUCCESS)) assert self.get_world().has_object(name) assert name in self.wrapper.get_object_names().object_names o_p = self.get_world().get_object(name).base_pose compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose) def add_cylinder(self, name=u'cylinder', size=[1, 1], pose=None): r = self.wrapper.add_cylinder(name=name, height=size[0], radius=size[1], pose=pose) assert r.error_codes == UpdateWorldResponse.SUCCESS, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(UpdateWorldResponse.SUCCESS)) assert self.get_world().has_object(name) assert name in self.wrapper.get_object_names().object_names def add_mesh(self, name=u'cylinder', path=u'', pose=None, expected_error=UpdateWorldResponse.SUCCESS): r = self.wrapper.add_mesh(name=name, mesh=path, pose=pose) assert r.error_codes == expected_error, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(expected_error)) if expected_error == UpdateWorldResponse.SUCCESS: assert self.get_world().has_object(name) assert name in self.wrapper.get_object_names().object_names o_p = self.get_world().get_object(name).base_pose compare_poses(o_p, self.wrapper.get_object_info(name).pose.pose) else: assert not self.get_world().has_object(name) assert name not in self.wrapper.get_object_names().object_names def add_urdf(self, name, urdf, pose, js_topic=u'', set_js_topic=None): r = self.wrapper.add_urdf(name, urdf, pose, js_topic, set_js_topic=set_js_topic) assert r.error_codes == UpdateWorldResponse.SUCCESS, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(UpdateWorldResponse.SUCCESS)) assert self.get_world().has_object(name) assert name in self.wrapper.get_object_names().object_names def add_CollisionShape(self, name, path, pose, js_topic=u'', set_js_topic=None): r = self.wrapper.add_urdf(name, urdf, pose, js_topic, set_js_topic=set_js_topic) assert r.error_codes == UpdateWorldResponse.SUCCESS, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(UpdateWorldResponse.SUCCESS)) assert self.get_world().has_object(name) assert name in self.wrapper.get_object_names().object_names def allow_all_collisions(self): self.wrapper.allow_all_collisions() def avoid_all_collisions(self, distance=0.5): self.wrapper.avoid_all_collisions(distance) self.loop_once() def allow_self_collision(self): self.wrapper.allow_self_collision() def avoid_self_collision(self): self.wrapper.avoid_self_collision() def add_collision_entries(self, collisions_entries): self.wrapper.set_collision_entries(collisions_entries) def allow_collision(self, robot_links, body_b, link_bs): ces = [] ces.append( CollisionEntry(type=CollisionEntry.ALLOW_COLLISION, robot_links=robot_links, body_b=body_b, link_bs=link_bs)) self.add_collision_entries(ces) def avoid_collision(self, robot_links, body_b, link_bs, min_dist): ces = [] ces.append( CollisionEntry(type=CollisionEntry.AVOID_COLLISION, robot_links=robot_links, body_b=body_b, link_bs=link_bs, min_dist=min_dist)) self.add_collision_entries(ces) def attach_box(self, name=u'box', size=None, frame_id=None, position=None, orientation=None, pose=None, expected_response=UpdateWorldResponse.SUCCESS): scm = self.get_robot().get_self_collision_matrix() if pose is None: expected_pose = PoseStamped() expected_pose.header.frame_id = frame_id expected_pose.pose.position = Point(*position) if orientation: expected_pose.pose.orientation = Quaternion(*orientation) else: expected_pose.pose.orientation = Quaternion(0, 0, 0, 1) else: expected_pose = deepcopy(pose) r = self.wrapper.attach_box(name, size, frame_id, position, orientation, pose) assert r.error_codes == expected_response, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(expected_response)) if expected_response == UpdateWorldResponse.SUCCESS: self.wait_for_synced() assert name in self.get_controllable_links() assert not self.get_world().has_object(name) assert not name in self.wrapper.get_object_names().object_names assert name in self.wrapper.get_attached_objects( ).object_names, 'object {} was not attached' assert scm.difference( self.get_robot().get_self_collision_matrix()) == set() assert len(scm) < len(self.get_robot().get_self_collision_matrix()) compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose) self.loop_once() def attach_cylinder(self, name=u'cylinder', height=1, radius=1, frame_id=None, position=None, orientation=None, expected_response=UpdateWorldResponse.SUCCESS): scm = self.get_robot().get_self_collision_matrix() expected_pose = PoseStamped() expected_pose.header.frame_id = frame_id expected_pose.pose.position = Point(*position) if orientation: expected_pose.pose.orientation = Quaternion(*orientation) else: expected_pose.pose.orientation = Quaternion(0, 0, 0, 1) r = self.wrapper.attach_cylinder(name, height, radius, frame_id, position, orientation) assert r.error_codes == expected_response, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(expected_response)) if expected_response == UpdateWorldResponse.SUCCESS: self.wait_for_synced() assert name in self.get_controllable_links() assert not self.get_world().has_object(name) assert not name in self.wrapper.get_object_names().object_names assert name in self.wrapper.get_attached_objects().object_names assert scm.difference( self.get_robot().get_self_collision_matrix()) == set() assert len(scm) < len(self.get_robot().get_self_collision_matrix()) compare_poses(expected_pose.pose, lookup_pose(frame_id, name).pose) self.loop_once() def attach_existing(self, name=u'box', frame_id=None, expected_response=UpdateWorldResponse.SUCCESS): scm = self.get_robot().get_self_collision_matrix() r = self.wrapper.attach_object(name, frame_id) assert r.error_codes == expected_response, \ u'got: {}, expected: {}'.format(update_world_error_code(r.error_codes), update_world_error_code(expected_response)) self.wait_for_synced() assert name in self.get_controllable_links() assert not self.get_world().has_object(name) assert not name in self.wrapper.get_object_names().object_names assert name in self.wrapper.get_attached_objects().object_names assert scm.difference( self.get_robot().get_self_collision_matrix()) == set() assert len(scm) < len(self.get_robot().get_self_collision_matrix()) self.loop_once() def get_external_collisions(self, link, distance_threshold): """ :param distance_threshold: :rtype: list """ collision_goals = [ CollisionEntry(type=CollisionEntry.AVOID_ALL_COLLISIONS, min_dist=distance_threshold) ] collision_matrix = self.get_world( ).collision_goals_to_collision_matrix(collision_goals, defaultdict(lambda: 0.3)) collisions = self.get_world().check_collisions(collision_matrix) controlled_parent_joint = self.get_robot().get_controlled_parent_joint( link) controlled_parent_link = self.get_robot().get_child_link_of_joint( controlled_parent_joint) collision_list = collisions.get_external_collisions( controlled_parent_link) for key, self_collisions in collisions.self_collisions.items(): if controlled_parent_link in key: collision_list.update(self_collisions) return collision_list def check_cpi_geq(self, links, distance_threshold): for link in links: collisions = self.get_external_collisions(link, distance_threshold) assert collisions[0].get_contact_distance() >= distance_threshold, \ u'distance for {}: {} >= {}'.format(link, collisions[0].get_contact_distance(), distance_threshold) def check_cpi_leq(self, links, distance_threshold): for link in links: collisions = self.get_external_collisions(link, distance_threshold) assert collisions[0].get_contact_distance() <= distance_threshold, \ u'distance for {}: {} <= {}'.format(link, collisions[0].get_contact_distance(), distance_threshold) def move_base(self, goal_pose): """ :type goal_pose: PoseStamped """ self.simple_base_pose_pub.publish(goal_pose) rospy.sleep(.07) self.wait_for_synced() current_pose = self.get_robot().get_base_pose() goal_pose = transform_pose(u'map', goal_pose) compare_poses(goal_pose.pose, current_pose.pose, decimal=1) def reset_base(self): p = PoseStamped() p.header.frame_id = self.map p.pose.orientation.w = 1 self.teleport_base(p)
#!/usr/bin/env python import rospy from giskardpy.python_interface import GiskardWrapper from giskardpy import logging if __name__ == '__main__': rospy.init_node('clear_world') giskard = GiskardWrapper() result = giskard.clear_world() if result.error_codes == result.SUCCESS: logging.loginfo('clear world') else: logging.logwarn('failed to clear world {}'.format(result))
#!/usr/bin/env python import rospy import sys from giskardpy.python_interface import GiskardWrapper if __name__ == '__main__': rospy.init_node('add_cylinder') giskard = GiskardWrapper() try: name = rospy.get_param('~name') result = giskard.add_cylinder(name=name, size=rospy.get_param('~size', (1, 1)), frame_id=rospy.get_param('~frame', 'map'), position=rospy.get_param('~position', (0, 0, 0)), orientation=rospy.get_param('~orientation', (0, 0, 0, 1))) if result.error_codes == result.SUCCESS: rospy.loginfo('cylinder \'{}\' added'.format(name)) else: rospy.logwarn('failed to add cylinder \'{}\''.format(name)) except KeyError: rospy.loginfo( 'Example call: rosrun giskardpy add_cylinder.py _name:=cylinder _size:=[1,1] _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]')
#!/usr/bin/env python import rospy import sys from giskardpy.python_interface import GiskardWrapper if __name__ == '__main__': rospy.init_node('remove_object') giskard = GiskardWrapper([]) try: name = rospy.get_param('~name') except KeyError: try: name = sys.argv[1] except IndexError: rospy.loginfo( 'Example call: rosrun giskardpy remove_object.py _name:=muh') sys.exit() result = giskard.remove_object(name) if result.error_codes == result.SUCCESS: rospy.loginfo('{} removed'.format(name)) else: rospy.logwarn('failed to remove {} {}'.format(name, result))
z_joint: rotation_from_matrix( quaternion_matrix([ goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w ]))[0] } giskard.set_joint_goal(js) giskard.plan_and_execute(wait=False) if __name__ == '__main__': try: rospy.init_node(u'move_base_simple_goal') init() x_joint = u'odom_x_joint' y_joint = u'odom_y_joint' z_joint = u'odom_z_joint' root = u'odom_combined' giskard = GiskardWrapper() sub = rospy.Subscriber(u'/move_base_simple/goal', PoseStamped, call_back, queue_size=10) rospy.spin() except rospy.ROSInterruptException: pass
class MoveArm(object): def __init__(self, enabled=True, knowrob=None, avoid_self_collisinon=True, tip='camera_link', root='ur5_shoulder_link'): self.move_time_limit = 25 self.enabled = enabled self.knowrob = knowrob self.giskard = GiskardWrapper() self.tip = tip self.root = root self.trans_p_gain = 0.5 self.rot_p_gain = 0.5 self.trans_max_speed = 0.1 self.rot_max_speed = 0.3 self.self_collision_min_dist = 0.03 self.avoid_self_collision = avoid_self_collisinon # TODO get this from param server of topic self.joint_names = ['ur5_shoulder_pan_joint', 'ur5_shoulder_lift_joint', 'ur5_elbow_joint', 'ur5_wrist_1_joint', 'ur5_wrist_2_joint', 'ur5_wrist_3_joint', ] def set_translation_goal(self, translation): goal_pose = PoseStamped() if isinstance(translation, PointStamped): goal_pose.header = translation.header goal_pose.pose.position = translation.point else: goal_pose = translation self.giskard.set_translation_goal(self.root, self.tip, goal_pose, self.trans_p_gain, self.trans_max_speed) def set_orientation_goal(self, orientation): goal_pose = PoseStamped() if isinstance(orientation, QuaternionStamped): goal_pose.header = orientation.header goal_pose.pose.orientation = orientation.quaternion else: goal_pose = orientation self.giskard.set_rotation_goal(self.root, self.tip, goal_pose, self.rot_p_gain, self.rot_max_speed) def set_default_self_collision_avoidance(self): if not self.avoid_self_collision: self.giskard.allow_self_collision() else: self.giskard.set_self_collision_distance(self.self_collision_min_dist) self.giskard.allow_collision(['ur5_wrist_3_link'], self.giskard.get_robot_name(), ['ur5_forearm_link']) def set_and_send_cartesian_goal(self, goal_pose): self.set_translation_goal(goal_pose) self.set_orientation_goal(goal_pose) self.set_default_self_collision_avoidance() return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS def send_cartesian_goal(self): if self.enabled: self.set_default_self_collision_avoidance() return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS def set_and_send_joint_goal(self, joint_state): if self.enabled: self.giskard.set_joint_goal(joint_state) self.set_default_self_collision_avoidance() return self.giskard.plan_and_execute().error_code == MoveResult.SUCCESS def move_absolute(self, target_pose, retry=True): if self.enabled: self.giskard.set_cart_goal('odom', 'base_footprint', target_pose) return self.giskard.plan_and_execute() # self.goal_pub.publish(target_pose) # goal = MoveBaseGoal() # goal.target_pose = target_pose # if self.knowrob is not None: # self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose)) # while True: # self.client.send_goal(goal) # wait_result = self.client.wait_for_result(rospy.Duration(self.timeout)) # result = self.client.get_result() # state = self.client.get_state() # if wait_result and state == GoalStatus.SUCCEEDED: # break # if retry: # cmd = raw_input('base movement did not finish in time, retry? [y/n]') # retry = cmd == 'y' # if not retry: # print('movement did not finish in time') # if self.knowrob is not None: # self.knowrob.finish_action() # raise TimeoutError() # if self.knowrob is not None: # self.knowrob.finish_action() # return result def move_other_frame(self, target_pose, frame='camera_link', retry=True): if self.enabled: target_pose = self.cam_pose_to_base_pose(target_pose, frame) target_pose = transform_pose('map', target_pose) target_pose.pose.position.z = 0 self.giskard.set_cart_goal('odom', 'base_footprint', target_pose) return self.giskard.plan_and_execute() # self.goal_pub.publish(target_pose) # goal = MoveBaseGoal() # goal.target_pose = target_pose # if self.knowrob is not None: # self.knowrob.start_base_movement(self.knowrob.pose_to_prolog(target_pose)) # while True: # self.client.send_goal(goal) # wait_result = self.client.wait_for_result(rospy.Duration(self.timeout)) # result = self.client.get_result() # state = self.client.get_state() # if wait_result and state == GoalStatus.SUCCEEDED: # break # if retry: # cmd = raw_input('base movement did not finish in time, retry? [y/n]') # retry = cmd == 'y' # if not retry: # print('movement did not finish in time') # if self.knowrob is not None: # self.knowrob.finish_action() # raise TimeoutError() # if self.knowrob is not None: # self.knowrob.finish_action() # return result def cam_pose_to_base_pose(self, pose, frame): """ :type pose: PoseStamped :rtype: PoseStamped """ T_shelf___cam_joint_g = msg_to_kdl(pose) T_map___bfg = T_shelf___cam_joint_g * self.get_frame_in_base_footprint_kdl(frame).Inverse() base_pose = kdl_to_posestamped(T_map___bfg, pose.header.frame_id) return base_pose def get_frame_in_base_footprint_kdl(self, frame): """ :rtype: PyKDL.Frame """ # if self.T_bf___cam_joint is None: T_bf___cam_joint = msg_to_kdl(lookup_transform('base_footprint', frame)) T_bf___cam_joint.p[2] = 0 T_bf___cam_joint.M = PyKDL.Rotation() return T_bf___cam_joint def place_pose_left(self): joint_state = JointState() joint_state.name = self.joint_names joint_state.position = [ -1.57, -1.39, -2.4, 0.46, 1.57, -1.57, ] return self.set_and_send_joint_goal(joint_state) def place_pose_right(self): joint_state = JointState() joint_state.name = self.joint_names joint_state.position = [ -np.pi / 2, -2.44177755311, 2.15026930371, 0.291547812391, np.pi / 2, np.pi / 2, ] return self.set_and_send_joint_goal(joint_state) def floor_detection_pose_right(self): joint_state = JointState() joint_state.name = self.joint_names joint_state.position = [ np.pi, -1.37, 0.51, -0.72, -0.22, 0, ] return self.set_and_send_joint_goal(joint_state) def floor_detection_pose_left(self): joint_state = JointState() joint_state.name = self.joint_names joint_state.position = [ 0.0, -1.768, -0.51, -2.396, 0.243438, -np.pi, ] return self.set_and_send_joint_goal(joint_state) def drive_pose(self): joint_state = JointState() joint_state.name = self.joint_names joint_state.position = [ np.pi * 1.5, -np.pi / 2, -2.3, -np.pi / 2, 0, -np.pi / 2, ] return self.set_and_send_joint_goal(joint_state) def pre_baseboard_pose(self): joint_state = JointState() joint_state.name = self.joint_names joint_state.position = [ 0.0, -1.6460, -2.171, -0.85549, 0.2181, -3.19172, ] return self.set_and_send_joint_goal(joint_state) def see_pose(self): joint_state = JointState() joint_state.name = self.joint_names joint_state.position = [ # -1.57114202181, # -1.44927913347, # -1.25000602404, # -4.01274043718, # -1.56251222292, # 1.62433183193, -np.pi/2, -2.18, 1.47, 1.03, np.pi/2, np.pi/2, ] return self.set_and_send_joint_goal(joint_state)
class PlaceServer: _feedback = PlaceFeedback() _result = PlaceResult() _root = u'odom' def __init__(self, name): self._action_name = name self._as = actionlib.SimpleActionServer(self._action_name, PlaceAction, execute_cb=self.execute_cb, auto_start=False) self._giskard_wrapper = GiskardWrapper() self._gripper = Gripper(apply_force_action_server=u'/hsrb/gripper_controller/apply_force', follow_joint_trajectory_server=u'/hsrb/gripper_controller/follow_joint_trajectory') self._manipulator = Manipulator(mode_rotation=self.get_mode_rotation()) self._as.start() if tfwrapper.tfBuffer is None: tfwrapper.init() rospy.loginfo("{} is ready and waiting for orders.".format(self._action_name)) def get_mode_rotation(self): mode_rotation = {} front_rotation = rospy.get_param(u'/manipulation/base_gripper_rotation/front', default=None) top_rotation = rospy.get_param(u'/manipulation/base_gripper_rotation/top', default=None) if front_rotation: mode_rotation[PlaceGoal.FRONT] = front_rotation if top_rotation: mode_rotation[PlaceGoal.TOP] = top_rotation return mode_rotation def execute_cb(self, goal): # uncomment to disable collision avoidance # self._manipulator.set_collision(None) rospy.loginfo("Placing: {}".format(goal)) # Set initial result value. success = True self._result.error_code = self._result.FAILED if goal.object_frame_id not in self._giskard_wrapper.get_attached_objects().object_names: rospy.logwarn("object not attached to gripper: {}".format(goal.object_frame_id)) # get current robot_pose robot_pose = tfwrapper.lookup_pose('map', 'base_footprint') success &= self._manipulator.move_to_goal(root_link=self._root, tip_link=u'hand_gripper_tool_frame', goal_pose=goal.goal_pose, robot_pose=robot_pose, mode=goal.place_mode, step=0.1) if success: self._gripper.set_gripper_joint_position(1.2) if goal.object_frame_id in self._giskard_wrapper.get_attached_objects().object_names: self._giskard_wrapper.detach_object(goal.object_frame_id) self._gripper.publish_object_in_gripper(goal.object_frame_id, goal.goal_pose, ObjectInGripper.PLACED) collision_whitelist = [] if goal.object_frame_id in self._giskard_wrapper.get_object_names().object_names: collision_whitelist.append(goal.object_frame_id) else: rospy.logwarn("unknown object: {}".format(goal.object_frame_id)) robot_pose.header.stamp = rospy.Time.now() # Might not be needed but is cleaner this way success &= self._manipulator.move_to_goal(root_link=self._root, tip_link=u'base_footprint', goal_pose=robot_pose, collision_whitelist=collision_whitelist) success &= self._manipulator.take_robot_pose(rospy.get_param(u'/manipulation/robot_poses/neutral')) if success: self._result.error_code = self._result.SUCCESS self._as.set_succeeded(self._result)