def test_revolute_ctor(): q = [4.5, 1.7, 0.5, 2.1, 0.1, 2.1] config = Configuration.from_revolute_values(q) assert config.types == [Joint.REVOLUTE] * 6 config = Configuration.from_revolute_values([pi / 2, 0., 0.]) assert to_degrees(config.values) == [90, 0, 0]
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 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 config_from_vrep(list_of_floats, scale): # compas_fab uses radians and meters, just like V-REP, # so in general, scale should always be 1 radians = list_of_floats[3:] prismatic_values = map(lambda v: v * scale, list_of_floats[0:3]) return Configuration.from_prismatic_and_revolute_values( prismatic_values, radians)
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_to_data(): config = Configuration.from_prismatic_and_revolute_values( [8.312], [1.5, 0., 0., 0., 1., 0.8]) # types=[Joint.PRISMATIC, Joint.REVOLUTE])) data = config.to_data() assert data['values'] == [8.312, 1.5, 0., 0., 0., 1., 0.8] assert data['types'] == [Joint.PRISMATIC] + [Joint.REVOLUTE] * 6
def compas_fab_ik_fn(frame_rcf, sampled=[]): tool_pose = pose_from_frame(frame_rcf) conf_vals = compute_inverse_kinematics(ikfast_ik_fn, tool_pose, sampled=sampled) return [ Configuration.from_revolute_values(q) for q in conf_vals if q is not None and len(q) > 0 ]
def from_joint_trajectory( cls, joint_trajectory ): # type: (JointTrajectory) -> MinimalTrajectory """Construct a instance from a :class:`compas_fab.robots.JointTrajectory`.""" conf_list = [ Configuration.from_revolute_values(pt.values) for pt in joint_trajectory.points ] return cls(conf_list)
def calculate_configurations_for_path(frames, robot, current_positions=[]): """Calculate possible configurations for a path. Args: frames (Frame): the path described with frames Returns: configurations: list of list of float """ configurations = [] for i, frame in enumerate(frames): configs = robot.inverse_kinematics(frame) qsols = [c.values for c in configs] if not len(qsols): return [] if i == 0: if len(current_positions): qsols_formatted = [] for jp_a in qsols: jp_a_formatted = format_joint_positions( jp_a, current_positions) qsols_formatted.append(jp_a_formatted) configurations.append(qsols_formatted) else: configurations.append(qsols) else: previous_qsols = configurations[-1][:] qsols_sorted = [] for jp_b in previous_qsols: diffs = [] qsols_formatted = [] for jp_a in qsols: jp_a_formatted = format_joint_positions(jp_a, jp_b) qsols_formatted.append(jp_a_formatted) diffs.append( sum([ math.fabs(qa - qb) for qa, qb in zip(jp_a_formatted, jp_b) ])) selected_idx = diffs.index(min(diffs)) qsols_sorted.append(qsols_formatted[selected_idx]) configurations.append(qsols_sorted) print(len(configurations)) print(len(configurations[0])) configurations = list(zip(*configurations)) print(len(configurations)) for i in range(len(configurations)): configurations[i] = list(configurations[i]) for j, q in enumerate(configurations[i]): configurations[i][j] = Configuration.from_revolute_values(q) return configurations
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 configuration_from_json(): # define filepath filename = "assignment_03.json" directory = "C:/Users/trtku/OneDrive/Data/03_MAS/01_github/compas_fs2021/compas_2_local/lecture_04/ko_tsuruta" filepath = os.path.join(directory, filename) # translate json into deserialisable data desirial_data = read_data_from_json(filepath) # get configurations from json desirial_configs = [ Configuration.from_data(data) for data in desirial_data ] return desirial_configs
def inverse_kinematics(self, tool0_frame_RCS): """Inverse kinematics function. Args: tool0_frame_RCS (:class:`Frame`): The tool0 frame to reach in robot coordinate system (RCS). Returns: configurations (:obj:`list` of :class:`Configuration`): A list of possible configurations. """ solutions = inverse_kinematics(tool0_frame_RCS, self.params) configurations = [] for joint_values in solutions: configurations.append( Configuration.from_revolute_values(joint_values)) return configurations
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 __init__( self, rob_conf: confuse.AttrDict, pick_station: PickStation, ros_port: int = 9090, ): super().__init__(ros_port=ros_port) self.rob_conf = rob_conf self.pick_place_tool = rob_conf.tools.get("pick_place") self.wobjs = rob_conf.wobjs self.global_speed_accel = rob_conf.robot_movement.global_speed_accel self.joint_positions = rob_conf.robot_movement.joint_positions self.speed = rob_conf.robot_movement.speed self.zone = rob_conf.robot_movement.zone self.docker_cfg = rob_conf.docker # Setup pick station self.pick_station = pick_station if hasattr(self.joint_positions, "travel_trajectory"): joint_positions = [ to_radians(jp) for jp in self.joint_positions.travel_trajectory ] configurations = [ Configuration.from_revolute_values(jps) for jps in joint_positions ] trajectory = MinimalTrajectory(configurations) trajectories = MinimalTrajectories([trajectory]) self.default_travel_trajectories = trajectories self.default_return_travel_trajectories = ( trajectories.reversed_recursively()) log.debug( f"default_travel_trajectories: {self.default_travel_trajectories}" )
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 get_config_or_pose(self, config_values_or_plane): """Parses multiple input data types and returns a configuration or a pose represented as a transformation matrix. Args: config_values_or_plane (comma-separated :obj:`string`, or :class:`Configuration` instance, or a Rhino :obj:`Plane`). """ if not config_values_or_plane: return None try: return Pose.from_list(vrep_pose_from_plane(config_values_or_plane)) except (TypeError, IndexError): try: if config_values_or_plane.external_axes and config_values_or_plane.joint_values: return config_values_or_plane except AttributeError: values = map(float, config_values_or_plane.split(',')) return Configuration.from_degrees_list(values)
loader = RosFileServerLoader(client) urdf = loader.load_urdf() srdf = loader.load_srdf() model = RobotModel.from_urdf_string(urdf) semantics = RobotSemantics.from_srdf_string(srdf, model) robot = Robot(model, semantics=semantics, client=client) group = robot.main_group_name frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) tolerance_position = 0.001 tolerance_axes = [math.radians(1)] * 3 start_configuration = Configuration.from_revolute_values( (0.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571)) # create goal constraints from frame goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerance_axes, group) trajectory = robot.plan_motion(goal_constraints, start_configuration, group, planner_id='RRT') print("Computed kinematic path with %d configurations." % len(trajectory.points)) print( "Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
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
import math from compas.geometry import Frame from compas_fab.backends import RosClient from compas_fab.robots import Configuration with RosClient('localhost') as client: robot = client.load_robot() group = robot.main_group_name frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1]) tolerance_position = 0.001 tolerance_axes = [math.radians(1)] * 3 start_configuration = Configuration.from_revolute_values( [-0.042, 4.295, 0, -3.327, 4.755, 0.]) # create goal constraints from frame goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerance_axes, group) trajectory = robot.plan_motion(goal_constraints, start_configuration, group, options=dict(planner_id='RRT')) print("Computed kinematic path with %d configurations." % len(trajectory.points)) print( "Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
def test_prismatic_revolute_ctor(): config = Configuration.from_prismatic_and_revolute_values( [8.312], [1.5, 0., 0., 0., 1., 0.8]) assert config.values == [8.312, 1.5, 0., 0., 0., 1., 0.8] assert config.types == [Joint.PRISMATIC] + [Joint.REVOLUTE] * 6
""" 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()
from compas.geometry import Frame from compas_fab.backends import RosClient from compas_fab.robots import Configuration with RosClient('localhost') as client: robot = client.load_robot() group = robot.main_group_name frames = [] frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])) frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0])) start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000]) trajectory = robot.plan_cartesian_motion(frames, start_configuration, group=group, options=dict( max_step=0.01, avoid_collisions=True, )) print("Computed cartesian path with %d configurations, " % len(trajectory.points)) print("following %d%% of requested trajectory." % (trajectory.fraction * 100)) print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
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
import os from compas.geometry import Frame from compas.robots import LocalPackageMeshLoader from compas.robots import RobotModel from compas_fab.robots import Configuration from ur_kinematics import inverse_kinematics_ur5 models_path = os.path.join(os.path.dirname(__file__), 'models') loader = LocalPackageMeshLoader(models_path, 'ur_description') model = RobotModel.from_urdf_file(loader.load_urdf('ur5.urdf')) f = Frame((0.417, 0.191, -0.005), (-0.000, 1.000, 0.000), (1.000, 0.000, 0.000)) f.point /= 0.001 sols = inverse_kinematics_ur5(f) for joint_values in sols: joint_names = model.get_configurable_joint_names() print(Configuration.from_revolute_values(joint_values, joint_names))
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_from_data(): config = Configuration.from_data( dict(values=[8.312, 1.5], types=[Joint.PRISMATIC, Joint.REVOLUTE])) assert str(config) == 'Configuration((8.312, 1.500), (2, 0))'
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)
# 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)