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_urdf_object(self, urdf_object, base_pose=Transform()):
"""
Spawns a new object into the Bullet world at a given pose.
:param urdf_object: New object to add to the world.
:type urdf_object: UrdfObject
:param base_pose: Pose at which to spawn the object.
:type base_pose: Transform
"""
self.spawn_object_from_urdf_str(urdf_object.name,
to_urdf_string(urdf_object), base_pose)
def test_named_empty_object(self):
my_obj = UrdfObject(name=u'foo')
urdf_string = to_urdf_string(my_obj)
self.assertEqual(urdf_string, u'<robot name="foo"><link name="foo"/></robot>')
def test_fixed_joint(self):
my_joint = FixedJoint(u'a_joint', Transform(), u'from_link', u'to_link')
urdf_string = to_urdf_string(my_joint)
self.assertEqual(u'<joint name="a_joint" type="fixed"><origin rpy="0.0 -0.0 0.0" xyz="0.0 0.0 0.0"/><parent link="from_link"/><child link="to_link"/></joint>', urdf_string)
def test_obj_with_box_visual(self):
my_obj = UrdfObject(name=u'my_box', visual_props=[VisualProperty(geometry=BoxShape(0.5, 1.5, 2.5))])
urdf_string = to_urdf_string(my_obj)
self.assertEqual(urdf_string, u'<robot name="my_box"><link name="my_box"><visual><origin rpy="0.0 -0.0 0.0" xyz="0.0 0.0 0.0"/><geometry><box size="0.5 1.5 2.5"/></geometry></visual></link></robot>')
def test_color_red(self):
my_obj = MaterialProperty(name=u'Red', color=ColorRgba(0.9, 0.0, 0.0, 1.0))
urdf_string = to_urdf_string(my_obj)
self.assertEqual(urdf_string, u'<material name="Red"><color rgba="0.9 0.0 0.0 1.0"/></material>')
def test_mesh_shape(self):
my_obj = MeshShape(filename=u'foo.bar', scale=[0.1, 0.2, 0.3])
urdf_string = to_urdf_string(my_obj)
self.assertEqual(urdf_string, u'<geometry><mesh filename="foo.bar" scale="0.1 0.2 0.3"/></geometry>')
def test_transform_with_rotation(self):
my_obj = Transform(rotation=Quaternion(0.707, 0.0, 0.707, 0.0))
urdf_string = to_urdf_string(my_obj)
self.assertEqual(urdf_string, u'<origin rpy="0.0 -1.57079632679 -3.14159265359" xyz="0.0 0.0 0.0"/>')
def test_transform_with_translation(self):
my_obj = Transform(translation=Point(1.1, 2.2, 3.3))
urdf_string = to_urdf_string(my_obj)
self.assertEqual(urdf_string, u'<origin rpy="0.0 -0.0 0.0" xyz="1.1 2.2 3.3"/>')
def test_named_empty_object_without_robot_tag(self):
my_obj = UrdfObject(name=u'foo')
urdf_string = to_urdf_string(my_obj, skip_robot_tag=True)
self.assertEqual(urdf_string, u'<link name="foo"/>')
