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)