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)
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)
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', '')
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)