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)
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(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) # 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().safe_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(u'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, topic=None): if topic is None: self.wrapper.set_object_joint_state(object_name, joint_state) else: self.joint_state_publisher[topic].publish( dict_to_joint_states(joint_state)) rospy.sleep(.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, topic=u'/kitchen/cram_joint_states') # # 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=u'{} at {} insteand of {}'.format( joint_name, current, goal)) def set_joint_goal(self, js): """ :rtype js: dict """ self.wrapper.set_joint_goal(js) def check_joint_state(self, expected, decimal=2): current_joint_state = to_joint_state_dict2( self.get_current_joint_state()) self.compare_joint_state(current_joint_state, expected, decimal=decimal) def send_and_check_joint_goal(self, goal, decimal=2): """ :type goal: dict """ self.set_joint_goal(goal) self.send_and_check_goal() 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 = dict_to_joint_states(js) self.set_base.call(goal) 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, tip_normal, root=None, root_normal=None): self.wrapper.align_planes(tip, tip_normal, root, root_normal) def set_rotation_goal(self, goal_pose, tip, root=None): if not root: root = self.default_root self.wrapper.set_rotation_goal(root, tip, goal_pose) def set_translation_goal(self, goal_pose, tip, root=None): if not root: root = self.default_root self.wrapper.set_translation_goal(root, tip, goal_pose) def set_cart_goal(self, goal_pose, tip, root=None): if not root: root = self.default_root self.wrapper.set_cart_goal(root, tip, goal_pose) def set_and_check_cart_goal(self, goal_pose, tip, root=None, expected_error_code=MoveResult.SUCCESS): goal_pose_in_map = transform_pose(u'map', goal_pose) self.set_cart_goal(goal_pose, tip, root) self.loop_once() self.send_and_check_goal(expected_error_code) self.loop_once() self.check_cart_goal(tip, goal_pose_in_map) def check_cart_goal(self, tip, goal_pose): goal_in_base = transform_pose(u'map', goal_pose) current_pose = lookup_pose(u'map', tip) 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 get_as(self): return Blackboard().get(u'giskardpy/command') def send_goal(self, goal=None, execute=True): """ :rtype: MoveResult """ if goal is None: goal = MoveActionGoal() goal.goal = self.wrapper._get_goal() if execute: goal.goal.type = MoveGoal.PLAN_AND_EXECUTE else: goal.goal.type = MoveGoal.PLAN_ONLY 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_and_check_goal(self, expected_error_code=MoveResult.SUCCESS, execute=True): r = self.send_goal(execute=execute) assert r.error_code == expected_error_code, \ u'got: {}, expected: {}'.format(move_result_error_code(r.error_code), move_result_error_code(expected_error_code)) 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_trajectory_msg(self): trajectory = self.get_god_map().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 # # BULLET WORLD ##################################################################################################### # def get_world(self): """ :rtype: PyBulletWorld """ return self.get_god_map().safe_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.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) 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) 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) 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) 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) def add_cylinder(self, name=u'cylinder', size=[1, 1], pose=None): r = self.wrapper.add_cylinder(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) def add_urdf(self, name, urdf, pose, js_topic): r = self.wrapper.add_urdf(name, urdf, pose, 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) 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 enable_self_collision(self): pass def allow_self_collision(self): self.wrapper.allow_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, 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_box(name, size, 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 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 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 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, self.get_god_map().safe_get_data(identifier.distance_thresholds)) collisions = self.get_world().check_collisions(collision_matrix) collisions = self.get_world().transform_contact_info(collisions) collision_list = collisions.external_collision[ self.get_robot().get_movable_parent_joint(link)] for key, self_collisions in collisions.self_collisions.items(): if 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].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].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)
class MotionTaskWithConstraint: """ the class helps to select the necessary constraints, to select the gripper and to check the orientation of the base. """ # list of Gripper _list_grippers = [] # Torso of robot _torso = "" # Reference frame to basefootprint _origin = "" # goal frame _goal = "" 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") def set_gripper_for_real_roboter(self): # 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() def r_action_gripper(self, value=0): goal = Pr2GripperCommandGoal() #GripperCommandGoal() goal.command.position = value goal.command.max_effort = 40 self.rgripper.send_goal_and_wait(goal) def l_action_gripper(self, value=0): goal = Pr2GripperCommandGoal() #GripperCommandGoal() goal.command.position = value goal.command.max_effort = 40 self.lgripper.send_goal_and_wait(goal) def execute_open_translational_motion_on_real_robot(self, goal, body, action, joint, joint_value): self.r_action_gripper(0.08) self.l_action_gripper(0.08) self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body) self._giskard.allow_all_collisions() self._giskard.plan_and_execute() self.r_action_gripper(0.012) self.l_action_gripper(0.012) self._giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action) self._giskard.allow_all_collisions() # self.giskard.avoid_all_collisions() self._giskard.plan_and_execute() self._giskard.set_object_joint_state(object_name='kitchen', joint_states={joint: joint_value}) def execute_close_translational_motion_on_real_robot(self, goal, body, action, joint, joint_value): self.r_action_gripper(0.08) self.l_action_gripper(0.08) self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body) self._giskard.allow_all_collisions() self._giskard.plan_and_execute() self._giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action) self._giskard.allow_all_collisions() # self.giskard.avoid_all_collisions() self._giskard.plan_and_execute() self._giskard.set_object_joint_state(object_name='kitchen', joint_states={joint: joint_value}) def execute_open_circular_motion_on_real_robot(self, goal, body, action, joint, joint_value): self.r_action_gripper(0.08) self.l_action_gripper(0.08) self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body) self._giskard.allow_all_collisions() self._giskard.plan_and_execute() self.r_action_gripper(0.012) self.l_action_gripper(0.012) self._giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action) self._giskard.allow_all_collisions() # self.giskard.avoid_all_collisions() self._giskard.plan_and_execute() self._giskard.set_object_joint_state(object_name='kitchen', joint_states={joint: joint_value}) def execute_close_circular_motion_on_real_robot(self, goal, body, action, joint, joint_value): self.r_action_gripper(0.08) self.l_action_gripper(0.08) self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body) self._giskard.allow_all_collisions() self._giskard.plan_and_execute() self._giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action) self._giskard.allow_all_collisions() # self.giskard.avoid_all_collisions() self._giskard.plan_and_execute() self._giskard.set_object_joint_state(object_name='kitchen', joint_states={joint: joint_value}) def execute_open_rotational_motion_on_real_robot(self, goal, body, action, joint, joint_value): self.r_action_gripper(0.08) self.l_action_gripper(0.08) self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body) self._giskard.allow_all_collisions() self._giskard.plan_and_execute() self.r_action_gripper(0.012) self.l_action_gripper(0.012) self._giskard.set_json_goal("TurnRotaryKnobConstraint", goal_name=goal, body_name=body, action=action) self._giskard.allow_all_collisions() # self.giskard.avoid_all_collisions() self._giskard.plan_and_execute() self._giskard.set_object_joint_state(object_name='kitchen', joint_states={joint: joint_value}) def execute_close_rotational_motion_on_real_robot(self, goal, body, action, joint, joint_value): self.r_action_gripper(0.08) self.l_action_gripper(0.08) self._giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body) self._giskard.allow_all_collisions() self._giskard.plan_and_execute() self._giskard.set_json_goal("TurnRotaryKnobConstraint", goal_name=goal, body_name=body, action=action) self.giskard.allow_all_collisions() # self.giskard.avoid_all_collisions() self.giskard.plan_and_execute() self._giskard.set_object_joint_state(object_name='kitchen', joint_states={joint: joint_value}) def setup_orientation_basis(self, basis_frame="odom_combined", torso="torso_lift_link", list_grippers=["r_gripper_tool_frame", "l_gripper_tool_frame"]): """ This method set the parameter for the orientations verification of the basis :param basis_frame: origin frame to the basis :type: str :param torso: torso of robot :type: str :param list_grippers: List of gripper :type: array of str :return: """ self._list_grippers = list_grippers self._torso = torso self._origin = basis_frame def set_goal(self, goal=""): """ set the goal in relation to which the base is to be oriented :param goal: goal :type: str :return: """ self._goal = goal def update_basis_orientation(self): """ This function update the orientation of the basis and get the next Gripper in short distance from goal :return: """ x, y, r = self.get_current_base_position() rospy.logout("The basis is at position :") rospy.logout(x) rospy.logout(y) rospy.logout(r) # get pose torso pose_torso = tf_wrapper.lookup_pose(self._origin, self._torso) # get pose goal pose_goal = tf_wrapper.lookup_pose(self._origin, self._goal) # get list of pose of gripper list_pose_gripper = [tf_wrapper.lookup_pose(self._origin, p) for p in self._list_grippers] # get angle to G angle = [float(w.get_angle_casadi(self.get_x_y(pose_torso), self.get_x_y(pose_goal), self.get_x_y(p))) for p in list_pose_gripper] # max angle G to goal max_angle = np.amax(angle) rospy.logout("Max angle is :") rospy.logout(max_angle) # update old and new orientation max_angle = r + max_angle rospy.logout("Max angle + r is :") rospy.logout(max_angle) # get selected Gripper gripper = np.where(np.array(angle) == np.amax(angle)) rospy.logout("Farther away Gripper:") rospy.logout(gripper[0][0]) # convert angle in axis q = w.quaternion_from_axis_angle([0, 0, 1], max_angle) # update basis orientation #self.set_cart_goal_for_basis(x, y, 0, q) #@staticmethod def get_x_y(self, pose): return [pose.pose.position.x, pose.pose.position.y, 0] def get_current_base_position(self): """ the method check the current pose of base_footprint to odom and get it :return: x, y, rotation """ basis = tf_wrapper.lookup_pose(self._origin, "base_footprint") t, r = [basis.pose.position.x, basis.pose.position.y, basis.pose.position.z], \ [basis.pose.orientation.x, basis.pose.orientation.y, basis.pose.orientation.z, basis.pose.orientation.w] # self.get_msg_translation_and_rotation(self._basis, "map") return self.base_position(t, r) def base_position(self, translation, rotation): """ the method take the translation and rotation from tf_ros and calculate the rotation in euler :param translation: vector :param rotation: quaternion (from tf_ros) :return: vector[x, y, rotation], current base position """ euler = tf.transformations.euler_from_quaternion(rotation) return translation[0], translation[1], euler[2] def set_cart_goal_for_basis(self, x, y, z, q): """ this function send goal to the basis of robot :param x: position on x axis :type: float :param y: position on y axis :type: float :param z: position on z axis :type: float :param q: quaternion orientation :type: array float :return: """ # take base_footprint poseStamp = lookup_pose(self._origin, "base_footprint") poseStamp.header.frame_id = self._origin # set goal pose poseStamp.pose.position.x = x poseStamp.pose.position.y = y poseStamp.pose.position.z = z poseStamp.pose.orientation.x = q[0] poseStamp.pose.orientation.y = q[1] poseStamp.pose.orientation.z = q[2] poseStamp.pose.orientation.w = q[3] self._giskard.set_cart_goal(self._origin, "base_footprint", poseStamp) self._giskard.avoid_all_collisions() self._giskard.plan_and_execute() def reset_kitchen(self, list_joints): for j in list_joints: self._giskard.set_object_joint_state(object_name='kitchen', joint_states={j: 0.0})
class Gripper: 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 r_action_gripper(self, value=0): goal = GripperCommandGoal() goal.command.position = value goal.command.max_effort = 10 self.rgripper.send_goal_and_wait(goal) def l_action_gripper(self, value=0): goal = GripperCommandGoal() goal.command.position = value goal.command.max_effort = 10 self.lgripper.send_goal_and_wait(goal) def some_mvt(self, goal, body): """ execute of simple movement of position and rotation with the grippers :param goal: goal name :type: str :param body: End Link, l_gripper or r_gripper :type: str :param axis: the axis on which the object must be caught, maybe 'x', 'y' or 'z' :type: str :return: """ # self.giskard.set_json_goal("MoveWithConstraint", goal_name=goal, body_name=body) self.giskard.set_json_goal("MoveToPoseConstraint", goal_name=goal, body_name=body) # movetopose # self.giskard.set_json_goal("CartesianPoseUpdate", root_link="odom_combined", tip_link=body, goal_name=goal) # self.giskard.avoid_all_collisions() self.giskard.allow_all_collisions() self.giskard.plan_and_execute() def open_or_close_with_translation(self, goal, body, action): """ execute simple translation of gripper with something :param goal: goal name (Frame) :type: str :param body: End Link, l_gripper or r_gripper :type: str :param action: percentage of movement :type: float :return: """ # self.giskard.set_json_goal("TranslationalAngularConstraint", goal_name=goal, body_name=body, action=action) self.giskard.set_json_goal("OpenCloseDrawerConstraint", goal_name=goal, body_name=body, action=action) # openclosedrawerconstraint #self.giskard.set_json_goal("OpenCloseDoorConstraint", goal_name=goal, body_name=body, action=action) # openclosedoorconstraint self.giskard.allow_all_collisions() # self.giskard.avoid_all_collisions() self.giskard.plan_and_execute() def do_rotational_mvt(self, goal, body, action): """ Execute rotational movement with the gripper(body) :param goal: goal name :type: str :param body: End Link, l/r Gripper :str: str :param action: percentage of movement :rtype: float :return: """ self.giskard.set_json_goal("RotationalConstraint", goal_name=goal, body_name=body, action=action) # self.giskard.allow_collision(robot_links=(body), link_bs=(goal)) self.giskard.allow_all_collisions() self.giskard.plan_and_execute() def mvt_l_gripper(self, l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint): """ execute mvt of left gripper :param l_finger_joint: value of l_gripper_l_finger_joint :type: float :param r_finger_joint: value of l_gripper_r_finger_joint :type: float :param l_finger_tip_joint: value of l_gripper_l_finger_tip_joint :type: float :param r_finger_tip_joint: value of l_gripper_r_finger_tip_joint :type: float :return: """ rospy.logout('Start mvt L gripper') gripper_request = SetJointStateRequest() gripper_request.state.name = ['l_gripper_l_finger_joint', 'l_gripper_r_finger_joint', 'l_gripper_l_finger_tip_joint', 'l_gripper_r_finger_tip_joint'] gripper_request.state.position = [l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint] gripper_request.state.velocity = [0, 0, 0, 0] gripper_request.state.effort = [0, 0, 0, 0] self.l_gripper_service.call(gripper_request) rospy.logout('End mvt L gripper') def mvt_r_gripper(self, l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint): """ execute mvt of right gripper :param l_finger_joint: value of r_gripper_l_finger_joint :type: float :param r_finger_joint: value of r_gripper_r_finger_joint :type: float :param l_finger_tip_joint: value of r_gripper_l_finger_tip_joint :type: float :param r_finger_tip_joint: value of r_gripper_r_finger_tip_joint :type: float :return: """ rospy.logout('Start mvt R gripper') gripper_request = SetJointStateRequest() gripper_request.state.name = ['r_gripper_l_finger_joint', 'r_gripper_r_finger_joint', 'r_gripper_l_finger_tip_joint', 'r_gripper_r_finger_tip_joint'] gripper_request.state.position = [l_finger_joint, r_finger_joint, l_finger_tip_joint, r_finger_tip_joint] gripper_request.state.velocity = [0, 0, 0, 0] gripper_request.state.effort = [0, 0, 0, 0] self.r_gripper_service.call(gripper_request) rospy.logout('End mvt R gripper') def get_pose(self, source, frame_id): """ the method get exactly the pose of link frame_id to source(map or odom) :param source: odom or map, typ string :param frame_id: string :return: PoseStamped """ tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) # print tf_listener ps = PoseStamped() transform = tf_buffer.lookup_transform(source, frame_id, rospy.Time(0), rospy.Duration(5.0)) # print transform return tf2_geometry_msgs.do_transform_pose(ps, transform) def info_about_joints(self, joint_name): """ This method get info about some joint ( limits and axis) :param joint_name: name of the joint :type: String :return: joint_limit, joint_axis :rtype: {'effort': float, 'lower': float, 'upper': float, 'velocity': float}, array 1*3 """ joint_limits = {} joint_axis = None for j in self.parsed_urdf.joints: if joint_name == j.name: if j.axis != None: joint_axis = j.axis if j.limit != None: joint_limits = {'effort': j.limit.effort, 'lower': j.limit.lower, 'upper': j.limit.upper, 'velocity': j.limit.velocity} return joint_limits, joint_axis def set_kitchen_goal(self, joint_name, joint_value): self.giskard.set_object_joint_state(object_name='kitchen', joint_states={joint_name: joint_value})