Notebook

Tracy Giskard Demo¶

1. Launch Tracy simulation¶

In [ ]:

import rospy
import time
from utils import open_desktop, open_rvizweb, run_command, create_marker, publish_marker_array, TracyDemo
from std_msgs.msg import ColorRGBA
import random

In [ ]:

# Open a desktop GUI or a Rvizweb on left sidecar
# Note: if you see connection error, try running this code cell again
# open_rvizweb()
open_desktop()

In [ ]:

# Run the roslaunch in a background process
# Note: To see the logs, run this command in a terminal tab. 
# Run this code cell will restart the simulation.
run_command('roslaunch giskardpy giskardpy_tracy_standalone.launch')

2. Control the Tracy with the Giskard Python Interface¶

In [ ]:

# initialize a ros node and the python wrapper for giskard
import rospy
from pprint import pprint
from geometry_msgs.msg import PoseStamped, Point, Quaternion, Vector3Stamped, PointStamped
from giskardpy.python_interface.python_interface import GiskardWrapper
from giskardpy.goals.tracebot import InsertCylinder


rospy.init_node('gk_python_demo')
giskard_wrapper = GiskardWrapper()

List Joints and links¶

In [ ]:

# List all the controlled joints
group_name = giskard_wrapper.world.get_group_names()[0]
robot_joints = giskard_wrapper.world.get_controlled_joints(group_name)
# List all the links
robot_links = giskard_wrapper.world.get_group_info(group_name).links

print('Joints:')
pprint(robot_joints)
print('Links:')
pprint(robot_links)

Define a default pose¶

In [ ]:

default_pose = {
    'left_shoulder_pan_joint': 2.539670467376709,
    'left_shoulder_lift_joint': -1.46823854119096,
    'left_elbow_joint': 2.1197431723224085,
    'left_wrist_1_joint': -1.4825000625899811,
    'left_wrist_2_joint': 5.467689037322998,
    'left_wrist_3_joint': -0.9808381239520472,
    'right_shoulder_pan_joint': 3.7588136196136475,
    'right_shoulder_lift_joint': -1.7489210567870082,
    'right_elbow_joint': -2.054229259490967,
    'right_wrist_1_joint': -1.6140786610045375,
    'right_wrist_2_joint': 0.7295855283737183,
    'right_wrist_3_joint': 3.944669485092163,
}

# teleport giskard into the default pose (only works in simulation)
giskard_wrapper.monitors.add_set_seed_configuration(seed_configuration=default_pose)
giskard_wrapper.add_default_end_motion_conditions()
giskard_wrapper.execute()

# move to default pose
def reset_to_default_pose():
    giskard_wrapper.motion_goals.add_joint_position(goal_state=default_pose)
    giskard_wrapper.motion_goals.allow_all_collisions()
    giskard_wrapper.add_default_end_motion_conditions()
    giskard_wrapper.execute()

Spawn and attach cylinders to the hands¶

In [ ]:

cylinder_height = 0.121
cylinder_radius = 0.0225

def spawn_cylinder(hand='r', color=(1, 1, 0, 1)):
    cylinder_name = f'C_{time.time()}'
    frame_id = f'{hand}_gripper_tool_frame'
    pose = PoseStamped()
    pose.header.frame_id = frame_id
    pose.pose.position.z = cylinder_height / 5
    pose.pose.orientation.w = 1
    giskard_wrapper.world.add_cylinder(name=cylinder_name,
                             height=cylinder_height,
                             radius=cylinder_radius,
                             pose=pose,
                             parent_link=frame_id)
    # dye it
    giskard_wrapper.world.dye_group(cylinder_name, color)
    return cylinder_name

Call a goal to "insert" the cylinder at a point¶

In [ ]:

def insert_cylider_to(x, y, cylinder_name):
    hole_point = PointStamped()
    hole_point.header.frame_id = 'table'
    hole_point.point.x = x
    hole_point.point.y = y
    hole_point.point.z = 0.01
    giskard_wrapper.motion_goals.add_motion_goal(motion_goal_class=InsertCylinder.__name__,
                          cylinder_name=cylinder_name,
                          cylinder_height=cylinder_height,
                          hole_point=hole_point)
    giskard_wrapper.add_default_end_motion_conditions()
    giskard_wrapper.execute()
    # giskard_wrapper.world.remove_group(cylinder_name)
    giskard_wrapper.world.detach_group(cylinder_name)
    

Demo: Place the cylinder to block of the same color¶

In [ ]:

# Define 4 different colors
COLORS = [
    ColorRGBA(0.259, 0.522, 0.957, 1.0), # Blue
    ColorRGBA(0.984, 0.737, 0.02, 1.0), # Green
    ColorRGBA(0.204, 0.659, 0.325, 1.0), # Yellow
    ColorRGBA(0.918, 0.263, 0.208, 1.0), # Red
]

# Initialize demo scenario
demo = TracyDemo(COLORS)
demo.setup_color_blocks()

In [ ]:

# Generate a random sample of colors
shuffled_color = random.choices(COLORS, k=10)
# shuffled_color = random.sample(COLORS, len(COLORS))

In [ ]:

# Spawn cylinders and place to target locations
for color in shuffled_color:
    # randomly choose left or right hand
    hand = random.choice(['r', 'l'])
    # Spawn a cylinder in the choosen hand
    cylinder_name = spawn_cylinder(color=(color.r, color.g, color.b, color.a), hand=hand)
    rospy.sleep(1)
    # Get the target location
    target_pos = demo.get_color_pos(color=color, hand=hand)
    # execute the motion
    insert_cylider_to(target_pos.x, target_pos.y, cylinder_name)
    reset_to_default_pose()

In [ ]: