def attach_object(self, req):
"""
:type req: UpdateWorldRequest
"""
# assumes that parent has god map lock
if self.unsafe_get_world().has_object(req.body.name):
p = PoseStamped()
p.header.frame_id = self.map_frame
p.pose = self.unsafe_get_world().get_object(req.body.name).base_pose
p = transform_pose(req.pose.header.frame_id, p)
world_object = self.unsafe_get_world().get_object(req.body.name)
self.unsafe_get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose)
m = world_object.as_marker_msg()
m.header.frame_id = p.header.frame_id
m.pose = p.pose
else:
world_object = WorldObject.from_world_body(req.body)
self.unsafe_get_world().robot.attach_urdf_object(world_object,
req.pose.header.frame_id,
req.pose.pose)
logging.loginfo(u'--> attached object {} on link {}'.format(req.body.name, req.pose.header.frame_id))
m = world_object.as_marker_msg()
m.pose = req.pose.pose
m.header = req.pose.header
try:
m.frame_locked = True
self.publish_object_as_marker(m)
except:
pass
def attach_object(self, req):
"""
:type req: UpdateWorldRequest
"""
if self.get_world().has_object(req.body.name):
p = PoseStamped()
p.header.frame_id = self.map_frame
p.pose = self.get_world().get_object(req.body.name).base_pose
p = transform_pose(req.pose.header.frame_id, p)
world_object = self.get_world().get_object(req.body.name)
self.get_world().attach_existing_obj_to_robot(req.body.name, req.pose.header.frame_id, p.pose)
m = world_object.as_marker_msg()
m.header.frame_id = p.header.frame_id
m.pose = p.pose
else:
world_object = WorldObject.from_world_body(req.body)
self.get_world().robot.attach_urdf_object(world_object,
req.pose.header.frame_id,
req.pose.pose)
m = world_object.as_marker_msg()
m.pose = req.pose.pose
m.header = req.pose.header
try:
m.frame_locked = True
self.publish_object_as_marker(m)
except:
pass
def __add_pybullet_bug_fix_hack(self):
if not self.has_object(self.hack_name):
path = resolve_ros_iris(
u'package://giskardpy/urdfs/tiny_ball.urdf')
plane = WorldObject.from_urdf_file(path)
plane.set_name(self.hack_name)
self.add_object(plane)
def update(self):
try:
while not self.lock.empty():
updates = self.lock.get()
for object_state in updates.object_states: # type: ObjectState
object_name = object_state.object_id
if not self.get_world().has_object(object_name):
try:
world_object = WorldObject.from_object_state(
object_state)
except CorruptShapeException:
# skip objects without visual
continue
try:
self.get_world().add_object(world_object)
except pybullet.error as e:
logging.logwarn(
u'mesh \'{}\' does not exist'.format(
object_state.mesh_path))
continue
pose_in_map = transform_pose(MAP, object_state.pose).pose
self.get_world().set_object_pose(object_name, pose_in_map)
except Empty:
pass
return Status.RUNNING
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 __add_ground_plane(self):
"""
Adds a ground plane to the Bullet World.
"""
if not self.has_object(self.ground_plane_name):
path = resolve_ros_iris(
u'package://giskardpy/urdfs/ground_plane.urdf')
plane = WorldObject.from_urdf_file(path)
plane.set_name(self.ground_plane_name)
self.add_object(plane)
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_detach_object2(self, test_folder):
r = self.cls(donbot_urdf(), path_to_data_folder=test_folder)
r.init_self_collision_matrix()
scm = r.get_self_collision_matrix()
box = WorldObject.from_world_body(make_world_body_box())
p = Pose()
p.position = Point(0, 0, 0)
p.orientation = Quaternion(0, 0, 0, 1)
r.attach_urdf_object(box, u'gripper_tool_frame', p)
assert len(scm) < len(r.get_self_collision_matrix())
r.detach_sub_tree(box.get_name())
assert scm.symmetric_difference(r.get_self_collision_matrix()) == set()
def detach(self, joint_name, from_obj=None):
if from_obj is None or self.robot.get_name() == from_obj:
# this only works because attached simple objects have joint names equal to their name
p = self.robot.get_fk_pose(self.robot.get_root(), joint_name)
p_map = kdl_to_pose(self.robot.root_T_map.Inverse() *
msg_to_kdl(p))
cut_off_obj = self.robot.detach_sub_tree(joint_name)
else:
raise UnsupportedOptionException(
u'only detach from robot supported')
wo = WorldObject.from_urdf_object(cut_off_obj) # type: WorldObject
wo.base_pose = p_map
self.add_object(wo)
def detach(self, joint_name, from_obj=None):
if joint_name not in self.robot.get_joint_names():
raise UnknownBodyException(u'can\'t detach: {}'.format(joint_name))
if from_obj is None or self.robot.get_name() == from_obj:
# this only works because attached simple objects have joint names equal to their name
p = self.robot.get_fk_pose(self.robot.get_root(), joint_name)
p_map = kdl_to_pose(self.robot.root_T_map.Inverse() * msg_to_kdl(p))
parent_link = self.robot.get_parent_link_of_joint(joint_name)
cut_off_obj = self.robot.detach_sub_tree(joint_name)
logging.loginfo(u'<-- detached {} from link {}'.format(joint_name, parent_link))
else:
raise UnsupportedOptionException(u'only detach from robot supported')
wo = WorldObject.from_urdf_object(cut_off_obj) # type: WorldObject
wo.base_pose = p_map
self.add_object(wo)
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 add_object(self, req):
"""
:type req: UpdateWorldRequest
"""
world_body = req.body
global_pose = transform_pose(self.map_frame, req.pose).pose
world_object = WorldObject.from_world_body(world_body)
self.get_world().add_object(world_object)
self.get_world().set_object_pose(world_body.name, global_pose)
try:
m = self.get_world().get_object(world_body.name).as_marker_msg()
m.header.frame_id = self.map_frame
self.publish_object_as_marker(m)
except:
pass
# SUB-CASE: If it is an articulated object, open up a joint state subscriber
# FIXME also keep track of base pose
if world_body.joint_state_topic:
callback = (lambda msg: self.object_js_cb(world_body.name, msg))
self.object_js_subs[world_body.name] = \
rospy.Subscriber(world_body.joint_state_topic, JointState, callback, queue_size=1)
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