def initialize_god_map():
god_map = GodMap()
blackboard = Blackboard
blackboard.god_map = god_map
god_map.safe_set_data(identifier.rosparam, rospy.get_param(rospy.get_name()))
god_map.safe_set_data(identifier.robot_description, rospy.get_param(u'robot_description'))
path_to_data_folder = god_map.safe_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.safe_set_data(identifier.data_folder, path_to_data_folder)
# fix nWSR
nWSR = god_map.safe_get_data(identifier.nWSR)
if nWSR == u'None':
nWSR = None
god_map.safe_set_data(identifier.nWSR, nWSR)
pbw.start_pybullet(god_map.safe_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(e)
else:
break
rospy.sleep(0.5)
joint_weight_symbols = process_joint_specific_params(identifier.joint_weights,
identifier.default_joint_weight_identifier, god_map)
process_joint_specific_params(identifier.collisions_distances, identifier.default_collision_distances, god_map)
joint_vel_symbols = process_joint_specific_params(identifier.joint_vel, identifier.default_joint_vel, god_map)
joint_acc_symbols = process_joint_specific_params(identifier.joint_acc, identifier.default_joint_acc, god_map)
world = PyBulletWorld(god_map.safe_get_data(identifier.gui),
blackboard.god_map.safe_get_data(identifier.data_folder))
god_map.safe_set_data(identifier.world, world)
robot = WorldObject(god_map.safe_get_data(identifier.robot_description),
None,
controlled_joints)
world.add_robot(robot, None, controlled_joints, joint_vel_symbols, joint_acc_symbols, joint_weight_symbols, True,
ignored_pairs=god_map.safe_get_data(identifier.ignored_self_collisions),
added_pairs=god_map.safe_get_data(identifier.added_self_collisions),
symengine_backend=god_map.safe_get_data(identifier.symengine_backend),
symengine_opt_level=god_map.safe_get_data(identifier.symengine_opt_level))
joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(),
identifier.joint_states,
suffix=[u'position'])
joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(),
identifier.joint_states,
suffix=[u'velocity'])
world.robot.reinitialize(joint_position_symbols.joint_map, joint_vel_symbols.joint_map)
return god_map
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