This tutorial shows you how to add objects to the world using the update_world service offered by Giskard. These objects can have simple shapes likes spheres and cubes but you can use meshes to define the shape of an object.

Adding Objects via Python

Adding A Sphere

We will start by adding a simple sphere to the world. In order to do that we have to create a UpdateWorldRequest request and call the service. The Python code to do that looks like this.

import rospy
from geometry_msgs.msg import Point, Quaternion, PoseStamped
from giskard_msgs.srv import UpdateWorld, UpdateWorldRequest
from giskard_msgs.msg import WorldBody
from shape_msgs.msg import SolidPrimitive
 
# The get_sphere_request() function creates a WorldBody which is defined in the giskard_msgs Package.
# The WorldBody message is used to specify which object you want to add to the world.
# Then the newly created WorldBody is used to create a request for the UpdateWorld service
# which is also defined in giskard_msgs.
def get_sphere_request():
    sphere = WorldBody()
    sphere.type = WorldBody.PRIMITIVE_BODY
    sphere.name = 'muh'
    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = 'map'
    pose.pose.position = Point(1.25, -0.3, 0.77)
    pose.pose.orientation = Quaternion(0, 0, 0, 1)
    sphere.shape.type = SolidPrimitive.SPHERE
    sphere.shape.dimensions.append(0.05)  # radius of 5cm
    return UpdateWorldRequest(UpdateWorldRequest.ADD, sphere, False, pose)
 
 
 
rospy.init_node('upload_object')
# connect to service
rospy.wait_for_service('giskard/update_world')
update_world = rospy.ServiceProxy('giskard/update_world', UpdateWorld)
 
# create goal
request = get_sphere_request()
 
# send request
response = update_world(request)
print(response)

Adding More Complex Objects

The WorldBody message also supports meshes and urdfs in case you want to add more complex objects to the world. Adding these objects is just as easy as adding simple shapes. All you have to do is to adjust the get_sphere_request function from above so that it uses a mesh instead of a simple shape. If you want to add a table for instance the code could look like this.

#This functions creates a request for the UpdateWorld service that adds a table mesh to the world
def get_table_request():
    table = WorldBody()
    table.type = WorldBody.MESH_BODY
    table.name = "table"
    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "map"
    pose.pose.position = Point(1.0, 0.0, 0.0)
    pose.pose.orientation = Quaternion(0,0,1,1.57)
    table.mesh = 'package://iai_kitchen/meshes/misc/big_table_1.stl'
    return UpdateWorldRequest(UpdateWorldRequest.ADD, table, False, pose)

You can use a urdf as well.

def add_2_pr2():
    pr2 = WorldBody()
    pr2.name = "pr3"
    pr2.type = WorldBody.URDF_BODY
    pr2.urdf = rospy.get_param('/robot_description')
    pr2.joint_state_topic = '/joint_states'
    pose = PoseStamped()
    pose.header.stamp = rospy.Time.now()
    pose.header.frame_id = "map"
    pose.pose.position = Point(2.0, 0.0, 0.0)
    pose.pose.orientation = Quaternion(0, 0, 1, 1.57)
    return UpdateWorldRequest(UpdateWorldRequest.ADD, pr2, False, pose)

Attaching Objects To The Robot

If you want to attach an object to the robot, you can do that by setting the rigidly_attached flag of the UpdateWorldRequest to true. The object is attached to the frame specified in the header of the pose.

def get_attach_cylinder_request():
    cylinder = WorldBody()
    cylinder.type = WorldBody.PRIMITIVE_BODY
    cylinder.name = 'cylinder'
    cylinder.shape.type = SolidPrimitive.CYLINDER
    cylinder.shape.dimensions.append(0.15) # height of 15cm
    cylinder.shape.dimensions.append(0.05) # radius of 0.5cm
    pose = PoseStamped()
    pose.header.frame_id = 'l_gripper_tool_frame'
    pose.pose.orientation.w = 1.0
    return UpdateWorldRequest(UpdateWorldRequest.ADD, cylinder, True, pose) # rigidly_attached is set to true to attach the cylinder

Running The Code

Save the code in a file called muh.py and run python muh.py. Make sure your workspace is sourced.

Adding Objects Via Console

There are scripts in the giskardpy package that let you add objects via console. Below is a list of available commands with example calls.

Add Box:
_name: The name of the object
_size: The size of the box in x,y and z direction
_frame_id: The name of the frame in which the object is added
_position: The position where the object is added
_orientation: The orientation of the object

rosrun giskardpy add_box.py _name:=box _size:=[1,1,1] _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]

Add Cylinder:
_name: The name of the object
_size: The height and radius of the cylinder _frame_id: The name of the frame in which the object is added
_position: The position where the object is added
_orientation: The orientation of the object

rosrun giskardpy add_cylinder.py _name:=cylinder _size:=[1,1] _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]

Add Sphere:
_name: The name of the object
_size: The radius of the sphere
_frame_id: The name of the frame in which the object is added
_position: The position where the object is added
_orientation: The orientation of the object

rosrun giskardpy add_sphere.py _name:=sphere _size:=1 _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]

Add URDF:
_name: The name of the object
_param: The name of the paramter on the paramter server that contains the URDF description
_js: The name of the topic where the joint states are published
_root_frame: _frame_id: The name of the frame in which the object is added

rosrun giskardpy add_urdf.py _name:=kitchen _param:=kitchen_description _js:=kitchen_joint_states _root_frame:=world _frame_id:=map

Attach Object:

rosrun giskardpy add_box.py _name:=box _size:=[1,1,1] _frame_id:=map _position:=[0,0,0] _orientation:=[0,0,0,1]

Clear World:

rosrun giskardpy clear_world.py

Remove Object:
_name: The name of the object that shall be removed

rosrun giskardpy remove_object.py _name:=box