def from_world_body(cls, world_body, *args, **kwargs):
"""
:type world_body: giskard_msgs.msg._WorldBody.WorldBody
:rtype: URDFObject
"""
links = []
joints = []
if world_body.type == world_body.PRIMITIVE_BODY or world_body.type == world_body.MESH_BODY:
if world_body.shape.type == world_body.shape.BOX:
geometry = up.Box(world_body.shape.dimensions)
elif world_body.shape.type == world_body.shape.SPHERE:
geometry = up.Sphere(world_body.shape.dimensions[0])
elif world_body.shape.type == world_body.shape.CYLINDER:
geometry = up.Cylinder(
world_body.shape.dimensions[
world_body.shape.CYLINDER_RADIUS],
world_body.shape.dimensions[
world_body.shape.CYLINDER_HEIGHT])
elif world_body.shape.type == world_body.shape.CONE:
raise TypeError(u'primitive shape cone not supported')
elif world_body.type == world_body.MESH_BODY:
geometry = up.Mesh(world_body.mesh)
else:
raise CorruptShapeException(
u'primitive shape \'{}\' not supported'.format(
world_body.shape.type))
# FIXME test if this works on 16.04
try:
link = up.Link(world_body.name)
link.add_aggregate(
u'visual',
up.Visual(geometry,
material=up.Material(u'green',
color=up.Color(0, 1, 0,
1))))
link.add_aggregate(u'collision', up.Collision(geometry))
except AssertionError:
link = up.Link(world_body.name,
visual=up.Visual(geometry,
material=up.Material(
u'green',
color=up.Color(0, 1, 0,
1))),
collision=up.Collision(geometry))
links.append(link)
elif world_body.type == world_body.URDF_BODY:
o = cls(world_body.urdf, *args, **kwargs)
o.set_name(world_body.name)
return o
else:
raise CorruptShapeException(
u'world body type \'{}\' not supported'.format(
world_body.type))
return cls.from_parts(world_body.name, links, joints, *args, **kwargs)
def add_object(self, object_):
"""
:type object_: giskardpy.world_object.WorldObject
:return:
"""
# TODO create from world object to avoid basepose and joint state getting lost?
try:
pwo = PyBulletWorldObject.from_urdf_object(object_)
pwo.base_pose = object_.base_pose
pwo.joint_state = object_.joint_state
except Exception as e:
if isinstance(e, error):
raise CorruptShapeException(e)
raise e
return super(PyBulletWorld, self).add_object(pwo)
def world_body_to_urdf_object(world_body_msg):
"""
Converts a body from a ROS message to the corresponding internal representation.
:param world_body_msg: Input message that shall be converted.
:type world_body_msg: WorldBody
:return: Internal representation of body, filled with data from input message.
:rtype UrdfObject
"""
if world_body_msg.type is WorldBody.MESH_BODY:
geom = MeshShape(filename=world_body_msg.mesh)
elif world_body_msg.type is WorldBody.PRIMITIVE_BODY:
if world_body_msg.shape.type is SolidPrimitive.BOX:
geom = BoxShape(
world_body_msg.shape.dimensions[SolidPrimitive.BOX_X],
world_body_msg.shape.dimensions[SolidPrimitive.BOX_Y],
world_body_msg.shape.dimensions[SolidPrimitive.BOX_Z])
elif world_body_msg.shape.type is SolidPrimitive.CYLINDER:
geom = CylinderShape(
world_body_msg.shape.dimensions[
SolidPrimitive.CYLINDER_RADIUS],
world_body_msg.shape.dimensions[
SolidPrimitive.CYLINDER_HEIGHT])
elif world_body_msg.shape.type is SolidPrimitive.SPHERE:
geom = SphereShape(
world_body_msg.shape.dimensions[SolidPrimitive.SPHERE_RADIUS])
else:
raise CorruptShapeException(
u'Invalid primitive shape \'{}\' of world body \'{}\''.format(
world_body_msg.shape.type, world_body_msg.name))
elif world_body_msg.type is WorldBody.URDF_BODY:
# TODO: complete me
pass
else:
# TODO: replace me by a proper exception that can be reported back to the service client
raise RuntimeError(u'Invalid shape of world body: {}'.format(
world_body_msg.shape))
col = CollisionProperty(name=world_body_msg.name + u'_col', geometry=geom)
vis = VisualProperty(name=world_body_msg.name + u'_vis', geometry=geom)
return UrdfObject(name=world_body_msg.name,
collision_props=[col],
visual_props=[vis])
def from_object_state(cls, object_state, *args, **kwargs):
"""
:type world_body: knowrob_objects.msg._ObjectState.ObjectState
:rtype: URDFObject
"""
links = []
joints = []
shape = [object_state.size.y,
object_state.size.x,
object_state.size.z]
if object_state.has_visual and object_state.mesh_path == u'':
geometry = up.Box(shape)
elif object_state.has_visual:
geometry = up.Mesh(object_state.mesh_path)
else:
raise CorruptShapeException(u'object state has no visual')
link = up.Link(object_state.object_id,
visual=up.Visual(geometry, material=up.Material(u'green', color=up.Color(0, 1, 0, 1))),
collision=up.Collision(geometry))
links.append(link)
return cls.from_parts(object_state.object_id, links, joints, *args, **kwargs)