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 add_object(self, object_):
"""
:type object_: URDFObject
"""
# FIXME this interface seems unintuitive, why not pass base pose as well?
if self.has_robot() and self.robot.get_name() == object_.get_name():
raise DuplicateNameException(u'object and robot have the same name')
if self.has_object(object_.get_name()):
raise DuplicateNameException(u'object with that name already exists')
self._objects[object_.get_name()] = object_
logging.loginfo(u'--> added {} to world'.format(object_.get_name()))
def load_urdf_string_into_bullet(urdf_string, pose=None):
"""
Loads a URDF string into the bullet world.
:param urdf_string: XML string of the URDF to load.
:type urdf_string: str
:param pose: Pose at which to load the URDF into the world.
:type pose: Pose
:return: internal PyBullet id of the loaded urdfs
:rtype: intload_urdf_string_into_bullet
"""
if pose is None:
pose = Pose()
pose.orientation.w = 1
if isinstance(pose, PoseStamped):
pose = pose.pose
object_name = URDFObject(urdf_string).get_name()
if object_name in get_body_names():
raise DuplicateNameException(
u'an object with name \'{}\' already exists in pybullet'.format(
object_name))
resolved_urdf = resolve_ros_iris_in_urdf(urdf_string)
filename = write_to_tmp(u'{}.urdfs'.format(random_string()), resolved_urdf)
with NullContextManager(
) if giskardpy.PRINT_LEVEL == DEBUG else suppress_stdout():
id = p.loadURDF(filename,
[pose.position.x, pose.position.y, pose.position.z], [
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w
],
flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT)
os.remove(filename)
return id
def add_robot(self, robot, base_pose, controlled_joints, joint_vel_limit,
joint_acc_limit, joint_weights, calc_self_collision_matrix,
ignored_pairs, added_pairs, symengine_backend,
symengine_opt_level):
"""
:type robot: giskardpy.world_object.WorldObject
:type controlled_joints: list
:type base_pose: PoseStamped
"""
if not isinstance(robot, WorldObject):
raise TypeError(u'only WorldObject can be added to world')
if self.has_robot():
raise RobotExistsException(u'A robot is already loaded')
if self.has_object(robot.get_name()):
raise DuplicateNameException(
u'can\'t add robot; object with name "{}" already exists'.
format(robot.get_name()))
self._robot = Robot.from_urdf_object(
urdf_object=robot,
base_pose=base_pose,
controlled_joints=controlled_joints,
path_to_data_folder=self._path_to_data_folder,
joint_vel_limit=joint_vel_limit,
joint_acc_limit=joint_acc_limit,
calc_self_collision_matrix=calc_self_collision_matrix,
joint_weights=joint_weights,
ignored_pairs=ignored_pairs,
added_pairs=added_pairs,
symengine_backend=symengine_backend,
symengine_opt_level=symengine_opt_level)
def attach_object(self, object, parent_link_name, transform):
"""
Rigidly attach another object to the robot.
:param object: Object that shall be attached to the robot.
:type object: UrdfObject
:param parent_link_name: Name of the link to which the object shall be attached.
:type parent_link_name: str
:param transform: Hom. transform between the reference frames of the parent link and the object.
:type Transform
"""
# TODO should only be called through world because this class does not know which objects exist
if self.has_attached_object(object.name):
# TODO: choose better exception type
raise DuplicateNameException(
u'An object \'{}\' has already been attached to the robot.'.
format(object.name))
# salvage last joint state and base pose
joint_state = self.get_joint_states()
base_pose = self.get_base_pose()
# salvage last collision matrix, and save collisions as pairs of link names
collision_matrix = set()
for collision in self.sometimes:
collision_matrix.add((self.link_id_to_name[collision[0]],
self.link_id_to_name[collision[1]]))
# assemble and store URDF string of new link and fixed joint
new_joint = FixedJoint(u'{}_joint'.format(object.name), transform,
parent_link_name, object.name)
self.attached_objects[object.name] = u'{}{}'.format(
to_urdf_string(new_joint), to_urdf_string(object, True))
new_urdf_string = self.get_urdf()
# remove last robot and load new robot from new URDF
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.removeBody(self.id)
self.id = load_urdf_string_into_bullet(new_urdf_string, base_pose)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
# reload joint info and last joint state
self.init_js_info()
self.set_joint_state(joint_state)
# reload last collision matrix as pairs of link IDs
self.sometimes = set()
for collision in collision_matrix:
self.sometimes.add((self.link_name_to_id[collision[0]],
self.link_name_to_id[collision[1]]))
# update the collision matrix for the newly attached object
object_id = self.link_name_to_id[object.name]
link_pairs = {(object_id, link_id)
for link_id in self.joint_id_to_info.keys()}
new_collisions = self.calc_self_collision_matrix(link_pairs)
self.sometimes.union(new_collisions)
print(u'object {} attached to {} in pybullet world'.format(
object.name, self.name))
def spawn_object_from_urdf_str(self, name, urdf, base_pose=Transform()):
"""
:type name: str
:param urdf: Path to URDF file, or content of already loaded URDF file.
:type urdf: str
:type base_pose: Transform
"""
if self.has_object(name):
raise DuplicateNameException(
u'object with name "{}" already exists'.format(name))
if self.has_robot() and self.get_robot().name == name:
raise DuplicateNameException(
u'robot with name "{}" already exists'.format(name))
self.deactivate_rendering()
self._objects[name] = PyBulletRobot(name, urdf, base_pose, False)
self.activate_rendering()
print(u'object {} added to pybullet world'.format(name))
def spawn_robot_from_urdf(self, robot_name, urdf, base_pose=Transform()):
"""
:type robot_name: str
:param urdf: URDF to spawn as loaded XML string.
:type urdf: str
:type base_pose: Transform
"""
if self.has_robot():
raise RobotExistsException(u'A robot is already loaded')
if self.has_object(robot_name):
raise DuplicateNameException(
u'can\'t add robot; object with name "{}" already exists'.
format(robot_name))
self.deactivate_rendering()
self._robot = PyBulletRobot(
robot_name,
urdf,
base_pose,
path_to_data_folder=self.path_to_data_folder)
self.activate_rendering()
def add_robot(self, robot, base_pose, controlled_joints, ignored_pairs, added_pairs):
"""
:type robot: giskardpy.world_object.WorldObject
:type controlled_joints: list
:type base_pose: PoseStamped
"""
if not isinstance(robot, WorldObject):
raise TypeError(u'only WorldObject can be added to world')
if self.has_robot():
raise RobotExistsException(u'A robot is already loaded')
if self.has_object(robot.get_name()):
raise DuplicateNameException(
u'can\'t add robot; object with name "{}" already exists'.format(robot.get_name()))
self._robot = Robot.from_urdf_object(urdf_object=robot,
base_pose=base_pose,
controlled_joints=controlled_joints,
path_to_data_folder=self._path_to_data_folder,
ignored_pairs=ignored_pairs,
added_pairs=added_pairs)
logging.loginfo(u'--> added {} to world'.format(robot.get_name()))
def attach_object(self, object, parent_link, transform):
if self.has_object(object.name):
raise DuplicateNameException(
u'Can\'t attach existing object \'{}\'.'.format(object.name))
self.get_robot().attach_object(object, parent_link, transform)