def connect_to_ros(request, doctest_namespace): if request.module.__name__ == 'compas_fab.robots.robot': doctest_namespace["robot"] = Robot() yield elif request.module.__name__ == 'compas_fab.robots.planning_scene': with RosClient() as client: robot = Robot(client) doctest_namespace["client"] = client doctest_namespace["robot"] = robot yield else: yield
def connect_to_ros(doctest_namespace): client = RosClient() client.run() robot = Robot(client) doctest_namespace["client"] = client doctest_namespace["robot"] = robot yield client.close()
def RunScript(self, ip, port, connect): ros_client = None ip = ip or '127.0.0.1' port = port or 9090 key = create_id(self, 'ros_client') ros_client = st.get(key, None) if ros_client: st[key].close() if connect: st[key] = RosClient(ip, port) st[key].run(5) ros_client = st.get(key, None) is_connected = ros_client.is_connected if ros_client else False return (ros_client, is_connected)
# Copy & Paste this exercise into a Grasshopper component # And add a `subscribe` input connected to a button import Rhino import scriptcontext as sc import roslibpy from compas_fab.backends import RosClient from compas_fab.backends.ros.messages.shape_msgs import Mesh from compas_ghpython.artists import MeshArtist if subscribe: def receive_mesh(message): mesh_message = Mesh.from_msg(message) artist = MeshArtist(mesh_message.mesh) sc.doc = Rhino.RhinoDoc.ActiveDoc sc.doc.Objects.AddMesh(artist.draw()) sc.doc = ghdoc client = RosClient() client.run() topic = roslibpy.Topic(client, '/meshes', 'shape_msgs/Mesh') topic.subscribe(receive_mesh)
""" Calculate the inverse kinematics of a robot based on the frame and a starting configuration. """ from ex23_load_robot import robot from compas.geometry import Frame from compas_fab.backends import RosClient from compas_fab.robots import Configuration robot.client = RosClient() robot.client.run() frame = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]) start_configuration = Configuration.from_revolute_values([0] * 6) group = "manipulator" # or robot.main_group_name response = robot.inverse_kinematics(frame, start_configuration, group, constraints=None, attempts=50) print(response.configuration) robot.client.close() robot.client.terminate()
def main(): parser = argparse.ArgumentParser() # ur_picknplace_multiple_piece parser.add_argument('-p', '--problem', default='ur_picknplace_single_piece', help='The name of the problem to solve') parser.add_argument('-rob', '--robot', default='ur3', help='The type of UR robot to use.') parser.add_argument('-m', '--plan_transit', action='store_false', help='Plans motions between each picking and placing') parser.add_argument('-v', '--viewer', action='store_true', help='Enables the viewer during planning (slow!)') parser.add_argument('-s', '--save_result', action='store_true', help='save planning results as a json file') parser.add_argument( '-scale', '--model_scale', default=0.001, help='model scale conversion to meter, default 0.001 (from millimeter)' ) parser.add_argument('-vik', '--view_ikfast', action='store_true', help='Visualize each ikfast solutions') parser.add_argument('-tres', '--transit_res', default=0.01, help='joint resolution (rad)') parser.add_argument('-ros', '--use_ros', action='store_true', help='use ros backend with moveit planners') parser.add_argument('-cart_ts', '--cartesian_time_step', default=0.1, help='cartesian time step in trajectory simulation') parser.add_argument('-trans_ts', '--transit_time_step', default=0.01, help='transition time step in trajectory simulation') parser.add_argument('-per_conf_step', '--per_conf_step', action='store_true', help='stepping each configuration in simulation') args = parser.parse_args() print('Arguments:', args) VIZ = args.viewer VIZ_IKFAST = args.view_ikfast TRANSITION_JT_RESOLUTION = float(args.transit_res) plan_transition = args.plan_transit use_moveit_planner = args.use_ros # sim settings CART_TIME_STEP = args.cartesian_time_step TRANSITION_TIME_STEP = args.transit_time_step PER_CONF_STEP = args.per_conf_step # transition motion planner settings RRT_RESTARTS = 5 RRT_ITERATIONS = 40 # choreo pkg settings choreo_problem_instance_dir = compas_fab.get('choreo_instances') unit_geos, static_obstacles = load_assembly_package( choreo_problem_instance_dir, args.problem, scale=args.model_scale) result_save_path = os.path.join( choreo_problem_instance_dir, 'results', 'choreo_result.json') if args.save_result else None # urdf, end effector settings if args.robot == 'ur3': # urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur3.urdf') urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur3_collision_viz.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur3_moveit_config/config/ur3.srdf') else: urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') urdf_pkg_name = 'ur_description' ee_filename = compas_fab.get( 'universal_robot/ur_description/meshes/' + 'pychoreo_workshop_gripper/collision/victor_gripper_jaw03.obj') # ee_sep_filename = compas_fab.get('universal_robot/ur_description/meshes/' + # 'pychoreo_workshop_gripper/collision/victor_gripper_jaw03_rough_sep.obj') # ee_decomp_file_dir = compas_fab.get('universal_robot/ur_description/meshes/' + # 'pychoreo_workshop_gripper/collision/decomp') # ee_decomp_file_prefix = 'victor_gripper_jaw03_decomp_' # decomp_parts_num = 36 client = RosClient() if use_moveit_planner else None # geometry file is not loaded here model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = RobotClass(model, semantics=semantics, client=client) group = robot.main_group_name base_link_name = robot.get_base_link_name() ee_link_name = robot.get_end_effector_link_name() ik_joint_names = robot.get_configurable_joint_names() # parse end effector mesh # ee_meshes = [Mesh.from_obj(os.path.join(ee_decomp_file_dir, ee_decomp_file_prefix + str(i) + '.obj')) for i in range(decomp_parts_num)] ee_meshes = [Mesh.from_obj(ee_filename)] # ee_meshes = [Mesh.from_obj(ee_sep_filename)] # define TCP transformation tcp_tf = Translation([0.099, 0, 0]) # in meters ur5_start_conf = [0, -1.65715, 1.71108, -1.62348, 0, 0] if use_moveit_planner: # TODO: attach end effector to the robot in planning scene # https://github.com/compas-dev/compas_fab/issues/66 scene = PlanningScene(robot) scene.remove_all_collision_objects() client.set_joint_positions(group, ik_joint_names, ur5_start_conf) else: scene = None # add static collision obstacles co_dict = {} for i, static_obs_mesh in enumerate(static_obstacles): # offset the table a bit... cm = CollisionMesh(static_obs_mesh, 'so_' + str(i), frame=Frame.from_transformation( Translation([0, 0, -0.02]))) if use_moveit_planner: scene.add_collision_mesh(cm) else: co_dict[cm.id] = {} co_dict[cm.id]['meshes'] = [cm.mesh] co_dict[cm.id]['mesh_poses'] = [cm.frame] if use_moveit_planner: # See: https://github.com/compas-dev/compas_fab/issues/63#issuecomment-519525879 time.sleep(1) co_dict = scene.get_collision_meshes_and_poses() # ====================================================== # ====================================================== # start pybullet environment & load pybullet robot connect(use_gui=VIZ) pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name, planning_scene=scene, ee_link_name=ee_link_name) ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot, ee_link_name) # update current joint conf and attach end effector pb_ik_joints = joints_from_names(pb_robot, ik_joint_names) pb_end_effector_link = link_from_name(pb_robot, ee_link_name) if not use_moveit_planner: set_joint_positions(pb_robot, pb_ik_joints, ur5_start_conf) for e_at in ee_attachs: e_at.assign() # draw TCP frame in pybullet if has_gui(): TCP_pb_pose = get_TCP_pose(pb_robot, ee_link_name, tcp_tf, return_pb_pose=True) handles = draw_pose(TCP_pb_pose, length=0.04) # wait_for_user() # deliver ros collision meshes to pybullet static_obstacles_from_name = convert_meshes_and_poses_to_pybullet_bodies( co_dict) # for now... for so_key, so_val in static_obstacles_from_name.items(): static_obstacles_from_name[so_key] = so_val[0] for unit_name, unit_geo in unit_geos.items(): geo_bodies = [] for sub_id, mesh in enumerate(unit_geo.mesh): geo_bodies.append(convert_mesh_to_pybullet_body(mesh)) unit_geo.pybullet_bodies = geo_bodies # check collision between obstacles and element geometries assert not sanity_check_collisions(unit_geos, static_obstacles_from_name) # from random import shuffle seq_assignment = list(range(len(unit_geos))) # shuffle(seq_assignment) element_seq = {seq_id: e_id for seq_id, e_id in enumerate(seq_assignment)} # for key, val in element_seq.items(): # # element_seq[key] = 'e_' + str(val) # element_seq[key] = val if has_gui(): for e_id in element_seq.values(): # for e_body in brick_from_index[e_id].body: set_pose(e_body, brick_from_index[e_id].goal_pose) handles.extend( draw_pose(unit_geos[e_id].initial_pb_pose, length=0.02)) handles.extend(draw_pose(unit_geos[e_id].goal_pb_pose, length=0.02)) for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) print('pybullet env loaded.') # wait_for_user() for h in handles: remove_debug(h) saved_world = WorldSaver() ik_fn = ikfast_ur3.get_ik if args.robot == 'ur3' else ikfast_ur5.get_ik tot_traj, graph_sizes = \ direct_ladder_graph_solve_picknplace(pb_robot, ik_joint_names, base_link_name, ee_link_name, ik_fn, unit_geos, element_seq, static_obstacles_from_name, tcp_transf=pb_pose_from_Transformation(tcp_tf), ee_attachs=ee_attachs, max_attempts=100, viz=VIZ_IKFAST, st_conf=ur5_start_conf) picknplace_cart_plans = divide_nested_list_chunks(tot_traj, graph_sizes) saved_world.restore() print('Cartesian planning finished.') # reset robot and parts for better visualization set_joint_positions(pb_robot, pb_ik_joints, ur5_start_conf) for ee in ee_attachs: ee.assign() for e_id in element_seq.values(): for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) # if has_gui(): # wait_for_user() def flatten_unit_geos_bodies(in_dict): out_list = [] for ug in in_dict.values(): out_list.extend(ug.pybullet_bodies) return out_list if plan_transition: print('Transition planning started.') for seq_id, unit_picknplace in enumerate(picknplace_cart_plans): print('----\ntransition seq#{}'.format(seq_id)) e_id = element_seq[seq_id] if seq_id != 0: tr_start_conf = picknplace_cart_plans[seq_id - 1]['place_retreat'][-1] else: tr_start_conf = ur5_start_conf # obstacles=static_obstacles + cur_mo_list place2pick_st_conf = list(tr_start_conf) place2pick_goal_conf = list( picknplace_cart_plans[seq_id]['pick_approach'][0]) # assert not client.is_joint_state_colliding(group, ik_joint_names, place2pick_st_conf) # assert not client.is_joint_state_colliding(group, ik_joint_names, place2pick_goal_conf) if use_moveit_planner: # TODO: add collision objects st_conf = Configuration.from_revolute_values( place2pick_st_conf) goal_conf = Configuration.from_revolute_values( place2pick_goal_conf) goal_constraints = robot.constraints_from_configuration( goal_conf, [math.radians(1)] * 6, group) place2pick_jt_traj = robot.plan_motion(goal_constraints, st_conf, group, planner_id='RRTConnect') place2pick_path = [ jt_pt['values'] for jt_pt in place2pick_jt_traj.to_data()['points'] ] else: saved_world = WorldSaver() set_joint_positions(pb_robot, pb_ik_joints, place2pick_st_conf) for ee_a in ee_attachs: ee_a.assign() place2pick_path = plan_joint_motion( pb_robot, pb_ik_joints, place2pick_goal_conf, attachments=ee_attachs, obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), self_collisions=True, resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ) saved_world.restore() if not place2pick_path: saved_world = WorldSaver() print('****\nseq #{} cannot find place2pick transition'. format(seq_id)) print('Diagnosis...') cfn = get_collision_fn_diagnosis(pb_robot, pb_ik_joints, \ obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), attachments=ee_attachs, self_collisions=True) print('start pose:') cfn(place2pick_st_conf) print('end pose:') cfn(place2pick_goal_conf) saved_world.restore() print('Diagnosis over') pick2place_st_conf = picknplace_cart_plans[seq_id]['pick_retreat'][ -1] pick2place_goal_conf = picknplace_cart_plans[seq_id][ 'place_approach'][0] if use_moveit_planner: st_conf = Configuration.from_revolute_values( picknplace_cart_plans[seq_id]['pick_retreat'][-1]) goal_conf = Configuration.from_revolute_values( picknplace_cart_plans[seq_id]['place_approach'][0]) goal_constraints = robot.constraints_from_configuration( goal_conf, [math.radians(1)] * 6, group) pick2place_jt_traj = robot.plan_motion(goal_constraints, st_conf, group, planner_id='RRTConnect') pick2place_path = [ jt_pt['values'] for jt_pt in pick2place_jt_traj.to_data()['points'] ] else: saved_world = WorldSaver() # create attachement without needing to keep track of grasp... set_joint_positions( pb_robot, pb_ik_joints, picknplace_cart_plans[seq_id]['pick_retreat'][0]) # attachs = [Attachment(robot, tool_link, invert(grasp.attach), e_body) for e_body in brick.body] element_attachs = [create_attachment(pb_robot, pb_end_effector_link, e_body) \ for e_body in unit_geos[e_id].pybullet_bodies] set_joint_positions(pb_robot, pb_ik_joints, pick2place_st_conf) for ee_a in ee_attachs: ee_a.assign() for e_a in element_attachs: e_a.assign() pick2place_path = plan_joint_motion( pb_robot, pb_ik_joints, pick2place_goal_conf, obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), attachments=ee_attachs + element_attachs, self_collisions=True, resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ) saved_world.restore() if not pick2place_path: saved_world = WorldSaver() print('****\nseq #{} cannot find pick2place transition'. format(seq_id)) print('Diagnosis...') cfn = get_collision_fn_diagnosis(pb_robot, pb_ik_joints, obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), \ attachments=ee_attachs + element_attachs, self_collisions=True) print('start pose:') cfn(pick2place_st_conf) print('end pose:') cfn(pick2place_goal_conf) saved_world.restore() print('Diagnosis over') picknplace_cart_plans[seq_id]['place2pick'] = place2pick_path picknplace_cart_plans[seq_id]['pick2place'] = pick2place_path for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].goal_pb_pose) if seq_id == len(picknplace_cart_plans) - 1: saved_world = WorldSaver() set_joint_positions( pb_robot, pb_ik_joints, picknplace_cart_plans[seq_id]['place_retreat'][-1]) for ee_a in ee_attachs: ee_a.assign() return2idle_path = plan_joint_motion( pb_robot, pb_ik_joints, ur5_start_conf, obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), attachments=ee_attachs, self_collisions=True, resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints), restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, ) saved_world.restore() picknplace_cart_plans[seq_id]['return2idle'] = return2idle_path print('Transition planning finished.') # convert to ros JointTrajectory traj_json_data = [] traj_time_count = 0.0 for i, element_process in enumerate(picknplace_cart_plans): e_proc_data = {} for sub_proc_name, sub_process in element_process.items(): sub_process_jt_traj_list = [] for jt_sol in sub_process: sub_process_jt_traj_list.append( JointTrajectoryPoint(values=jt_sol, types=[0] * 6, time_from_start=Duration( traj_time_count, 0))) traj_time_count += 1.0 # meaningless timestamp e_proc_data[sub_proc_name] = JointTrajectory( trajectory_points=sub_process_jt_traj_list, start_configuration=sub_process_jt_traj_list[0]).to_data() traj_json_data.append(e_proc_data) if result_save_path: with open(result_save_path, 'w+') as outfile: json.dump(traj_json_data, outfile, indent=4) print('planned trajectories saved to {}'.format(result_save_path)) print('\n*************************\nplanning completed. Simulate?') if has_gui(): wait_for_user() for e_id in element_seq.values(): for e_body in unit_geos[e_id].pybullet_bodies: set_pose(e_body, unit_geos[e_id].initial_pb_pose) display_picknplace_trajectories(pb_robot, ik_joint_names, ee_link_name, unit_geos, traj_json_data, \ ee_attachs=ee_attachs, cartesian_time_step=CART_TIME_STEP, transition_time_step=TRANSITION_TIME_STEP, step_sim=True, per_conf_step=PER_CONF_STEP) if use_moveit_planner: scene.remove_all_collision_objects()
import time from roslibpy import Message from roslibpy import Topic from compas_fab.backends import RosClient client = RosClient() talker = Topic(client, '/chatter', 'std_msgs/String') def start_talking(): while client.is_connected: talker.publish(Message({'data': 'Hello RobArch World!'})) print('Sending message...') time.sleep(1) talker.unadvertise() client.on_ready(start_talking) client.run_forever()
default='localhost') parser.add_argument('-p', '--ros-port', help='ROS port', default=9090) parser.add_argument('-f', '--frequency', help='Frequency to publish joint states (in HZ)', default=5) args = parser.parse_args() print('Starting UR joint state publisher...') print('Press CTRL+C to abort') print( 'NOTE: if CTRL+C does not abort cleanly, set the env variable to FOR_DISABLE_CONSOLE_CTRL_HANDLER=1' ) print('ROS Nodes status:') client = RosClient(host=args.ros_host, port=int(args.ros_port)) print(' [ ] ROS Topics', end='\r') joint_states_topic = roslibpy.Topic(client, '/ur_joint_states', 'sensor_msgs/JointState') joint_states_topic.advertise() client.on_ready( functools.partial(joint_states_publisher, topic=joint_states_topic, frequency=args.frequency)) print(' [X] ROS Topics') print('Ready!') client.run_forever()
import time from roslibpy import Message from roslibpy import Topic from compas_fab.backends import RosClient client = RosClient() listener = Topic(client, '/chatter', 'std_msgs/String') def start_listening(): listener.subscribe(receive_message) def receive_message(message): print('Heard talking: ' + message['data']) client.on_ready(start_listening) client.run_forever()
# Before running this example, make sure to run # "docker compose up" on the docker/moveit folder import compas from compas_fab.backends import RosClient from compas_rhino.artists import RobotModelArtist # Set high precision to import meshes defined in meters compas.PRECISION = '12f' # Load robot and its geometry with RosClient('localhost') as ros: robot = ros.load_robot(load_geometry=True) robot.artist = RobotModelArtist(robot.model, layer='COMPAS::Robot Viz') robot.artist.clear_layer() robot.artist.draw()
def std_move_to_frame( frame, tool="tool0", wobj="wobj0", motion_type=Motion.LINEAR, speed=200, accel=100, zone=Zone.FINE, timeout=None, ): """Move robot arm to target or targets. Parameters ---------- frame : :class:`compas.geometry.Frame` or :obj:`list` of :class:`compas.geometry.Frame` Target frame or frames. tool : :obj:`str` Name of tool as named in RAPID code on controller to use for TCP data. wobj : :obj:`str` Name of work object as named in RAPID code on controller to use for coordinate system. motion_type : :class:`compas_rrc.Motion` Motion type, either linear (:class:`~compas_rrc.Motion.LINEAR`) or joint (:class:`~compas_rrc.Motion.JOINT`). speed : :obj:`float` TCP speed in mm/s. Limited by hard coded max speed in this function as well as safety systems on controller. accel : :obj:`float` Acceleration in percentage of standard acceleration. zone : :class:`compas_rrc.Zone` Set zone value of movement, (acceptable deviation from target). timeout : :obj:`float` Time to wait for indication of finished movement. If not defined no feedback will be requested. """ # noqa: E501 if not isinstance(frame, Sequence): frames = [frame] else: frames = frame with RosClient() as ros: # Create ABB Client abb = AbbClient(ros) abb.run(timeout=5) abb.send(SetTool(tool)) abb.send(SetWorkObject(wobj)) # Set acceleration data ramp = 100 # % Higher values makes the acceleration ramp longer abb.send(SetAcceleration(accel, ramp)) # Set speed override and max speed speed_override = 100 # % max_tcp_speed = 500 # mm/s abb.send(SetMaxSpeed(speed_override, max_tcp_speed)) # Move to Frame for f in frames: abb.send( PrintText(" Moving to {:.3f}, {:.3f}, {:.3f}.".format( *f.point.data))) abb.send(MoveToFrame(f, speed, zone, motion_type)) if timeout: abb.send_and_wait(Noop(), timeout=timeout)
) print('UR Server starting...', end='\r') server = Server(args.server_address, args.server_port) server.start() server.client_ips.update({'UR': args.ur_address}) print('UR Server started. ') print('Waiting for UR client...', end='\r') ur = ClientWrapper("UR") ur.wait_for_connected() print('UR client connected. Ready to process tasks.') print('ROS Nodes status:') print(' [ ] ROS Services', end='\r') client = RosClient(host=args.ros_host, port=int(args.ros_port)) storage_service = roslibpy.Service(client, '/store_assembly_task', 'workshop_tum_msgs/AssemblyTask') storage_service.advertise(storage_handler) execute_service = roslibpy.Service(client, '/execute_assembly_task', 'workshop_tum_msgs/AssemblyTask') execute_service.advertise(functools.partial(execution_handler, ur=ur)) print(' [X] ROS Services') client.on_ready( functools.partial(joint_states_received, ur=ur, ros=client, frequency=args.frequency))
import json from compas_fab.backends import RosClient from roslibpy import Topic from helpers import get_current_buffer from helpers import get_input print('Enter your name:') name = get_input() client = RosClient('ros-vzby-dzht.yaler.io', port=80) def start_chat(): print('Connected as {}! [type a message or q to quit]'.format(name)) talker = Topic(client, '/messages', 'std_msgs/String') talker.advertise() listener = Topic(client, '/messages', 'std_msgs/String') listener.subscribe(receive_message) while client.is_connected: text = get_input() if text == 'q': break send_message(talker, text) client.terminate()
def main(): choreo_problem_instance_dir = compas_fab.get('choreo_instances') result_save_path = os.path.join(choreo_problem_instance_dir, 'results', 'choreo_result.json') with open(result_save_path, 'r') as f: json_data = json.loads(f.read()) traj_time_cnt = 0.0 max_jt_vel = 0.2 max_jt_acc = 0.1 last_jt_pt = None # pybullet traj preview settings pybullet_preview = True PB_VIZ_CART_TIME_STEP = 0.05 PB_VIZ_TRANS_TIME_STEP = 0.04 PB_VIZ_PER_CONF_SIM = False urdf_filename = compas_fab.get( 'universal_robot/ur_description/urdf/ur5.urdf') srdf_filename = compas_fab.get( 'universal_robot/ur5_moveit_config/config/ur5.srdf') urdf_pkg_name = 'ur_description' ee_filename = compas_fab.get( 'universal_robot/ur_description/meshes/' + 'pychoreo_workshop_gripper/collision/victor_gripper_jaw03.obj') # [0.0, -94.94770102010436, 98.0376624092449, -93.01855212389889, 0.0, 0.0] # UR=192.168.0.30, Linux=192.168.0.1, Windows=192.168.0.2 # the following host IP should agree with the Linux machine host_ip = '192.168.0.1' if REAL_EXECUTION else 'localhost' with RosClient(host=host_ip, port=9090) as client: client.on_ready( lambda: print('Is ROS connected?', client.is_connected)) # get current configuration listener = roslibpy.Topic(client, JOINT_TOPIC_NAME, 'sensor_msgs/JointState') msg_getter = MsgGetter() listener.subscribe(msg_getter.receive_msg) time.sleep(2) last_seen_state = msg_getter.get_msg() print('current jt state: {}'.format(last_seen_state['position'])) model = RobotModel.from_urdf_file(urdf_filename) semantics = RobotSemantics.from_srdf_file(srdf_filename, model) robot = RobotClass(model, semantics=semantics, client=client) group = robot.main_group_name joint_names = robot.get_configurable_joint_names() # base_link_name = robot.get_base_link_name() ee_link_name = robot.get_end_effector_link_name() if pybullet_preview: from conrob_pybullet import connect from compas_fab.backends.ros.plugins_choreo import display_trajectory_chunk from compas_fab.backends.pybullet import attach_end_effector_geometry, \ convert_mesh_to_pybullet_body, create_pb_robot_from_ros_urdf connect(use_gui=True) pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name, ee_link_name=ee_link_name) ee_meshes = [Mesh.from_obj(ee_filename)] ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot, ee_link_name) st_conf = Configuration.from_revolute_values( last_seen_state['position']) goal_conf = Configuration.from_revolute_values( json_data[0]['place2pick']['start_configuration']['values']) goal_constraints = robot.constraints_from_configuration( goal_conf, [math.radians(1)] * 6, group) init_traj_raw = robot.plan_motion(goal_constraints, st_conf, group, planner_id='RRTStar') init_traj = traj_reparam(init_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, init_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) print('************\nexecuting init transition') exec_jt_traj(client, joint_names, init_traj) print('executed?') input() for seq_id, e_process_data in enumerate(json_data): print('************\nexecuting #{} picknplace process'.format( seq_id)) # open gripper gripper_srv_call(client, state=0) print( '=====\nexecuting #{} place-retreat to pick-approach transition process' .format(seq_id)) traj_data = e_process_data['place2pick'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('=====\nexecuting #{} pick-approach to pick-grasp process'. format(seq_id)) traj_data = e_process_data['pick_approach'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('executed?') input() # close gripper gripper_srv_call(client, state=1) print('=====\nexecuting #{} pick-grasp to pick-retreat process'. format(seq_id)) traj_data = e_process_data['pick_retreat'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('executed?') input() print( '=====\nexecuting #{} pick-retreat to place-approach transition process' .format(seq_id)) traj_data = e_process_data['pick2place'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('executed?') input() print('=====\nexecuting #{} place-approach to place-grasp process'. format(seq_id)) traj_data = e_process_data['place_approach'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) # open gripper gripper_srv_call(client, state=0) print('executed?') input() print('=====\nexecuting #{} place-grasp to place-retreat process'. format(seq_id)) traj_data = e_process_data['place_retreat'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj) print('executed?') input() if 'return2idle' in json_data[-1]: print('=====\nexecuting #{} return-to-idle transition process'. format(seq_id)) traj_data = e_process_data['return2idle'] ros_jt_traj_raw = JointTrajectory.from_data(traj_data) ros_jt_traj = traj_reparam(ros_jt_traj_raw, max_jt_vel, max_jt_acc, inspect_sol=False) if pybullet_preview: display_trajectory_chunk(pb_robot, joint_names, ros_jt_traj.to_data(), \ ee_attachs=ee_attachs, grasped_attach=[], time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM) exec_jt_traj(client, joint_names, ros_jt_traj)
filepath) # Load assembly print('\n[ ] Loading assembly...', end='\r') assembly = Assembly.from_json(filepath) print('[OK] Assembly loaded: {} '.format(filename)) planned_elemement_keys = get_planned_element_keys(assembly) print('[OK] Found {} planned elements out of {} total elements'.format( len(planned_elemement_keys), assembly.number_of_elements())) print('[OK] Planned sequence: {}'.format(planned_elemement_keys)) # Connect client print('[ ] Connecting to ROS {}:{}'.format(args.ros_host, int(args.ros_port)), end='\r') with RosClient(host=args.ros_host, port=int(args.ros_port)) as client: print('[OK] Connected to ROS: \n') # Start UI loop to send elements while True: element_key = input_non_zero_number( 'Please enter the element key to send for execution: ') if element_key not in planned_elemement_keys: print( ' » Element not found, must be one of the planned element keys' ) continue send_element_trajectories(client, assembly, element_key)
# TODO: This _mesh_import should also add support for precision return _mesh_import(url, filename) if __name__ == "__main__": """ Start following processes on client side: roslaunch YOUR_ROBOT_moveit_config demo.launch rviz_tutorial:=true roslaunch rosbridge_server rosbridge_websocket.launch roslaunch file_server.launch """ import logging from compas.robots import RobotModel from compas_fab.backends import RosClient FORMAT = '%(asctime)-15s [%(levelname)s] %(message)s' logging.basicConfig(level=logging.DEBUG, format=FORMAT) with RosClient() as ros: local_directory = os.path.join(os.path.expanduser('~'), 'workspace', 'robot_description') importer = RosFileServerLoader(ros, local_cache=True, local_cache_directory=local_directory) importer.robot_name = 'abb_irb1600_6_12' urdf = importer.load_urdf() srdf = importer.load_srdf() model = RobotModel.from_urdf_string(urdf) model.load_geometry(importer) print(model)
import os import time from compas_fab.backends import RosClient from compas_fab.robots import PlanningScene from compas_fab.robots import Tool HERE = os.path.dirname(__file__) DATA = os.path.abspath(os.path.join(HERE, "..", "data")) tool = Tool.from_json(os.path.join(DATA, "vacuum_gripper.json")) with RosClient('localhost') as client: robot = client.load_robot() scene = PlanningScene(robot) # Attach the tool robot.attach_tool(tool) scene.add_attached_tool() time.sleep(3) # Remove the tool scene.remove_attached_tool() scene.remove_collision_mesh(tool.name) robot.detach_tool() time.sleep(1)
import time from roslibpy import Topic from compas_fab.backends import RosClient client = RosClient() client.run() topic = Topic(client, '/messages', 'std_msgs/String') topic.subscribe(print) while True: time.sleep(1) client.terminate()
from compas_fab.backends import RosClient from compas_fab.robots import Configuration with RosClient() as client: robot = client.load_robot() assert robot.name == 'ur5' configuration = Configuration.from_revolute_values( [-2.238, -1.153, -2.174, 0.185, 0.667, 0.]) frame_WCF = robot.forward_kinematics(configuration) print("Frame in the world coordinate system") print(frame_WCF)
from roslibpy import Topic from compas_fab.backends import RosClient def receive_message(message): print(message) if __name__ == '__main__': import argparse parser = argparse.ArgumentParser(description='Joint states publisher') parser.add_argument('-r', '--ros-host', help='ROS host', default='localhost') parser.add_argument('-p', '--ros-port', help='ROS port', default=9090) args = parser.parse_args() client = RosClient(host=args.ros_host, port=int(args.ros_port)) # Subscribe to joint state updates listener = Topic(client, '/ur_joint_states', 'sensor_msgs/JointState') listener.subscribe(receive_message) # Start connection client.on_ready(lambda: print('[OK] Connected to ROS: \n')) client.run_forever() client.terminate()