Table of Contents
The Giskard wrapper has several functions to add objects to the scene or to attach them to one of the links of the robot. Below is a list of all these functions with examples how to use them. At the bottom of the page you can find a Python example that demonstrates how to attach a box to one of the tip links of the PR2. In order to display the added objects in rviz you have to add a MarkerArray that listens to the topic /giskard/visualization_marker_array. There Giskard publishes markers of the added objects.
add_box
Description:
add_box() adds a box at the specified position in the given frame.
Parameters:
name: the name of the box
size: the length of the X,Y and Z side of the box.
frame_id: the frame in which the box is added
position: the coordinates where the box is added relative to the frame in frame_id
orientation: a quaternion that describes the orientation of the box relative to the frame in frame_id
Example:
add_box(self, name='box', size=(1, 1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1))
add_sphere
Description:
add_sphere() adds a sphere at the specified position in the given frame.
Parameters:
name: the name of the sphere
size: the radius of the sphere
frame_id: the frame in which the sphere is added
position: the coordinates where the sphere is added relative to the frame in frame_id
orientation: a quaternion that describes the orientation of the sphere relative to the frame in frame_id
Example:
add_sphere(self, name='sphere', size=1, frame_id='map', position=(0,0,0), orientation=(0,0,0,1))
add_cylinder
Description:
add_cylinder() adds a cylinder at the specified position in the given frame.
Parameters:
name: the name of the cylinder
size: the height and radius of the cylinder
frame_id: the frame in which the cylinder is added
position: the coordinates where the cylinder is added relative to the frame in frame_id
orientation: a quaternion that describes the orientation of the cylinder relative to the frame in frame_id
Example:
add_cylinder(self, name='cylinder', size=(1,1), frame_id='map', position=(0,0,0), orientation=(0,0,0,1))
add_urdf
Description:
add_urdf() adds a robot described by a urdf at the specified position in the given frame.
Parameters:
name: the name of the cylinder
urdf: the urdf of the robot
js_topic: the joint state topic the robot listens on
pose: a geometry_msgs/PoseStamped that describes the position of robot
Example:
p = PoseStamped() add_urdf(self, name='urdf_robot', urdf='urdf_string', js_topic='/js_topic', pose=p)
attach_box
Description:
attach_box() attaches a box to the specified frame.
Parameters:
name: the name of the box
size: the length of the X,Y and Z side of the box.
frame_id: the frame the box will be attached to
position: the coordinates where the box is added relative to the frame in frame_id
orientation: a quaternion that describes the orientation of the box relative to the frame in frame_id
Example:
attach_box(self, name='box', size=(1, 1, 1), frame_id=u'map', position=(0, 0, 0), orientation=(0, 0, 0, 1))
remove_object
Description:
remove_object() removes the object with the given name.
Parameters:
name: the name of the object
Example:
remove_object(self, name='object_name')
Python Example
The following code demonstrates how to attach a box to the left gripper of the PR2. It also shows that the collision avoidance works with attached objects.
import rospy from giskardpy.python_interface import GiskardWrapper from geometry_msgs.msg import PoseStamped # the starting pose for this tutorial starting_pose = { 'l_elbow_flex_joint': - 1.1343683863086362, 'l_forearm_roll_joint': 7.517553513504836, 'l_shoulder_lift_joint': 0.5726770101613905, 'l_shoulder_pan_joint': 0.1592669164939349, 'l_upper_arm_roll_joint': 0.5532568387077381, 'l_wrist_flex_joint': - 1.215660155912625, 'l_wrist_roll_joint': 4.249300323527076, 'r_forearm_roll_joint': 0.0, 'r_shoulder_lift_joint': 0.0, 'r_shoulder_pan_joint': 0.0, 'r_upper_arm_roll_joint': 0.0, 'r_wrist_roll_joint': 0.0, 'r_elbow_flex_joint': 0.0, 'r_wrist_flex_joint': 0.0, 'torso_lift_joint': 0.2} l_tip = "l_gripper_tool_frame" r_tip = 'r_gripper_tool_frame' default_root = 'base_link' rospy.init_node('test') giskard_wrapper = GiskardWrapper() # move the robot into starting position by setting a joint goal and executing it giskard_wrapper.set_joint_goal(starting_pose) giskard_wrapper.plan_and_execute() # attach a box to the left gripper attached_link_name = 'box' giskard_wrapper.attach_box(attached_link_name, [0.1, 0.02, 0.02], l_tip, [0.05, 0, 0]) # set a joint goal to prevent the PR2 from lifting the torso giskard_wrapper.set_joint_goal({'torso_lift_joint': 0.2}) # create a cartesian goal for the left gripper 20cm above the current position p = PoseStamped() p.header.frame_id = l_tip p.pose.position.z = 0.2 p.pose.orientation.w = 1 # set the cartesian goal and execute it giskard_wrapper.set_cart_goal(default_root, l_tip, p) giskard_wrapper.plan_and_execute() # remove the attached box giskard_wrapper.remove_object(attached_link_name)