コード例 #1
0
def main():
    world_view_folder = "example_07_jogging"

    world_view_client = WorldViewClient()
    move_group = example_utils.get_move_group()
    current_pose = move_group.get_end_effector().compute_pose(
        move_group.get_current_joint_positions())

    jogging_client = JoggingClient()
    jogging_client.set_move_group_name(example_utils.get_move_group_name())
    # register feedback function, to get alerted when an error occurs
    jogging_client.register(feedback_function)

    current_pose = move_group.get_end_effector().compute_pose(
        move_group.get_current_joint_positions())

    point_name = "TrackingPose"
    # Write pose to World View, so the tracking begins with the current end effector pose
    try:
        world_view_client.add_pose(point_name, world_view_folder, current_pose)
    except Xamla_ArgumentError:
        world_view_client.update_pose(point_name, world_view_folder,
                                      current_pose)

    get_pose = lambda: world_view_client.get_pose(point_name, world_view_folder
                                                  )

    #Begin tracking
    jogging_client.toggle_tracking(True)
    # Track for 20 seconds with a frequency of 50 Hz
    track_point(time_amount=20,
                frequency=50,
                point_name=point_name,
                get_pose=get_pose,
                jogging_client=jogging_client)

    # Stop tracking
    jogging_client.toggle_tracking(False)
    # Unregister the feedback function
    jogging_client.unregister(feedback_function)

    # Remove tracking pose
    world_view_client.remove_element(point_name, world_view_folder)
コード例 #2
0
    world_view_folder = "example_03_shifting"

    world_view_client = WorldViewClient()

    # Read joint values from world view 
    joint_values_dict = world_view_client.query_joint_values(
                              "{}/jointValues".format(world_view_folder))
    joint_values_list = list(joint_values_dict.values())
    # Read names too to associate the joint values to the poses
    names = list(map( lambda posix_path: posix_path.name, list(joint_values_dict.keys())))
    # Create no names for joint values
    new_names = list(map( lambda name: "shifted_joint_values_of_{}".format(name), names))


    # Read poses from world view 
    reference_pose = world_view_client.get_pose("Reference", 
                              "/{}/poses".format(world_view_folder))
    shift_pose = world_view_client.get_pose("Shift", 
                              "/{}/poses".format(world_view_folder))

    # Create a pose defining the shift from the reference_poses view 
    # First undo translation and rotation of reference pose
    # then translate and rotate by the shift pose
    diff_pose = Pose( -reference_pose.translation, reference_pose.quaternion.inverse)\
      .translate(shift_pose.translation).rotate(shift_pose.quaternion)
    
    shifted_joint_values_list = main(joint_values_list, diff_pose)
    # Save the shifted joint values to world view
    print("Save the shifted joint values to world view")
    try:
        # delete folder if it already exists
        world_view_client.remove_element("generated", world_view_folder)
            print("Placing element {}".format(i + 1))
            loop.run_until_complete(place(pre_place_jvs[i], place_jvs[i]))
    finally:
        loop.close()


if __name__ == '__main__':
    # Called when running this script standalone
    world_view_folder = "example_02_palletizing/example_pick_place_poses"

    move_group = example_utils.get_move_group()

    world_view_client = WorldViewClient()

    # Read poses from world view
    pose_1 = world_view_client.get_pose("Pose_1", world_view_folder)
    pose_2 = world_view_client.get_pose("Pose_2", world_view_folder)
    pose_3 = world_view_client.get_pose("Pose_3", world_view_folder)
    # Read poses from world view
    joint_values_1 = world_view_client.get_joint_values(
        "JointValues_1", world_view_folder)
    joint_values_2 = world_view_client.get_joint_values(
        "JointValues_2", world_view_folder)
    joint_values_3 = world_view_client.get_joint_values(
        "JointValues_3", world_view_folder)
    home = world_view_client.get_joint_values("Home", world_view_folder)
    end_effector = example_utils.get_move_group().get_end_effector()
    poses = [pose_1, pose_2, pose_3]
    joint_values = [joint_values_1, joint_values_2, joint_values_3]
    main(poses, joint_values, home)
コード例 #4
0
def main():
    # create entities which should be added to and manipulated in world view
    joint_set1 = JointSet(['Joint1', 'Joint2', 'Joint3'])

    joint_values1 = JointValues(joint_set1, [1.0, 2.0, 3.0])

    joint_set2 = JointSet(['Joint1', 'Joint2'])
    joint_values2 = JointValues(joint_set2, [19.0, 20.0])

    joint_set3 = JointSet(['Joint1', 'Joint4'])
    joint_values3 = JointValues(joint_set3, [100.0, 30.0])

    t1 = [0.502522, 0.2580, 0.3670]
    q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666)

    t2 = [0.23795, 0.46845, 0.44505]
    q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096)

    t3 = [0.6089578, 0.3406782, 0.208865]
    q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387)

    pose1 = Pose(t1, q1, 'world')
    pose2 = Pose(t2, q2, 'world')
    pose3 = Pose(t3, q3, 'world')

    cartesian_path1 = CartesianPath([pose1, pose2, pose3])

    sphere = CollisionPrimitive.create_sphere(0.1, pose2)
    cylinder = CollisionPrimitive.create_cylinder(0.4, 0.05, pose2)
    box = CollisionPrimitive.create_box(0.2, 0.2, 0.1, pose1)

    plane = CollisionPrimitive.create_plane(1.0, 1.0, 1.0, 1.0, pose3)
    cone = CollisionPrimitive.create_cone(0.2, 0.2, pose3)

    collision_object1 = CollisionObject([box])
    collision_object2 = CollisionObject([sphere, cylinder])
    collision_object3 = CollisionObject([plane, cone])

    # create a instance of WorldViewClient to get access to rosvita world view
    world_view_client = WorldViewClient()

    print('---------------- add folder --------------')
    # add folder test at WorldView root
    world_view_client.add_folder('test', '')
    # add folder test at WorldView root/test
    world_view_client.add_folder('joint_values', '/test')

    world_view_client.add_folder('poses', '/test')

    world_view_client.add_folder('cartesian_paths', '/test')

    world_view_client.add_folder('collision_objects', '/test')

    print('---------------- add joint values --------------')
    world_view_client.add_joint_values('joint_values1', 'test/joint_values',
                                       joint_values1)
    world_view_client.add_joint_values('joint_values2', 'test/joint_values',
                                       joint_values2)
    world_view_client.add_joint_values('joint_values3', 'test/joint_values',
                                       joint_values3)
    world_view_client.add_joint_values('test', 'test/joint_values',
                                       joint_values3)

    print('---------------- update joint values --------------')
    world_view_client.update_joint_values('joint_values1', 'test/joint_values',
                                          joint_values2)
    world_view_client.update_joint_values('joint_values2', 'test/joint_values',
                                          joint_values3)
    world_view_client.update_joint_values('joint_values3', 'test/joint_values',
                                          joint_values1)

    print('---------------- get joint values --------------')

    get_value = world_view_client.get_joint_values('joint_values1',
                                                   'test/joint_values')
    print('joint_values1 is: ' + str(get_value))

    print('---------------- query joint values --------------')
    # query all values which start with t under test/joint_values
    queried_values = world_view_client.query_joint_values(
        'test/joint_values', 't')

    print('---------------- add poses --------------')
    world_view_client.add_pose('pose1', 'test/poses', pose1)
    world_view_client.add_pose('pose2', 'test/poses', pose2)
    world_view_client.add_pose('pose3', 'test/poses', pose3)
    world_view_client.add_pose('test', 'test/poses', pose3)

    print('---------------- update pose --------------')
    world_view_client.update_pose('pose1', 'test/poses', pose2)
    world_view_client.update_pose('pose2', 'test/poses', pose3)
    world_view_client.update_pose('pose3', 'test/poses', pose1)

    print('---------------- get pose --------------')

    get_value = world_view_client.get_pose('pose1', 'test/poses')
    print('pose1 is: ' + str(get_value))

    print('---------------- query poses --------------')
    # query all poses which start with t under test/pose
    queried_values = world_view_client.query_poses('test/poses', 't')

    print('---------------- add cartesian path --------------')
    world_view_client.add_cartesian_path('cartesian_path1',
                                         'test/cartesian_paths',
                                         cartesian_path1)
    world_view_client.add_cartesian_path('test1', 'test/cartesian_paths',
                                         cartesian_path1)

    print('---------------- update cartesian path --------------')
    world_view_client.update_cartesian_path('cartesian_path1',
                                            'test/cartesian_paths',
                                            cartesian_path1)

    print('---------------- get cartesian path --------------')

    get_value = world_view_client.get_cartesian_path('cartesian_path1',
                                                     'test/cartesian_paths')
    print('cartesian_path1 is: ' + str(get_value))

    print('---------------- query cartesian paths --------------')
    # query all cartesian_paths which start with t under test/cartesian_path
    queried_values = world_view_client.query_cartesian_paths(
        'test/cartesian_paths', 't')

    for v in queried_values:
        print(str(v))

    print('---------------- add collision object --------------')
    world_view_client.add_collision_object('collision_object1',
                                           'test/collision_objects',
                                           collision_object1)
    world_view_client.add_collision_object('collision_object2',
                                           'test/collision_objects',
                                           collision_object2)
    world_view_client.add_collision_object('collision_object3',
                                           'test/collision_objects',
                                           collision_object3)
    world_view_client.add_collision_object('test1', 'test/collision_objects',
                                           collision_object1)

    print('---------------- update collision object --------------')
    world_view_client.update_collision_object('collision_object1',
                                              'test/collision_objects',
                                              collision_object1)

    print('---------------- get collision object --------------')

    get_value = world_view_client.get_collision_object(
        'collision_object1', 'test/collision_objects')
    print('collision_object1 is: ' + str(get_value))

    print('---------------- query collision object --------------')
    # query all collision_objects which start with t under test/collision_object
    queried_values = world_view_client.query_collision_objects(
        'test/collision_objects', 't')

    for v in queried_values:
        print(str(v))

    input('Press enter to clean up')

    print('----------------- remove folder test ---------------')
    world_view_client.remove_element('test', '')
コード例 #5
0
def main(xSize: int, ySize: int, xStepSize: float , yStepSize: float):
    """
    The parameter for this function describes the size of the grid.
    To manipulate orientation and position, one can alter the Pose named "GridPose" 
    in the folder "example_02_palletizing" of the world view.

    Parameters
    ----------
    xSize : int
        Number of elements of the grid in x-direction
    ySize : int
        Number of elements of the grid in y-direction
    xStepSize : float
        The distance between the poses in x-direction
    yStepSize : float
        The distance between the poses in y-direction
    """

    world_view_folder = "example_02_palletizing"
    # Create move group instance targeting the right arm of the robot
    move_group = example_utils.get_move_group()

    # Get the gripper attached at the right arm
    wsg_gripper = example_utils.get_gripper(move_group)

    # Create a instance of WorldViewClient to get access to rosvita world view
    world_view_client = WorldViewClient()

    # Generate the folders used by this example in world vew
    generate_folders(world_view_client)

    # Get the pose of the position which defines the location and rotation of the grid
    pose = world_view_client.get_pose("GridPose", world_view_folder)
    jv_home = world_view_client.get_joint_values("Home", world_view_folder)


    # Get the target place poses
    poses = example_02_1_generate_grid.main(pose, xSize, ySize, xStepSize, yStepSize)
    rotation = pose.quaternion

    # Calculate the orthogonal vector of the plane we want to span
    # Since we use [1,0,0] and [0,1,0] vectors to span the grid relatively to the 
    # pose orientation, [0,0,1] is orthogonal to the grid
    orthogonal = rotation.rotate(np.array([0,0,1]))

    # For visualization and possible collisions, add some boxes below the positions 
    # we want to visit
    getBoxPose = lambda pose : Pose(pose.translation + (orthogonal * (0.02)), pose.quaternion) 
    boxPoses = list(map(getBoxPose, poses))
    boxes = example_02_2_create_collision_boxes.main(boxPoses, (xStepSize*0.9, yStepSize*0.9, 0.01))
    world_view_client.add_collision_object("collision_matrix", 
                                    "/{}/generated/collision_objects".format(world_view_folder), 
                                    boxes)

    # Now calculate the pre place poses, which hover over the desired place poses
    # Since we want to access every pose in the grid "from above", we apply a 
    # translation orthogonal to the place poses for the pre place poses  
    #func = lambda pose : Pose(pose.translation + (orthogonal * (-0.1)), pose.quaternion) 
    func = lambda pose : Pose(pose.translation + (pose.quaternion.rotate(np.array([0,0,1])) * (-0.1)), pose.quaternion) 
    pre_place_poses = list(map( func , poses))

    # Now that we have all the poses we want to visit, we should find the 
    # corresponding joint values
    pre_place_jvs = calculate_pre_place_joint_values(pre_place_poses, 
                                                jv_home, 
                                                move_group, 
                                                world_view_client,
                                                world_view_folder) 

    example_02_4_pick_place_poses.main(poses, pre_place_jvs, jv_home, move_group)

    world_view_client.remove_element("generated", world_view_folder)