def test_get_symbol_map(self, keys_values):
keys, values = keys_values
gm = GodMap()
for key, value in zip(keys, values):
gm.set_data([key], value)
gm.to_symbol([key])
self.assertEqual(len(gm.get_values(keys)), len(keys))
def test_list_double_index(self):
key = 'asdf'
value = np.array([[0, 1], [2, 3]])
db = GodMap()
db.set_data([key], value)
for i in range(value.shape[0]):
for j in range(value.shape[1]):
self.assertEqual(db.get_data([key, i, j]), value[i, j])
def test_dict2(self, key, key_values):
d = {}
db = GodMap()
db.set_data([key], d)
for k, v in zip(*key_values):
db.set_data([key, k], v)
for k, v in zip(*key_values):
self.assertEqual(db.get_data([key, k]), v)
def test_class3(self, class_names):
db = GodMap()
for i, name in enumerate(class_names):
c = type(str(name), (object,), {})()
db.set_data(class_names[:i + 1], c)
for i, name in enumerate(class_names):
c = db.get_data(class_names[:i + 1])
self.assertEqual(type(c).__name__, name)
def test_list_overwrite_entry(self, key, lists):
first_values, second_values = lists
db = GodMap()
db.set_data([key], first_values)
for i, v in enumerate(first_values):
self.assertEqual(db.get_data([key, i]), v)
db.set_data([key, i], second_values[i])
for i, v in enumerate(second_values):
self.assertEqual(db.get_data([key, i]), v)
def test_clear_cache(self):
db = GodMap()
d = {'b': 'c'}
db.set_data(['a'], d)
self.assertEqual('c', db.get_data(['a', 'b']))
db.clear_cache()
class C(object):
b = 'c'
db.set_data(['a'], C())
self.assertEqual('c', db.get_data(['a', 'b']))
def test_function_no_param(self, key1, key2, key3):
db = GodMap()
class MUH(object):
def __call__(self):
return [key3]
a = MUH()
d = {key2: a}
db.set_data([key1], d)
self.assertEqual(key3, db.get_data([key1, key2, tuple(), 0]))
def test_function2(self, key, dict_key, return_value):
db = GodMap()
class MUH(object):
def __call__(self, god_map):
return return_value
a = MUH()
d = {dict_key: a}
db.set_data([key], d)
self.assertEqual(db.get_data([key, dict_key]), return_value)
def test_function_2param_call(self, key1, key2, key3, key4):
db = GodMap()
class MUH(object):
def __call__(self, next_member, next_next_member):
return next_next_member
a = MUH()
d = {key2: a}
db.set_data([key1], d)
self.assertEqual(db.get_data([key1, key2, (key3, key4)]), key4)
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=[],
ignored_pairs=set(),
added_pairs=set())
gm.set_data([u'world'], w)
gm_robot = gm.get_data(identifier.robot)
assert 'pr2' == gm_robot.get_name()
def test_function4(self, key1, key2, key3, key4, key5):
db = GodMap()
class MUH(object):
def __call__(self, next_member, next_next_member):
return [key5]
a = MUH()
d = {key2: a}
db.set_data([key1], d)
self.assertEqual(key5, db.get_data([key1, key2, (key3, key4), 0]))
self.assertEqual(key5, db.get_data([key1, key2, (key3, key4), 0]))
self.assertEqual(key5, db.get_data([key1, key2, (key3, key4), 0]))
def test_class1(self, key, class_name, key_values):
c = type(str(class_name), (object,), {})()
for k, v in zip(*key_values):
setattr(c, k, None)
db = GodMap()
db.set_data([key], c)
for k, v in zip(*key_values):
self.assertEqual(db.get_data([key, k]), None)
for k, v in zip(*key_values):
db.set_data([key, k], v)
for k, v in zip(*key_values):
self.assertEqual(db.get_data([key, k]), v)
def test_function3(self, key1, key2, key3, key4, key5):
db = GodMap()
class MUH(object):
def __call__(self, next_member):
return [key5]
a = MUH()
d = {key2: a}
db.set_data([key1], d)
try:
db.get_data([key1, key2, (key3, key4), 0])
assert False
except TypeError:
assert True
def test_to_symbol(self, key, value):
gm = GodMap()
gm.set_data([key], value)
self.assertTrue(isinstance(gm.to_symbol([key]), sw.Symbol))
self.assertTrue(key in str(gm.to_symbol([key])))
class Node(object):
def __init__(self, urdf_path, base_link, eef_link, wrist_link, wps, projection_frame):
self.vis = ROSVisualizer('force_test', base_frame=base_link, plot_topic='plot')
self.wps = [Frame(Vector3(*wp[3:]), Quaternion(*list(quaternion_from_rpy(*wp[:3])))) for wp in wps]
self.wpsIdx = 0
self.base_link = base_link
self.god_map = GodMap()
self.js_prefix = 'joint_state'
self.traj_pub = rospy.Publisher('~joint_trajectory', JointTrajectoryMsg, queue_size=1, tcp_nodelay=True)
self.wrench_pub = rospy.Publisher('~wrist_force_transformed', WrenchMsg, queue_size=1, tcp_nodelay=True)
# Giskard ----------------------------
# Init controller, setup controlled joints
self.controller = Controller(res_pkg_path(urdf_path), res_pkg_path('package://pbsbtest/.controllers/'), 0.6)
self.robot = self.controller.robot
self.js_input = JointStatesInput.prefix_constructor(self.god_map.get_expr, self.robot.get_joint_names(), self.js_prefix, 'position')
self.robot.set_joint_symbol_map(self.js_input)
self.controller.set_controlled_joints(self.robot.get_joint_names())
# Get eef and sensor frame
self.sensor_frame = self.robot.get_fk_expression(base_link, wrist_link)
self.eef_frame = self.robot.get_fk_expression(base_link, eef_link)
self.eef_pos = pos_of(self.eef_frame)
# Construct motion frame
mplate_pos = pos_of(self.robot.get_fk_expression(base_link, 'arm_mounting_plate'))
self.motion_frame = frame3_rpy(pi, 0, pi * 0.5, mplate_pos)
wp_frame = self.motion_frame * FrameInput.prefix_constructor('goal/position', 'goal/quaternion', self.god_map.get_expr).get_frame()
wp_pos = pos_of(wp_frame)
# Projection frame
self.world_to_projection_frame = projection_frame
# Pre init
self.god_map.set_data(['goal'], self.wps[0])
self.tick_rate = 50
deltaT = 1.0 / self.tick_rate
js = {j: SingleJointState(j) for j in self.robot.get_joint_names()}
js['gripper_base_gripper_left_joint'] = SingleJointState('gripper_base_gripper_left_joint')
self.god_map.set_data([self.js_prefix], js)
err = norm(wp_pos - self.eef_pos)
rot_err = dot(wp_frame * unitZ, self.eef_frame * unitZ)
scons = position_conv(wp_pos, self.eef_pos) # {'align position': SoftConstraint(-err, -err, 1, err)}
scons.update(rotation_conv(rot_of(wp_frame), rot_of(self.eef_frame), rot_of(self.eef_frame).subs(self.god_map.get_expr_values())))
self.controller.init(scons, self.god_map.get_free_symbols())
print('Solving for initial state...')
# Go to initial configuration
js = run_controller_until(self.god_map, self.controller, js, self.js_prefix, [(err, 0.01), (1 - rot_err, 0.001)], deltaT)[0]
print('About to plan trajectory...')
# Generate trajectory
trajectory = OrderedDict()
traj_stamp = rospy.Duration(0.0)
section_stamps = []
for x in range(1, len(self.wps)):
print('Heading for point {}'.format(x))
self.god_map.set_data(['goal'], self.wps[x])
js, sub_traj = run_controller_until(self.god_map, self.controller, js, self.js_prefix, [(err, 0.01)], deltaT)
for time_code, position in sub_traj.items():
trajectory[traj_stamp + time_code] = position
traj_stamp += sub_traj.keys()[-1]
print('Trajectory planned! {} points over a duration of {} seconds.'.format(len(trajectory), len(trajectory) * deltaT))
traj_msg = JointTrajectoryMsg()
traj_msg.header.stamp = rospy.Time.now()
traj_msg.joint_names = trajectory.items()[0][1].keys()
for t, v in trajectory.items():
point = JointTrajectoryPointMsg()
point.positions = [v[j].position for j in traj_msg.joint_names]
point.time_from_start = t
traj_msg.points.append(point)
js_advertised = False
print('Waiting for joint state topic to be published.')
while not js_advertised and not rospy.is_shutdown():
topics = [t for t, tt in rospy.get_published_topics()]
if '/joint_states' in topics:
js_advertised = True
if not js_advertised:
print('Robot does not seem to be started. Aborting...')
return
print('Joint state has been advertised. Waiting 2 seconds for the controller...')
rospy.sleep(2.0)
print('Sending trajectory.')
self.traj_pub.publish(traj_msg)
self.tfListener = tf.TransformListener()
self.wrench_sub = rospy.Subscriber('~wrist_wrench', WrenchMsg, callback=self.transform_wrench, queue_size=1)
def transform_wrench(self, wrench_msg):
try:
self.tfListener.waitForTransform(self.base_link, wrench_msg.header.frame_id, rospy.Time(0), rospy.Duration(0.1))
(trans,rot) = self.tfListener.lookupTransform(self.base_link, wrench_msg.header.frame_id, rospy.Time(0))
except:
print('Failed to transform wrench from {} to {}'.format(wrench_msg.header.frame_id, self.base_link))
return
f = wrench_msg.wrench.force
f = self.world_to_projection_frame * frame3_quaternion(trans[0], trans[1], trans[2], rot[0], rot[1], rot[2], rot[3]) * vector3(f.x, f.y, f.z)
#t = self.projection_frame.subs(cvals) * vec3(*state.m)
sensor_msg = WrenchMsg()
sensor_msg.header.stamp = wrench_msg.header.stamp
sensor_msg.header.frame_id = self.base_link
sensor_msg.wrench.force.x = f[0]
sensor_msg.wrench.force.y = f[1]
sensor_msg.wrench.force.z = f[2]
# sensor_msg.wrench.torque.x = t[0]
# sensor_msg.wrench.torque.y = t[1]
# sensor_msg.wrench.torque.z = t[2]
self.wrench_pub.publish(sensor_msg)
def test_namedtuple(self, key, tuple_name, key_values):
Frame = namedtuple(tuple_name, key_values[0])
db = GodMap()
db.set_data([key], Frame(*key_values[1]))
for k, v in zip(*key_values):
self.assertEqual(db.get_data([key, k]), v)
def test_namedtuple1(self):
Frame = namedtuple(u'Frame', [u'pos'])
db = GodMap()
db.set_data([u'f12'], Frame(pos=2))
with self.assertRaises(AttributeError):
db.set_data([u'f12', u'pos'], 42)
def test_set_get_integer2(self, key, number):
db = GodMap()
db.set_data([key], number)
self.assertEqual(db.get_data([key]), number, msg=u'key={}, number={}'.format(key, number))
self.assertEqual(db.get_data([key]), number, msg=u'key={}, number={}'.format(key, number))
def test_set_get_float(self, key, number):
db = GodMap()
db.set_data([key], number)
db.set_data([key], number)
self.assertEqual(db.get_data([key]), number)
def test_dict3(self, key, tuple_key, value):
tuple_key = tuple(tuple_key)
d = {tuple_key: value}
db = GodMap()
db.set_data([key], d)
self.assertEqual(db.get_data([key, tuple_key]), value)
def initialize_god_map():
god_map = GodMap()
blackboard = Blackboard
blackboard.god_map = god_map
load_config_file()
god_map.set_data(identifier.rosparam, rospy.get_param(rospy.get_name()))
god_map.set_data(identifier.robot_description, rospy.get_param(u'robot_description'))
path_to_data_folder = god_map.get_data(identifier.data_folder)
# fix path to data folder
if not path_to_data_folder.endswith(u'/'):
path_to_data_folder += u'/'
god_map.set_data(identifier.data_folder, path_to_data_folder)
# fix nWSR
nWSR = god_map.get_data(identifier.nWSR)
if nWSR == u'None':
nWSR = None
god_map.set_data(identifier.nWSR, nWSR)
pbw.start_pybullet(god_map.get_data(identifier.gui))
while not rospy.is_shutdown():
try:
controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state',
JointTrajectoryControllerState,
timeout=5.0).joint_names
except ROSException as e:
logging.logerr(u'state topic not available')
logging.logerr(str(e))
else:
break
rospy.sleep(0.5)
joint_weight_symbols = process_joint_specific_params(identifier.joint_weight,
identifier.joint_weight_default,
identifier.joint_weight_override,
god_map)
process_joint_specific_params(identifier.self_collision_avoidance_distance,
identifier.self_collision_avoidance_default_threshold,
identifier.self_collision_avoidance_default_override,
god_map)
process_joint_specific_params(identifier.external_collision_avoidance_distance,
identifier.external_collision_avoidance_default_threshold,
identifier.external_collision_avoidance_default_override,
god_map)
# TODO add checks to test if joints listed as linear are actually linear
joint_velocity_linear_limit_symbols = process_joint_specific_params(identifier.joint_velocity_linear_limit,
identifier.joint_velocity_linear_limit_default,
identifier.joint_velocity_linear_limit_override,
god_map)
joint_velocity_angular_limit_symbols = process_joint_specific_params(identifier.joint_velocity_angular_limit,
identifier.joint_velocity_angular_limit_default,
identifier.joint_velocity_angular_limit_override,
god_map)
joint_acceleration_linear_limit_symbols = process_joint_specific_params(identifier.joint_acceleration_linear_limit,
identifier.joint_acceleration_linear_limit_default,
identifier.joint_acceleration_linear_limit_override,
god_map)
joint_acceleration_angular_limit_symbols = process_joint_specific_params(
identifier.joint_acceleration_angular_limit,
identifier.joint_acceleration_angular_limit_default,
identifier.joint_acceleration_angular_limit_override,
god_map)
world = PyBulletWorld(False, blackboard.god_map.get_data(identifier.data_folder))
god_map.set_data(identifier.world, world)
robot = WorldObject(god_map.get_data(identifier.robot_description),
None,
controlled_joints)
world.add_robot(robot, None, controlled_joints,
ignored_pairs=god_map.get_data(identifier.ignored_self_collisions),
added_pairs=god_map.get_data(identifier.added_self_collisions))
joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(),
identifier.joint_states,
suffix=[u'position'])
joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_movable_joints(),
identifier.joint_states,
suffix=[u'velocity'])
world.robot.update_joint_symbols(joint_position_symbols.joint_map, joint_vel_symbols.joint_map,
joint_weight_symbols,
joint_velocity_linear_limit_symbols, joint_velocity_angular_limit_symbols,
joint_acceleration_linear_limit_symbols, joint_acceleration_angular_limit_symbols)
world.robot.init_self_collision_matrix()
return god_map
def test_tuple1(self, key, value):
value = tuple(value)
db = GodMap()
db.set_data([key], value)
for i, v in enumerate(value):
self.assertEqual(db.get_data([key, i]), v)
def test_to_symbol(self, key, value):
gm = GodMap()
gm.set_data([key], value)
self.assertTrue(w.is_symbol(gm.to_symbol([key])))
self.assertTrue(key in str(gm.to_symbol([key])))
def test_list_index_error(self, key, l):
db = GodMap()
db.set_data([key], l)
with self.assertRaises(IndexError):
db.set_data([key, len(l) + 1], 0)
def test_god_map_key_error(self, key, wrong_key, number, default_value):
assume(key != wrong_key)
db = GodMap(default_value)
db.set_data([key], number)
self.assertEqual(db.get_data([wrong_key]), default_value, msg=u'key={}, number={}'.format(key, number))
def test_list_negative_index(self, key, l):
db = GodMap()
db.set_data([key], l)
for i in range(len(l)):
self.assertEqual(db.get_data([key, -i]), l[-i])
def test_function1(self, key, value):
# TODO not clean that i try to call every function
db = GodMap()
f = lambda gm: value
db.set_data([key], f)
self.assertEqual(db.get_data([key]), value)
def test_function_1param_lambda(self, key, key2):
db = GodMap()
f = lambda x: x
db.set_data([key], f)
self.assertEqual(db.get_data([key, (key2,)]), key2)
