def __init__(self, urdf, base_pose=None, controlled_joints=None, path_to_data_folder=u'', *args, **kwargs): """ :param urdf: :type urdf: str :param joints_to_symbols_map: maps urdfs joint names to symbols :type joints_to_symbols_map: dict :param joint_vel_limit: all velocity limits which are undefined or higher than this will be set to this :type joint_vel_limit: Symbol """ self._fk_expressions = {} self._fks = {} self._evaluated_fks = {} self._joint_to_frame = {} self._joint_position_symbols = KeyDefaultDict( lambda x: w.Symbol(x)) # don't iterate over this map!! self._joint_velocity_symbols = KeyDefaultDict( lambda x: 0) # don't iterate over this map!! self._joint_velocity_linear_limit = KeyDefaultDict( lambda x: 10000) # don't overwrite urdf limits by default self._joint_velocity_angular_limit = KeyDefaultDict(lambda x: 100000) self._joint_acc_linear_limit = defaultdict( lambda: 100) # no acceleration limit per default self._joint_acc_angular_limit = defaultdict( lambda: 100) # no acceleration limit per default self._joint_weights = defaultdict(lambda: 0) super(Robot, self).__init__(urdf, base_pose, controlled_joints, path_to_data_folder, *args, **kwargs) self.reinitialize()
def __init__(self, urdf, joint_weights=None, base_pose=None, controlled_joints=None, path_to_data_folder=u'', joint_vel_limit=None, joint_acc_limit=None, calc_self_collision_matrix=True, *args, **kwargs): """ :param urdf: :type urdf: str :param joints_to_symbols_map: maps urdfs joint names to symbols :type joints_to_symbols_map: dict :param joint_vel_limit: all velocity limits which are undefined or higher than this will be set to this :type joint_vel_limit: Symbol """ self._fk_expressions = {} self._fks = {} self._evaluated_fks = {} self._joint_to_frame = {} if joint_weights is None: self._joint_weights = defaultdict(lambda: 0) else: self._joint_weights = joint_weights if joint_vel_limit is None: self._joint_velocity_limit = defaultdict( lambda: 100) # use urdf limits per default else: self._joint_velocity_limit = joint_vel_limit if joint_acc_limit is None: self._joint_acc_limit = defaultdict( lambda: 100) # no acceleration limit per default else: self._joint_acc_limit = joint_acc_limit self._joint_position_symbols = KeyDefaultDict( lambda x: w.Symbol(x)) # don't iterate over this map!! self._joint_velocity_symbols = KeyDefaultDict( lambda x: 0) # don't iterate over this map!! super(Robot, self).__init__(urdf, base_pose, controlled_joints, path_to_data_folder, calc_self_collision_matrix, *args, **kwargs) self.reinitialize() self.update_self_collision_matrix(added_links=set( combinations(self.get_link_names_with_collision(), 2)))
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 process_joint_specific_params(identifier_, default, override, god_map): default_value = god_map.unsafe_get_data(default) d = defaultdict(lambda: default_value) override = god_map.get_data(override) if isinstance(override, dict): d.update(override) god_map.set_data(identifier_, d) return KeyDefaultDict(lambda key: god_map.to_symbol(identifier_ + [key]))
def init_fast_fks(self): def f(key): root, tip = key fk = self.get_fk_expression(root, tip) m = w.speed_up(fk, w.free_symbols(fk)) return m self._fks = KeyDefaultDict(f)
def init_fast_fks(self): def f(key): root, tip = key fk = self.get_fk_expression(root, tip) m = spw.speed_up(fk, fk.free_symbols, backend=self.symengine_backend, opt_level=self.symengine_opt_level) return m self._fks = KeyDefaultDict(f)
def test_god_map_with_world(self): gm = GodMap() w = World() r = WorldObject(pr2_urdf()) w.add_robot(r, PoseStamped(), [], 0, KeyDefaultDict(lambda key: 0), False) gm.safe_set_data([u'world'], w) assert r == gm.safe_get_data([u'world', u'robot'])
def test_god_map_with_world(self): gm = GodMap() w = World() r = WorldObject(pr2_urdf()) w.add_robot(robot=r, base_pose=PoseStamped(), controlled_joints=[], joint_vel_limit=KeyDefaultDict(lambda key: 0), joint_acc_limit=KeyDefaultDict(lambda key: 0), joint_weights=KeyDefaultDict(lambda key: 0), calc_self_collision_matrix=False, ignored_pairs=set(), added_pairs=set(), symengine_backend='llvm', symengine_opt_level=0) gm.safe_set_data([u'world'], w) gm_robot = gm.safe_get_data(identifier.robot) assert 'pr2' == gm_robot.get_name()
def collisions_to_closest_point(self, collisions, min_allowed_distance): """ :param collisions: (robot_link, body_b, link_b) -> ContactInfo :type collisions: dict :param min_allowed_distance: (robot_link, body_b, link_b) -> min allowed distance :type min_allowed_distance: dict :return: robot_link -> ClosestPointInfo of closest thing :rtype: dict """ closest_point = KeyDefaultDict(lambda k: ClosestPointInfo( (10, 0, 0), (0, 0, 0), 100, 0.0, k, '', '', (1, 0, 0), k)) root_T_map = self.robot.root_T_map root = self.robot.get_root() fk = self.robot.get_fk_np for key, contact_info in collisions.items( ): # type: ((str, str, str), ContactInfo) if contact_info is None: continue link1 = key[0] link_T_root = np_to_kdl(fk(link1, root)) a_in_robot_root = link_T_root * root_T_map * PyKDL.Vector( *contact_info.position_on_a) b_in_robot_root = root_T_map * PyKDL.Vector( *contact_info.position_on_b) n_in_robot_root = root_T_map.M * PyKDL.Vector( *contact_info.contact_normal_on_b) try: cpi = ClosestPointInfo(a_in_robot_root, b_in_robot_root, contact_info.contact_distance, min_allowed_distance[key], key[0], key[1], key[2], n_in_robot_root, key) except KeyError: continue if link1 in closest_point: closest_point[link1] = min(closest_point[link1], cpi, key=lambda x: x.contact_distance) else: closest_point[link1] = cpi return closest_point
def __init__(self, to_expr, joint_names, prefix=(), suffix=()): self.joint_map = KeyDefaultDict(lambda joint_name: to_expr( list(prefix) + [joint_name] + list(suffix))) for joint_name in joint_names: self.joint_map[joint_name] # trigger factory
def process_joint_specific_params(identifier_, default, god_map): d = KeyDefaultDict(lambda key: god_map.get_data(default)) d.update(god_map.safe_get_data(identifier_)) god_map.safe_set_data(identifier_, d) return KeyDefaultDict(lambda key: god_map.to_symbol(identifier_ + [key]))
def reset_cache(self): self._split_chain = KeyDefaultDict(self.__calc_get_split_chain)