Table of Contents
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