async def main(): move_group = example_utils.get_move_group() end_effector = move_group.get_end_effector() pose = end_effector.get_current_pose() # Create a cartesian path of the same poses path = [] for i in range(100): path.append(pose) cartesian_path = CartesianPath(path) # This applies a sinus trajectory an amplitude of 5 mm with 10 periods # over the path on the y axis target = add_sin_to_trajectory(cartesian_path=cartesian_path, amplitude=0.005, frequency=1 / 10, axis=1) # This applies a sinus trajectory an amplitude of 2 mm with 100 periods # over the path on the z axis target = add_sin_to_trajectory(target, amplitude=0.002, frequency=1 / 100, axis=2) # Move the end effector according to the now sinus shaped path await end_effector.move_poses_linear(target, max_deviation=0.05)
def setup_class(cls): cls.client = WorldViewClient() joint_set = JointSet('1,2,3') cls.joint_values_1 = JointValues(joint_set, 0.0) cls.joint_values_2 = JointValues(joint_set, [1, 2, 3]) cls.pose_1 = Pose.identity() cls.pose_2 = Pose.identity().translate([0.1, 0.2, 1.8]) cls.pose_3 = Pose.identity().translate([0.1, -0.2, 1.8]) cls.cartesian_path_1 = CartesianPath([cls.pose_1, cls.pose_2]) cls.cartesian_path_2 = CartesianPath([cls.pose_2, cls.pose_1]) collision_box_1 = CollisionPrimitive.create_box( 0.05, 0.05, 0.05, cls.pose_1 ) collision_box_2 = CollisionPrimitive.create_box( 0.05, 0.05, 0.05, cls.pose_2 ) collision_box_3 = CollisionPrimitive.create_box( 0.05, 0.05, 0.05, cls.pose_3 ) cls.collision_object_1 = CollisionObject([collision_box_1]) cls.collision_object_2 = CollisionObject( [collision_box_2, collision_box_3]) cls.folder_path = 'test/my_test' cls.joint_values_path = 'test_joint_values' cls.pose_path = 'test_pose' cls.cartesian_path = 'test_cartesian_path' cls.collision_object_path = 'test_collision_object'
async def imitate_move(end_effector, move_group): input( "Press enter to set the current position as reference start point, and start to listen to poses from client" ) start = end_effector.get_current_pose() with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: s.bind((HOST, PORT)) s.listen() while True: conn = None try: conn, addr = s.accept() with conn: while True: data = conn.recv(1024) if not data: break pose_list = eval(data.decode("utf-8")) # print(pose_list) path = [] for pose in pose_list: coordinate = pose[:3] # The following line should be changed according to current pose and the frame of robot base coordinate[0], coordinate[1], coordinate[ 2] = coordinate[2], coordinate[0], coordinate[ 1] coordinate = np.array( coordinate) + start.translation quat_element = pose[3:7] # The following line should be changed to match the previous line of coordinate quat = Quaternion(quat_element[0], quat_element[3], quat_element[1], quat_element[2]) quat = quat * start.quaternion pose = Pose(coordinate, quat) path.append(pose) cartesian_path = CartesianPath(path) joint_path = end_effector.inverse_kinematics_many( cartesian_path, False).path move_joints = move_group.move_joints(joint_path) move_joints = move_joints.with_velocity_scaling(0.1) move_joints_plan = move_joints.plan() await move_joints_plan.execute_async() except KeyboardInterrupt: if conn: conn.close() break
async def place(jv_pre_place: JointValues, place: Pose) -> None: """ This asynchronous function lets the robot move from home to every place position and back in a "pick and place" manner The movement from pre place JointValues to place Pose and back is done by moving the end effector in a linear fashion in cartesian space. The movement is planned an executed on the fly. Parameters ---------- jv_pre_place : JointValues The pre place configuration jv_place : JointValues The place configuration """ end_effector = move_group.get_end_effector() # Creates a joint path over the joint values to the target pose joint_path = JointPath(home.joint_set, [home, jv_pre_place ]) # Move over the joints to target pose await move_group.move_joints_collision_free(joint_path) pre_place_pose = end_effector.compute_pose(jv_pre_place) await end_effector.move_poses_linear(CartesianPath([pre_place_pose, place]), velocity_scaling = 0.05) # do something, e.g. place an object await end_effector.move_poses_linear(CartesianPath([place, pre_place_pose]), velocity_scaling = 0.05) # Creates a joint path over the joint values to the home pose joint_path_back = JointPath(home.joint_set, [jv_pre_place, home ]) # Move back over the joint path await move_group.move_joints_collision_free(joint_path_back)
def new_path(move_group, end_effector, view_num): joint_values = [] poses = [] for i in range(int(view_num)): inst = input( "Move Robot to viewpoint %d, and press Enter to continue..." % i) poses.append(end_effector.get_current_pose()) joint_values.append(move_group.get_current_joint_positions()) path = { 'poses': CartesianPath(poses), 'joint_values': JointPath(joint_values[0].joint_set, joint_values) } print("successfully created the new path") return path
def main(poses: List[Pose], seed: JointValues, end_effector: EndEffector) \ -> List[JointValues]: """ Calculates joint values from poses and a seed Calling inverse_kinematics_many with "const_seed = True" lets us exclusively using the "seed" joint values as seed. Parameters ---------- poses : List[Pose] A list of poses for which the joint values should be calculated seed : JointValues The seed to be used end_effector: EndEffector Returns ------ List[JointValues] A list of joint values for every pose Exceptions ------ Exception Raised when there was one or more poses for which the inverse kinematics operation failed """ cartesian_path = CartesianPath(poses) ik_results = end_effector.inverse_kinematics_many(cartesian_path, collision_check=True, seed=seed, const_seed=True) # Check if a configuration has been found for every pose if not ik_results.succeeded: raise Exception( "The inverse kinematics operation could not be applied on all the positions." ) # joint_path now contains the result of the ik-operation return ik_results.path
def main(): # create move group instance move_group = MoveGroup() # get default endeffector of the movegroup end_effector = move_group.get_end_effector() # create cartesian path and equivalent joint path 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) pose_1 = Pose(t1, q1) pose_2 = Pose(t2, q2) pose_3 = Pose(t3, q3) cartesian_path = CartesianPath([pose_1, pose_2, pose_3]) plan = end_effector.move_cartesian(cartesian_path, collision_check=True).plan() stepped_client = plan.execute_supervised() robot_chat = RobotChatClient() robot_chat_stepped_motion = RobotChatSteppedMotion(robot_chat, stepped_client, move_group.name) loop = asyncio.get_event_loop() register_asyncio_shutdown_handler(loop) try: loop.run_until_complete( robot_chat_stepped_motion.handle_stepwise_motions()) finally: loop.close()
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([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/test world_view_client.add_folder('test/joint_values') world_view_client.add_folder('test/poses') world_view_client.add_folder('test/cartesian_paths') world_view_client.add_folder('test/collision_objects') print('---------------- add joint values --------------') world_view_client.add_joint_values( 'test/joint_values/joint_values1', joint_values1 ) world_view_client.add_joint_values( 'test/joint_values/test', joint_values2 ) print('---------------- update joint values --------------') world_view_client.add_joint_values( 'test/joint_values/joint_values1', joint_values2 ) print('---------------- get joint values --------------') get_value = world_view_client.get_joint_values( 'test/joint_values/joint_values1' ) 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' ) for v in queried_values: print(str(v)) print('---------------- add poses --------------') world_view_client.add_pose( 'test/poses/pose1', pose1 ) world_view_client.add_pose( 'test/poses/test', pose3 ) print('---------------- update pose --------------') world_view_client.add_pose( 'test/poses/pose1', pose2 ) print('---------------- get pose --------------') get_value = world_view_client.get_pose( 'test/poses/pose1' ) 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') for v in queried_values: print(str(v)) print('---------------- add cartesian path --------------') world_view_client.add_cartesian_path( 'test/cartesian_paths/cartesian_path1', cartesian_path1 ) world_view_client.add_cartesian_path( 'test/cartesian_paths/test', cartesian_path1 ) print('---------------- update cartesian path --------------') world_view_client.add_cartesian_path( 'test/cartesian_paths/cartesian_path1', cartesian_path1 ) print('---------------- get cartesian path --------------') get_value = world_view_client.get_cartesian_path( 'test/cartesian_paths/cartesian_path1' ) 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( 'test/collision_objects/collision_object1', collision_object1 ) world_view_client.add_collision_object( 'test/collision_objects/test', collision_object1 ) print('---------------- update collision object --------------') world_view_client.add_collision_object( 'test/collision_objects/collision_object1', collision_object2 ) print('---------------- get collision object --------------') get_value = world_view_client.get_collision_object( 'test/collision_objects/collision_object1' ) 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', '')
async def test_run(move_group, path, view_num): new_path = path while 1: command = input("Enter command:") if command == "e": print("--- moving to the start point ---") move_joints_cf = move_group.move_joints_collision_free( new_path['joint_values'][0], velocity_scaling=0.2) await move_joints_cf.plan().execute_async() print('start test run') temp_path = new_path['joint_values'] move_joints_cf = move_group.move_joints_collision_free(temp_path) move_joints_cf = move_joints_cf.with_velocity_scaling(0.2) await move_joints_cf.plan().execute_async() print('test run finished') elif command.isdigit(): num = int(command) if num < int(view_num): view_points = new_path['joint_values'] print('--- Going to Viewpoint %d---' % num) move_joints_cf = move_group.move_joints_collision_free( view_points[num], velocity_scaling=0.2) await move_joints_cf.plan().execute_async() else: print('There are %s viewpoints, please enter a valid number' % view_num) elif command == 's': name = input("Enter the name of the path: ") if not name: name = "default" file_path = open('paths/%s.obj' % name, 'wb') pickle.dump(new_path, file_path) print('saved as %s.obj' % name) break elif command == 'l': name = input("Enter the name of the path you want to load: ") if not name: name = "default" file_path = open('paths/%s.obj' % name, 'r+b') move_group = MoveGroup() end_effector = move_group.get_end_effector() path = pickle.load(file_path) joint_values = [] poses = [] for i in range(len(path['poses'])): poses.append(path['poses'][i]) joint_values.append(path['joint_values'][i]) pos = input( "Enter the name of the position you want to overwrite: ") input( "Move Robot to the new position, and press Enter to continue.") poses[int(pos)] = end_effector.get_current_pose() joint_values[int(pos)] = move_group.get_current_joint_positions() print(path) path2 = { 'poses': CartesianPath(poses), 'joint_values': JointPath(joint_values[0].joint_set, joint_values) } print(path2) print("successfully created the new path") pickle.dump(path2, file_path) print('saved as %s.obj' % name) break elif command == 'd': break else: print("""usage: - e: execute a test run of path - number: go to specified viewpoint - s: save the path to local directory - d: discard the path""")
def main(): # create move group instance move_group = MoveGroup() # get default endeffector of the movegroup end_effector = move_group.get_end_effector() # create cartesian path and equivalent joint path 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) pose_1 = Pose(t1, q1) pose_2 = Pose(t2, q2) pose_3 = Pose(t3, q3) cartesian_path = CartesianPath([pose_1, pose_2, pose_3]) joint_path = end_effector.inverse_kinematics_many(cartesian_path, False).path loop = asyncio.get_event_loop() register_asyncio_shutdown_handler(loop) async def example_moves(): print('test MoveGroup class') print('---------------- move joints -------------------') move_joints = move_group.move_joints(joint_path) move_joints = move_joints.with_velocity_scaling(0.1) move_joints_plan = move_joints.plan() await move_joints_plan.execute_async() print('---------------- move joints supervised -------------------') move_joints = move_joints.with_velocity_scaling(0.5) stepped_motion_client = move_joints.plan().execute_supervised() await run_supervised(stepped_motion_client) print('---------------- move joints collision free -------------------') move_joints_cf = MoveJointsCollisionFreeOperation( move_joints.to_args()) await move_joints_cf.plan().execute_async() print('test EndEffector class') print('---------------- move cartesian -------------------') move_cartesian = end_effector.move_cartesian(cartesian_path) move_cartesian = move_cartesian.with_velocity_scaling(0.4) move_cartesian_plan = move_cartesian.plan() await move_cartesian_plan.execute_async() print('---------------- move cartesian supervised -------------------') move_cartesian = move_cartesian.with_velocity_scaling(0.8) stepped_motion_client = move_cartesian.plan().execute_supervised() await run_supervised(stepped_motion_client) print('---------------- move cartesian collision free -------------------') move_cartesian_cf = MoveCartesianCollisionFreeOperation( move_cartesian.to_args() ) await move_cartesian_cf.plan().execute_async() try: loop.run_until_complete(example_moves()) finally: loop.close()
def add_sin_to_trajectory(cartesian_path: CartesianPath, amplitude: float, frequency: float, axis: int) -> CartesianPath: """ Applies a translation to every pose in the cartesian path in form of a sinus shape Parameters ---------- cartesian_path : CartesianPath The CartesianPath object to be altered amplitude : float The amplitude of the sinus function in mm (the maximal deviationfrom the pose) frequency : float The frequency of the sinus function (how long a period takes relatively to the path length) axis: int The axis on which the translation should be applied Returns ------ CartesianPath The cartesian_path altered by a sinus shaped translation """ if axis < 0 or axis > 2: msg = 'axis can be x=0, y=1, z=2 but has value: {}'.format(axis) raise ValueError(msg) def sin_generator(number_of_samples: int, amplitude: float, frequency: float): """ This generator calculates values for an index according to a sinus function defined by the parameters. It yields the current index and the corresponding sinus value Parameters ---------- number_of_samples : int The number of samples amplitude : float The amplitude of the sinus function in mm frequency : float The frequency of the sinus function (how long a period takes relatively to the path length) axis: int The axis on which the translation should be applied """ for sample_index in range(number_of_samples): # Calculate the angle at index sample_index angle = 1 / frequency * (sample_index * 360.0) / number_of_samples # Yield a tuple of index and the sinus value of the angle, multiplied by the amplitude yield sample_index, amplitude * math.sin(math.radians(angle)) new_path = [] new_translation = np.asarray([0.0, 0.0, 0.0]) number_of_samples = len(cartesian_path) # For every pose, get the translation value of the sinus shaped movement for i, s in sin_generator(number_of_samples, amplitude, frequency): # Apply it only to one axis new_translation[axis] = s # Apply the translation to corresponding pose new_pose = cartesian_path.points[i].translate(new_translation) new_path.append(new_pose) return CartesianPath(new_path)