def are_entries_known(self, collision_goals):
robot_name = self.robot.get_name()
robot_links = set(self.robot.get_link_names())
for collision_entry in collision_goals:
if not (collision_entry.body_b == robot_name
or collision_entry.body_b in self.get_object_names()
or self.all_body_bs(collision_entry)):
raise UnknownBodyException(u'body b \'{}\' unknown'.format(
collision_entry.body_b))
if not self.all_robot_links(collision_entry):
for robot_link in collision_entry.robot_links:
if robot_link not in robot_links:
raise UnknownBodyException(
u'robot link \'{}\' unknown'.format(robot_link))
if collision_entry.body_b == robot_name:
for robot_link in collision_entry.link_bs:
if robot_link != CollisionEntry.ALL and robot_link not in robot_links:
raise UnknownBodyException(
u'link b \'{}\' of body \'{}\' unknown'.format(
robot_link, collision_entry.body_b))
elif not self.all_body_bs(
collision_entry) and not self.all_link_bs(collision_entry):
object_links = self.get_object(
collision_entry.body_b).get_link_names()
for link_b in collision_entry.link_bs:
if link_b not in object_links:
raise UnknownBodyException(
u'link b \'{}\' of body \'{}\' unknown'.format(
link_b, collision_entry.body_b))
def attach_urdf_object(self, urdf_object, parent_link, pose, round_to=3):
"""
Rigidly attach another object to the robot.
:param urdf_object: Object that shall be attached to the robot.
:type urdf_object: URDFObject
:param parent_link_name: Name of the link to which the object shall be attached.
:type parent_link_name: str
:param pose: Hom. transform between the reference frames of the parent link and the object.
:type pose: Pose
"""
if urdf_object.get_name() in self.get_link_names():
raise DuplicateNameException(
u'\'{}\' already has link with name \'{}\'.'.format(
self.get_name(), urdf_object.get_name()))
if urdf_object.get_name() in self.get_joint_names():
raise DuplicateNameException(
u'\'{}\' already has joint with name \'{}\'.'.format(
self.get_name(), urdf_object.get_name()))
if parent_link not in self.get_link_names():
raise UnknownBodyException(
u'can not attach \'{}\' to non existent parent link \'{}\' of \'{}\''
.format(urdf_object.get_name(), parent_link, self.get_name()))
if len(
set(urdf_object.get_link_names()).intersection(
set(self.get_link_names()))) != 0:
raise DuplicateNameException(
u'can not merge urdfs that share link names')
if len(
set(urdf_object.get_joint_names()).intersection(
set(self.get_joint_names()))) != 0:
raise DuplicateNameException(
u'can not merge urdfs that share joint names')
origin = up.Pose([
np.round(pose.position.x, round_to),
np.round(pose.position.y, round_to),
np.round(pose.position.z, round_to)
],
euler_from_quaternion([
np.round(pose.orientation.x, round_to),
np.round(pose.orientation.y, round_to),
np.round(pose.orientation.z, round_to),
np.round(pose.orientation.w, round_to)
]))
joint = up.Joint(self.robot_name_to_root_joint(urdf_object.get_name()),
parent=parent_link,
child=urdf_object.get_root(),
joint_type=FIXED_JOINT,
origin=origin)
self._urdf_robot.add_joint(joint)
for j in urdf_object._urdf_robot.joints:
self._urdf_robot.add_joint(j)
for l in urdf_object._urdf_robot.links:
self._urdf_robot.add_link(l)
try:
del self._link_to_marker[urdf_object.get_name()]
except:
pass
self.reinitialize()
def remove_object(self, name):
if self.has_object(name):
self._objects[name].suicide()
logging.loginfo(u'<-- removed object {} from world'.format(name))
del (self._objects[name])
else:
raise UnknownBodyException(u'can\'t remove object \'{}\', because it doesn\' exist'.format(name))
def delete_object(self, object_name):
"""
Deletes an object with a specific name from the world.
:type object_name: str
"""
if not self.has_object(object_name):
raise UnknownBodyException(
u'Cannot delete unknown object {}'.format(object_name))
self.deactivate_rendering()
p.removeBody(self._objects[object_name].id)
self.activate_rendering()
del (self._objects[object_name])
print(u'object {} deleted from pybullet world'.format(object_name))
def remove_object(self, name):
if self.world.has_object(name):
self.world.delete_object(name)
if self.object_js_subs.has_key(name):
self.object_js_subs[name].unregister()
del (self.object_js_subs[name])
try:
del (self.object_joint_states[name])
except:
pass
elif self.world.get_robot().has_attached_object(name):
self.world.get_robot().detach_object(name)
else:
raise UnknownBodyException(
u'Cannot delete unknown object {}'.format(name))
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))
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 collision_goals_to_collision_matrix(self, collision_goals):
"""
:param collision_goals: list of CollisionEntry
:type collision_goals: list
:return: dict mapping (robot_link, body_b, link_b) -> min allowed distance
:rtype: dict
"""
if collision_goals is None:
collision_goals = []
min_allowed_distance = dict()
if len([
x for x in collision_goals if x.type in [
CollisionEntry.AVOID_ALL_COLLISIONS,
CollisionEntry.ALLOW_ALL_COLLISIONS
]
]) == 0:
# add avoid all collision if there is no other avoid or allow all
collision_goals.insert(
0,
CollisionEntry(
type=CollisionEntry.AVOID_ALL_COLLISIONS,
min_dist=self.default_collision_avoidance_distance))
controllable_links = self.god_map.get_data(
[self.controllable_links_identifier])
for collision_entry in collision_goals: # type: CollisionEntry
# check if msg got properly filled
if collision_entry.body_b == u'' and collision_entry.link_b != u'':
raise PhysicsWorldException(
u'body_b is empty but link_b is not')
# if robot link is empty, use all robot links
if collision_entry.robot_link == u'':
robot_links = set(self.world.get_robot().get_link_names())
elif collision_entry.robot_link in self.world.get_robot(
).get_link_names():
# TODO this check is linear but could be constant
robot_links = {collision_entry.robot_link}
else:
raise UnknownBodyException(u'robot_link \'{}\' unknown'.format(
collision_entry.robot_link))
# remove all non controllable links
# TODO make pybullet robot know which links are controllable
# TODO on first look controllable links are none
if controllable_links is not None:
robot_links.intersection_update(controllable_links)
# if body_b is empty, use all objects
if collision_entry.body_b == u'':
bodies_b = self.world.get_object_names()
elif self.world.has_object(collision_entry.body_b):
bodies_b = [collision_entry.body_b]
else:
raise UnknownBodyException(u'body_b \'{}\' unknown'.format(
collision_entry.body_b))
for body_b in bodies_b:
# if link_b is empty, use all links from body_b
if collision_entry.link_b == u'': # empty link b means every link from body b
links_b = self.world.get_object(body_b).get_link_names()
elif collision_entry.link_b in self.world.get_object(
body_b).get_link_names():
links_b = [collision_entry.link_b]
else:
raise UnknownBodyException(u'link_b \'{}\' unknown'.format(
collision_entry.link_b))
for robot_link, link_b in product(robot_links, links_b):
key = (robot_link, body_b, link_b)
if collision_entry.type == CollisionEntry.ALLOW_COLLISION or \
collision_entry.type == CollisionEntry.ALLOW_ALL_COLLISIONS:
if key in min_allowed_distance:
del min_allowed_distance[key]
elif collision_entry.type == CollisionEntry.AVOID_COLLISION or \
collision_entry.type == CollisionEntry.AVOID_ALL_COLLISIONS:
min_allowed_distance[key] = collision_entry.min_dist
return min_allowed_distance
