def test_config_merge(): config = Configuration(values=[1, 2, 3], types=[Joint.REVOLUTE] * 3, joint_names=['a', 'b', 'c']) other_config = Configuration(values=[3, 2, 0], types=[Joint.REVOLUTE] * 3, joint_names=['a', 'b', 'd']) config.merge(other_config) assert config.joint_dict == {'a': 3, 'b': 2, 'c': 3, 'd': 0}
def to_rlf_robot_full_conf(robot11_confval, robot12_confval, scale=1e-3): # convert to full configuration of the RFL robot robot21_confval = [38000, 0, -4915, 0, 0, 0, 0, 0, 0] robot22_confval = [-12237, -4915, 0, 0, 0, 0, 0, 0] return Configuration( values=( *[robot11_confval[i] * scale for i in range(0, 3)], *[rad(robot11_confval[i]) for i in range(3, 9)], *[robot12_confval[i] * scale for i in range(0, 2)], *[rad(robot12_confval[i]) for i in range(2, 8)], # below fixed *[robot21_confval[i] * 1e-3 for i in range(0, 3)], *[rad(robot21_confval[i]) for i in range(3, 9)], *[robot22_confval[i] * 1e-3 for i in range(0, 2)], *[rad(robot22_confval[i]) for i in range(2, 8)], ), types=(2, 2, 2, 0, 0, 0, 0, 0, 0, 2, 2, 0, 0, 0, 0, 0, 0, 2, 2, 2, 0, 0, 0, 0, 0, 0, 2, 2, 0, 0, 0, 0, 0, 0), joint_names=('bridge1_joint_EA_X', 'robot11_joint_EA_Y', 'robot11_joint_EA_Z', 'robot11_joint_1', 'robot11_joint_2', 'robot11_joint_3', 'robot11_joint_4', 'robot11_joint_5', 'robot11_joint_6', 'robot12_joint_EA_Y', 'robot12_joint_EA_Z', 'robot12_joint_1', 'robot12_joint_2', 'robot12_joint_3', 'robot12_joint_4', 'robot12_joint_5', 'robot12_joint_6', 'bridge2_joint_EA_X', 'robot21_joint_EA_Y', 'robot21_joint_EA_Z', 'robot21_joint_1', 'robot21_joint_2', 'robot21_joint_3', 'robot21_joint_4', 'robot21_joint_5', 'robot21_joint_6', 'robot22_joint_EA_Y', 'robot22_joint_EA_Z', 'robot22_joint_1', 'robot22_joint_2', 'robot22_joint_3', 'robot22_joint_4', 'robot22_joint_5', 'robot22_joint_6'))
def test_scale(): values = [1.5, 1., 2.] types = [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR] config = Configuration(values, types) config.scale(1000.) assert config.values == [1.5, 1000., 2000.]
def plan_moving_and_placing_motion(robot, element, start_configuration, tolerance_vector, safelevel_vector, attached_element_mesh): """Returns two trajectories for moving and placing an element. Parameters ---------- robot : :class:`compas.robots.Robot` element : :class:`Element` start_configuration : :class:`Configuration` tolerance_vector : :class:`Vector` safelevel_vector : :class:`Vector` attached_element_mesh : :class:`AttachedCollisionMesh` Returns ------- list of :class:`JointTrajectory` """ tolerance_position = 0.001 tolerance_axes = [math.radians(1)] * 3 target_frame = element._tool_frame.copy() target_frame.point += tolerance_vector safelevel_target_frame = target_frame.copy() safelevel_target_frame.point += safelevel_vector # Calculate goal constraints safelevel_target_frame_tool0 = robot.from_tcf_to_t0cf( [safelevel_target_frame])[0] goal_constraints = robot.constraints_from_frame(safelevel_target_frame_tool0, tolerance_position, tolerance_axes) moving_trajectory = robot.plan_motion(goal_constraints, start_configuration, options=dict( planner_id='SBL', attached_collision_meshes=[attached_element_mesh], num_planning_attempts=20, allowed_planning_time=10 )) frames = [safelevel_target_frame, target_frame] frames_tool0 = robot.from_tcf_to_t0cf(frames) # as start configuration take last trajectory's end configuration last_configuration = Configuration(moving_trajectory.points[-1].values, moving_trajectory.points[-1].types) placing_trajectory = robot.plan_cartesian_motion(frames_tool0, last_configuration, options=dict( max_step=0.01, attached_collision_meshes=[attached_element_mesh] )) return moving_trajectory, placing_trajectory
def convert_to_trajectory(response): trajectory = JointTrajectory() trajectory.source_message = response trajectory.fraction = response.fraction trajectory.points = convert_trajectory_points( response.solution.joint_trajectory.points, joint_types) trajectory.start_configuration = Configuration( response.start_state.joint_state.position, start_configuration.types) callback(trajectory)
def convert_to_trajectory(response): trajectory = JointTrajectory() trajectory.source_message = response trajectory.fraction = 1. trajectory.points = convert_trajectory_points( response.trajectory.joint_trajectory.points, joint_types) trajectory.start_configuration = Configuration( response.trajectory_start.joint_state.position, start_configuration.types) trajectory.planning_time = response.planning_time callback(trajectory)
def convert_to_trajectory(response): trajectory = JointTrajectory() trajectory.source_message = response trajectory.fraction = 1. trajectory.joint_names = response.trajectory.joint_trajectory.joint_names trajectory.planning_time = response.planning_time joint_types = robot.get_joint_types_by_names(trajectory.joint_names) trajectory.points = convert_trajectory_points( response.trajectory.joint_trajectory.points, joint_types) start_state = response.trajectory_start.joint_state start_state_types = robot.get_joint_types_by_names(start_state.name) trajectory.start_configuration = Configuration(start_state.position, start_state_types) callback(trajectory)
def convert_to_trajectory(response): try: trajectory = JointTrajectory() trajectory.source_message = response trajectory.fraction = response.fraction trajectory.joint_names = response.solution.joint_trajectory.joint_names joint_types = [joint_type_by_name[name] for name in trajectory.joint_names] trajectory.points = convert_trajectory_points( response.solution.joint_trajectory.points, joint_types) start_state = response.start_state.joint_state start_state_types = [joint_type_by_name[name] for name in start_state.name] trajectory.start_configuration = Configuration(start_state.position, start_state_types, start_state.name) callback(trajectory) except Exception as e: errback(e)
def test_cast_to_str(): config = Configuration([pi / 2, 3., 0.1], [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR]) assert str(config) == 'Configuration((1.571, 3.000, 0.100), (0, 2, 5))'
def test_collision_checker(rfl_setup, itj_beam_cm, viewer, diagnosis): # modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py urdf_filename, semantics = rfl_setup move_group = 'robot11_eaXYZ' # 'robot12_eaYZ' with PyChoreoClient(viewer=viewer) as client: cprint(urdf_filename, 'green') with LockRenderer(): robot = client.load_robot(urdf_filename) robot.semantics = semantics robot_uid = client.get_robot_pybullet_uid(robot) ik_joint_names = robot.get_configurable_joint_names(group=move_group) ik_joint_types = robot.get_joint_types_by_names(ik_joint_names) flange_link_name = robot.get_end_effector_link_name(group=move_group) ee_touched_link_names = ['robot12_tool0', 'robot12_link_6'] # ee_acms = [AttachedCollisionMesh(ee_cm, flange_link_name, ee_touched_link_names) for ee_cm in itj_TC_PG500_cms] # beam_acm = AttachedCollisionMesh(itj_beam_cm, flange_link_name, ee_touched_link_names) draw_pose(unit_pose(), length=1.) cam = rfl_camera() set_camera_pose(cam['target'], cam['location']) ik_joints = joints_from_names(robot_uid, ik_joint_names) flange_link = link_from_name(robot_uid, flange_link_name) # robot_base_link = link_from_name(robot_uid, base_link_name) r11_start_conf_vals = np.array([22700.0, 0.0, -4900.0, \ 0.0, -80.0, 65.0, 65.0, 20.0, -20.0]) r12_start_conf_vals = np.array([-4056.0883789999998, -4000.8486330000001, \ 0.0, -22.834741999999999, -30.711554, 0.0, 57.335655000000003, 0.0]) full_start_conf = to_rlf_robot_full_conf(r11_start_conf_vals, r12_start_conf_vals) # full_joints = joints_from_names(robot_uid, full_start_conf.joint_names) client.set_robot_configuration(robot, full_start_conf) # # * add attachment # for ee_acm in ee_acms: # client.add_attached_collision_mesh(ee_acm, options={'robot' : robot, 'mass': 1}) # client.add_attached_collision_mesh(beam_acm, options={'robot' : robot, 'mass': 1}) # safe start conf start_confval = np.hstack([ r11_start_conf_vals[:3] * 1e-3, np.radians(r11_start_conf_vals[3:]) ]) conf = Configuration(values=start_confval.tolist(), types=ik_joint_types, joint_names=ik_joint_names) assert not client.check_collisions( robot, conf, options={'diagnosis': diagnosis}) # TODO add more tests from pybullet_planning import get_link_subtree, prune_fixed_joints, clone_body, get_movable_joints, get_all_links, \ set_color, child_link_from_joint first_joint = ik_joints[0] target_link = flange_link # selected_links = get_link_subtree(robot_uid, first_joint) # TODO: child_link_from_joint? selected_links = [child_link_from_joint(joint) for joint in ik_joints] selected_movable_joints = prune_fixed_joints(robot_uid, selected_links) assert (target_link in selected_links) selected_target_link = selected_links.index(target_link) sub_robot = clone_body(robot_uid, links=selected_links, visual=True, collision=True) # TODO: joint limits sub_movable_joints = get_movable_joints(sub_robot) # conf = Configuration(values=[0.]*6, types=ik_joint_types, joint_names=ik_joint_names) # assert not client.configuration_in_collision(conf, group=move_group, options={'diagnosis':True}) wait_if_gui()
def test_ctor(): values = [1., 3., 0.1] types = [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR] config = Configuration(values, types) assert config.values == values assert config.types == types
from math import pi from compas.robots import Joint from compas_fab.robots import Configuration print(Joint.REVOLUTE) print(Joint.PRISMATIC) print(Joint.FIXED) values = [0] * 6 types = [Joint.REVOLUTE] * 6 config = Configuration(values, types) config = Configuration([pi / 2, 3., 0.1], [Joint.REVOLUTE, Joint.PRISMATIC, Joint.PLANAR]) config = Configuration.from_revolute_values([pi / 2, 0., 0., pi / 2, pi, 0]) config = Configuration.from_prismatic_and_revolute_values( [8.312], [pi / 2, 0., 0., 0., 2 * pi, 0.8])
def test_plan_motion(abb_irb4600_40_255_setup, itj_TC_g1_cms, itj_beam_cm, column_obstacle_cm, base_plate_cm, itj_tool_changer_grasp_transf, itj_gripper_grasp_transf, itj_beam_grasp_transf, tool_type, itj_tool_changer_urdf_path, itj_g1_urdf_path, viewer, diagnosis): # modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py urdf_filename, semantics = abb_irb4600_40_255_setup move_group = 'bare_arm' ee_touched_link_names = ['link_6'] with PyChoreoClient(viewer=viewer) as client: with LockRenderer(): robot = client.load_robot(urdf_filename) robot.semantics = semantics client.disabled_collisions = robot.semantics.disabled_collisions if tool_type == 'static': for _, ee_cm in itj_TC_g1_cms.items(): client.add_collision_mesh(ee_cm) else: client.add_tool_from_urdf('TC', itj_tool_changer_urdf_path) client.add_tool_from_urdf('g1', itj_g1_urdf_path) # * add static obstacles client.add_collision_mesh(base_plate_cm) client.add_collision_mesh(column_obstacle_cm) ik_joint_names = robot.get_configurable_joint_names(group=move_group) ik_joint_types = robot.get_joint_types_by_names(ik_joint_names) flange_link_name = robot.get_end_effector_link_name(group=move_group) tool0_tf = Transformation.from_frame( client.get_link_frame_from_name(robot, flange_link_name)) tool0_from_tool_changer_base = itj_tool_changer_grasp_transf tool0_from_gripper_base = itj_gripper_grasp_transf client.set_object_frame( '^{}'.format('TC'), Frame.from_transformation(tool0_tf * tool0_from_tool_changer_base)) client.set_object_frame( '^{}'.format('g1'), Frame.from_transformation(tool0_tf * tool0_from_gripper_base)) names = client._get_collision_object_names('^{}'.format('g1')) + \ client._get_collision_object_names('^{}'.format('TC')) for ee_name in names: attach_options = {'robot': robot} if tool_type == 'actuated': attached_child_link_name = 'toolchanger_base' if 'TC' in ee_name else 'gripper_base' attach_options.update( {'attached_child_link_name': attached_child_link_name}) client.add_attached_collision_mesh(AttachedCollisionMesh( CollisionMesh(None, ee_name), flange_link_name, touch_links=ee_touched_link_names), options=attach_options) #* attach beam client.add_collision_mesh(itj_beam_cm) tool0_tf = Transformation.from_frame( client.get_link_frame_from_name(robot, flange_link_name)) tool0_from_beam_base = itj_beam_grasp_transf client.set_object_frame( '^{}$'.format('itj_beam_b2'), Frame.from_transformation(tool0_tf * tool0_from_beam_base)) client.add_attached_collision_mesh(AttachedCollisionMesh( CollisionMesh(None, 'itj_beam_b2'), flange_link_name, touch_links=[]), options={'robot': robot}) wait_if_gui('beam attached.') vals = [ -1.4660765716752369, -0.22689280275926285, 0.27925268031909273, 0.17453292519943295, 0.22689280275926285, -0.22689280275926285 ] start_conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) # client.set_robot_configuration(robot, start_conf) # wait_if_gui() # vals = [0.05235987755982989, -0.087266462599716474, -0.05235987755982989, 1.7104226669544429, 0.13962634015954636, -0.43633231299858238] vals = [ 0.034906585039886591, 0.68067840827778847, 0.15707963267948966, -0.89011791851710809, -0.034906585039886591, -2.2514747350726849 ] end_conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) # client.set_robot_configuration(robot, end_conf) # wait_if_gui() plan_options = {'diagnosis': True, 'resolutions': 0.05} goal_constraints = robot.constraints_from_configuration( end_conf, [0.01], [0.01], group=move_group) st_time = time.time() trajectory = client.plan_motion(robot, goal_constraints, start_configuration=start_conf, group=move_group, options=plan_options) print('Solving time: {}'.format(elapsed_time(st_time))) if trajectory is None: cprint('Client motion planner CANNOT find a plan!', 'red') # assert False, 'Client motion planner CANNOT find a plan!' # TODO warning else: cprint('Client motion planning find a plan!', 'green') wait_if_gui('Start sim.') time_step = 0.03 for traj_pt in trajectory.points: client.set_robot_configuration(robot, traj_pt) wait_for_duration(time_step) wait_if_gui("Finished.")
)) assert (trajectory1.fraction == 1.) # ========================================================================== key = 0 element = assembly.element(key) print("Calculating free-space path to safe vector above target frame...") # 2. Calulate a free-space motion to the safelevel_target_frame safelevel_target_frame_tool0 = robot.from_tcf_to_t0cf( [safelevel_target_frame])[0] goal_constraints = robot.constraints_from_frame( safelevel_target_frame_tool0, tolerance_position, tolerance_axes) # as start configuration take last trajectory's end configuration start_configuration = Configuration(trajectory1.points[-1].values, trajectory1.points[-1].types) trajectory2 = robot.plan_motion(goal_constraints, start_configuration, options=dict( planner_id='RRT', attached_collision_meshes=[brick_acm])) # 3. Calculate a cartesian motion to the target_frame print("Calculating cartesian path for placement...") frames = [safelevel_target_frame, target_frame] # as start configuration take last trajectory's end configuration start_configuration = Configuration(trajectory2.points[-1].values, trajectory2.points[-1].types) trajectory3 = robot.plan_cartesian_motion( robot.from_tcf_to_t0cf(frames), start_configuration,
def test_collision_checker(abb_irb4600_40_255_setup, itj_TC_g1_cms, itj_beam_cm, column_obstacle_cm, base_plate_cm, itj_tool_changer_grasp_transf, itj_gripper_grasp_transf, itj_beam_grasp_transf, tool_type, itj_tool_changer_urdf_path, itj_g1_urdf_path, viewer, diagnosis): # modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py urdf_filename, semantics = abb_irb4600_40_255_setup move_group = 'bare_arm' ee_touched_link_names = ['link_6'] with PyChoreoClient(viewer=viewer) as client: with LockRenderer(): robot = client.load_robot(urdf_filename) robot.semantics = semantics client.disabled_collisions = robot.semantics.disabled_collisions if tool_type == 'static': for _, ee_cm in itj_TC_g1_cms.items(): client.add_collision_mesh(ee_cm) else: client.add_tool_from_urdf('TC', itj_tool_changer_urdf_path) client.add_tool_from_urdf('g1', itj_g1_urdf_path) # * add static obstacles client.add_collision_mesh(base_plate_cm) client.add_collision_mesh(column_obstacle_cm) ik_joint_names = robot.get_configurable_joint_names(group=move_group) ik_joint_types = robot.get_joint_types_by_names(ik_joint_names) flange_link_name = robot.get_end_effector_link_name(group=move_group) tool0_tf = Transformation.from_frame( client.get_link_frame_from_name(robot, flange_link_name)) tool0_from_tool_changer_base = itj_tool_changer_grasp_transf tool0_from_gripper_base = itj_gripper_grasp_transf client.set_object_frame( '^{}'.format('TC'), Frame.from_transformation(tool0_tf * tool0_from_tool_changer_base)) client.set_object_frame( '^{}'.format('g1'), Frame.from_transformation(tool0_tf * tool0_from_gripper_base)) names = client._get_collision_object_names('^{}'.format('g1')) + \ client._get_collision_object_names('^{}'.format('TC')) for ee_name in names: attach_options = {'robot': robot} if tool_type == 'actuated': attached_child_link_name = 'toolchanger_base' if 'TC' in ee_name else 'gripper_base' attach_options.update( {'attached_child_link_name': attached_child_link_name}) client.add_attached_collision_mesh(AttachedCollisionMesh( CollisionMesh(None, ee_name), flange_link_name, touch_links=ee_touched_link_names), options=attach_options) # client._print_object_summary() # wait_if_gui('EE attached.') if tool_type == 'actuated': # lower 0.0008 upper 0.01 tool_bodies = client._get_bodies('^{}'.format('itj_PG500')) tool_conf = Configuration( values=[0.01, 0.01], types=[Joint.PRISMATIC, Joint.PRISMATIC], joint_names=['joint_gripper_jaw_l', 'joint_gripper_jaw_r']) for b in tool_bodies: client._set_body_configuration(b, tool_conf) wait_if_gui('Open') tool_conf = Configuration( values=[0.0008, 0.0008], types=[Joint.PRISMATIC, Joint.PRISMATIC], joint_names=['joint_gripper_jaw_l', 'joint_gripper_jaw_r']) for b in tool_bodies: client._set_body_configuration(b, tool_conf) wait_if_gui('Close') cprint('safe start conf', 'green') conf = Configuration(values=[0.] * 6, types=ik_joint_types, joint_names=ik_joint_names) assert not client.check_collisions( robot, conf, options={'diagnosis': diagnosis}) cprint('joint over limit', 'cyan') conf = Configuration(values=[0., 0., 1.5, 0, 0, 0], types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('attached gripper-obstacle collision - column', 'cyan') vals = [ -0.33161255787892263, -0.43633231299858238, 0.43633231299858238, -1.0471975511965976, 0.087266462599716474, 0.0 ] # conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) # client.set_robot_configuration(robot, conf) # wait_if_gui() assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) #* attach beam client.add_collision_mesh(itj_beam_cm) tool0_tf = Transformation.from_frame( client.get_link_frame_from_name(robot, flange_link_name)) tool0_from_beam_base = itj_beam_grasp_transf client.set_object_frame( '^{}$'.format('itj_beam_b2'), Frame.from_transformation(tool0_tf * tool0_from_beam_base)) client.add_attached_collision_mesh(AttachedCollisionMesh( CollisionMesh(None, 'itj_beam_b2'), flange_link_name, touch_links=[]), options={'robot': robot}) # wait_if_gui('beam attached.') cprint('attached beam-robot body self collision', 'cyan') vals = [ 0.73303828583761843, -0.59341194567807209, 0.54105206811824214, -0.17453292519943295, 1.064650843716541, 1.7278759594743862 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('attached beam-obstacle collision - column', 'cyan') vals = [ 0.087266462599716474, -0.19198621771937624, 0.20943951023931956, 0.069813170079773182, 1.2740903539558606, 0.069813170079773182 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('attached beam-obstacle collision - ground', 'cyan') vals = [ -0.017453292519943295, 0.6108652381980153, 0.20943951023931956, 1.7627825445142729, 1.2740903539558606, 0.069813170079773182 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('robot link-obstacle collision - column', 'cyan') vals = [ -0.41887902047863912, 0.20943951023931956, 0.20943951023931956, 1.7627825445142729, 1.2740903539558606, 0.069813170079773182 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('robot link-obstacle collision - ground', 'cyan') vals = [ 0.33161255787892263, 1.4660765716752369, 0.27925268031909273, 0.17453292519943295, 0.22689280275926285, 0.54105206811824214 ] conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert client.check_collisions(robot, conf, options={'diagnosis': diagnosis}) cprint('Sweeping collision', 'cyan') vals = [ -0.12217304763960307, -0.73303828583761843, 0.83775804095727824, -2.4609142453120048, 1.2391837689159739, -0.85521133347722145 ] conf1 = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert not client.check_collisions( robot, conf1, options={'diagnosis': diagnosis}) # wait_if_gui() vals = [ -0.12217304763960307, -0.73303828583761843, 0.83775804095727824, -2.4958208303518914, -1.5533430342749532, -0.85521133347722145 ] conf2 = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names) assert not client.check_collisions( robot, conf2, options={'diagnosis': diagnosis}) # wait_if_gui() assert client.check_sweeping_collisions(robot, conf1, conf2, options={ 'diagnosis': diagnosis, 'line_width': 3.0 }) wait_if_gui("Finished.")
# Units: # - Revolute joint : radiants # - Prismatic joint: meters import math from compas.robots.model import Joint from compas_fab.robots import Configuration print('Default constructor') print(Configuration([math.pi, 4], [Joint.REVOLUTE, Joint.PRISMATIC])) print( Configuration([math.pi, 4], [Joint.REVOLUTE, Joint.PRISMATIC], ['joint_1', 'ext_axis_1'])) print() print('Construct from revolute values') print(Configuration.from_revolute_values([math.pi, 0])) print(Configuration.from_revolute_values([math.pi, 0], ['joint_1', 'joint_2'])) print() print('Construct from prismatic & revolute values') print(Configuration.from_prismatic_and_revolute_values([4], [math.pi])) print( Configuration.from_prismatic_and_revolute_values( [4], [math.pi], ['ext_axis_1', 'joint_1'])) print() print('Merge two configurations') ext_axes = Configuration([4], [Joint.PRISMATIC], ['ext_axis_1']) arm_joints = Configuration([math.pi], [Joint.REVOLUTE], ['joint_1']) full_cfg = ext_axes.merged(arm_joints)
# create cylinder in yz plane radius, length = 0.3, 5 cylinder = Cylinder(Circle(Plane([0, 0, 0], [1, 0, 0]), radius), length) cylinder.transform(Translation([length / 2., 0, 0])) mesh = Mesh.from_shape(cylinder) # create robot robot = RobotModel("robot", links=[], joints=[]) link0 = robot.add_link("world") # Add all other links to robot for count in range(1, 8): # add new link to robot robot.add_link("link" + str(count), visual_mesh=mesh.copy(), visual_color=(count * 0.72 % 1.0, count * 0.23 % 1.0 , 0.6)) # add the joint between the last two links axis = (0, 0, 1) origin = Frame((length , 0, 0), (1, 0, 0), (0, 1, 0)) robot.add_joint("joint" + str(count), Joint.CONTINUOUS, robot.links[-2] , robot.links[-1], origin, axis) artist = RobotArtist(robot) artist.clear_layer() joint_names = robot.get_configurable_joint_names() joint_values = [0.2] * len(joint_names) joint_types = [Joint.CONTINUOUS] * len(joint_names) config = Configuration(joint_values, joint_types) artist.update(config, joint_names) artist.draw_visual()
def inverse_kinematics(self, robot, frame_WCF, start_configuration=None, group=None, options=None): """Calculate the robot's inverse kinematic for a given frame. Parameters ---------- robot : :class:`compas_fab.robots.Robot` The robot instance for which inverse kinematics is being calculated. frame_WCF: :class:`compas.geometry.Frame` The frame to calculate the inverse for. start_configuration: :class:`compas_fab.robots.Configuration`, optional If passed, the inverse will be calculated such that the calculated joint positions differ the least from the start_configuration. Defaults to the init configuration. group: str, optional The planning group used for calculation. Defaults to the robot's main planning group. options: dict, optional Dictionary containing the following key-value pairs: - ``"base_link"``: (:obj:`str`) Name of the base link. - ``"avoid_collisions"``: (:obj:`bool`, optional) Whether or not to avoid collisions. Defaults to `True`. - ``"constraints"``: (:obj:`list` of :class:`compas_fab.robots.Constraint`, optional) A set of constraints that the request must obey. Defaults to `None`. - ``"attempts"``: (:obj:`int`, optional) The maximum number of inverse kinematic attempts. Defaults to `8`. - ``"attached_collision_meshes"``: (:obj:`list` of :class:`compas_fab.robots.AttachedCollisionMesh`, optional) Defaults to `None`. - ``"return_all"``: (bool, optional) return a list of all computed ik solutions Defaults to False Raises ------ compas_fab.backends.exceptions.BackendError If no configuration can be found. Returns ------- :class:`compas_fab.robots.Configuration` The planning group's configuration. """ max_iterations = is_valid_option(options, 'attempts', 8) avoid_collisions = is_valid_option(options, 'avoid_collisions', True) return_all = is_valid_option(options, 'return_all', False) custom_limits = options.get('custom_limits') or {} # return_closest_to_start = is_valid_option(options, 'return_closest_to_start', False) # cull = is_valid_option(options, 'cull', True) robot_uid = robot.attributes['pybullet_uid'] ik_joint_names = robot.get_configurable_joint_names(group=group) ik_joints = joints_from_names(robot_uid, ik_joint_names) tool_link_name = robot.get_end_effector_link_name(group=group) tool_link = link_from_name(robot_uid, tool_link_name) pb_custom_limits = get_custom_limits( robot_uid, ik_joints, custom_limits={ joint_from_name(robot_uid, jn): lims for jn, lims in custom_limits.items() }) target_pose = pose_from_frame(frame_WCF) with WorldSaver(): if start_configuration is not None: start_conf_vals = start_configuration.values set_joint_positions(robot_uid, ik_joints, start_conf_vals) # else: # sample_fn = get_sample_fn(robot_uid, ik_joints) # start_conf_vals = sample_fn() # if group not in self.client.planner.ik_fn_from_group: # use default ik fn conf_vals = self._compute_ik(robot_uid, ik_joints, tool_link, target_pose, max_iterations, custom_limits=pb_custom_limits) joint_types = robot.get_joint_types_by_names(ik_joint_names) configurations = [Configuration(values=conf_val, types=joint_types, joint_names=ik_joint_names) \ for conf_val in conf_vals if conf_val is not None] # else: # # qs = client.inverse_kinematics(frame_WCF, group=move_group) # configurations = self.client.planner.ik_fn_from_group[group](frame_WCF, group=group, options=options) if avoid_collisions: configurations = [ conf for conf in configurations if not self.client.check_collisions( robot, conf, options=options) ] if return_all: return configurations else: return configurations[0] if len(configurations) > 0 else None
# 1. Add a collison mesh to the planning scene: floor, desk, etc. for cm in scene_collision_meshes: scene.add_collision_mesh(cm) if not LOAD_FROM_EXISTING: scene.remove_collision_mesh('assembly') # 2. Compute picking trajectory picking_trajectory = plan_picking_motion(robot, picking_frame, safelevel_picking_frame, picking_configuration, attached_element_mesh) # 3. Save the last configuration from that trajectory as new # start_configuration start_configuration = Configuration(picking_trajectory.points[-1].values, picking_trajectory.points[-1].types) sequence = [key for key in assembly.network.nodes()] sequence = list(range(10)) print(sequence) exclude_keys = [ vkey for vkey in assembly.network.nodes_where({'is_planned': True}) ] sequence = [k for k in sequence if k not in exclude_keys] print(sequence) for key in sequence: print("=" * 30 + "\nCalculating path for element with key %d." % key) element = assembly.element(key)