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 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 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 __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}" )
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)
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_circle_cartesian(fixed_waam_setup, viewer, planner_ik_conf): urdf_filename, semantics, robotA_tool = fixed_waam_setup planner_id, ik_engine = planner_ik_conf move_group = 'robotA' print('\n') print('=' * 10) cprint( 'Cartesian planner {} with IK engine {}'.format(planner_id, ik_engine), 'yellow') with PyChoreoClient(viewer=viewer) as client: robot = client.load_robot(urdf_filename) robot.semantics = semantics robot_uid = client.get_robot_pybullet_uid(robot) base_link_name = robot.get_base_link_name(group=move_group) ik_joint_names = robot.get_configurable_joint_names(group=move_group) tool_link_name = robot.get_end_effector_link_name(group=move_group) base_frame = robot.get_base_frame(group=move_group) if ik_engine == 'default-single': ik_solver = None elif ik_engine == 'lobster-analytical': ik_solver = InverseKinematicsSolver(robot, move_group, ik_abb_irb4600_40_255, base_frame, robotA_tool.frame) elif ik_engine == 'ikfast-analytical': ikfast_fn = get_ik_fn_from_ikfast(ikfast_abb_irb4600_40_255.get_ik) ik_solver = InverseKinematicsSolver(robot, move_group, ikfast_fn, base_frame, robotA_tool.frame) else: raise ValueError('invalid ik engine name.') init_conf = Configuration.from_revolute_values(np.zeros(6), ik_joint_names) # replace default ik function with a customized one if ik_solver is not None: client.planner.inverse_kinematics = ik_solver.inverse_kinematics_function( ) tool_link = link_from_name(robot_uid, tool_link_name) robot_base_link = link_from_name(robot_uid, base_link_name) ik_joints = joints_from_names(robot_uid, ik_joint_names) # * draw EE pose tcp_pose = get_link_pose(robot_uid, tool_link) draw_pose(tcp_pose) # * generate multiple circles circle_center = np.array([2, 0, 0.2]) circle_r = 0.2 # full_angle = np.pi # full_angle = 2*2*np.pi angle_range = (-0.5 * np.pi, 0.5 * np.pi) # total num of path pts, one path point per 5 degree ee_poses = compute_circle_path(circle_center, circle_r, angle_range) ee_frames_WCF = [frame_from_pose(ee_pose) for ee_pose in ee_poses] options = {'planner_id': planner_id} if planner_id == 'LadderGraph': client.set_robot_configuration(robot, init_conf) st_time = time.time() # options.update({'ik_function' : lambda pose: compute_inverse_kinematics(ikfast_abb_irb4600_40_255.get_ik, pose, sampled=[])}) trajectory = client.plan_cartesian_motion(robot, ee_frames_WCF, group=move_group, options=options) cprint( 'W/o frame variant solving time: {}'.format( elapsed_time(st_time)), 'blue') cprint( 'Cost: {}'.format( compute_trajectory_cost(trajectory, init_conf_val=init_conf.values)), 'blue') print('-' * 5) f_variant_options = {'delta_yaw': np.pi / 3, 'yaw_sample_size': 5} options.update({ 'frame_variant_generator': PyChoreoFiniteEulerAngleVariantGenerator( options=f_variant_options) }) print('With frame variant config: {}'.format(f_variant_options)) client.set_robot_configuration(robot, init_conf) st_time = time.time() trajectory = client.plan_cartesian_motion(robot, ee_frames_WCF, group=move_group, options=options) cprint( '{} solving time: {}'.format( 'With frame variant ' if planner_id == 'LadderGraph' else 'Direct', elapsed_time(st_time)), 'cyan') cprint( 'Cost: {}'.format( compute_trajectory_cost(trajectory, init_conf_val=init_conf.values)), 'cyan') if trajectory is None: cprint( 'Client Cartesian planner {} CANNOT find a plan!'.format( planner_id), 'red') else: cprint( 'Client Cartesian planning {} find a plan!'.format(planner_id), '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()
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)
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)
""" 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)
# - 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) print(full_cfg)
def joint_angles_to_configurations(A1, A2, A3, A4, A5, A6): return [ Configuration.from_revolute_values([a1, a2, a3, a4, a5, a6]) for a1, a2, a3, a4, a5, a6 in zip(A1, A2, A3, A4, A5, A6) ]
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 forward_kinematics(self, configuration): q = configuration.values[:] q[5] += math.pi return super(UR3, self).forward_kinematics( Configuration.from_revolute_values(q))
Configuration.from_revolute_values(q)) def inverse_kinematics(self, tool0_frame_RCS): configurations = super(UR3, self).inverse_kinematics(tool0_frame_RCS) for q in configurations: print(q) for i in range(len(configurations)): configurations[i].values[5] -= math.pi return configurations if __name__ == "__main__": import math from compas_fab.utilities import sign from compas.geometry import Frame from .kinematics import format_joint_positions ur = UR3() q = [-0.44244, -1.5318, 1.34588, -1.38512, -1.05009, -0.4495] q = Configuration.from_revolute_values(q) Ts = ur.get_forward_transformations(q) for T in Ts: print(T) print() frame = ur.forward_kinematics(q) qsols = ur.inverse_kinematics(frame) for q in qsols: print(q) ur.get_transformed_model(Ts)
# create tool from json filepath = os.path.join(DATA, "vacuum_gripper.json") tool = Tool.from_json(filepath) element_height = 0.014 safelevel_height = 0.05 with RosClient('localhost') as client: robot = client.load_robot() # 1. Set tool robot.attach_tool(tool) # 2. Define start configuration start_configuration = Configuration.from_revolute_values( [-5.961, 4.407, -2.265, 5.712, 1.571, -2.820]) # 3. Define frames picking_frame = Frame([-0.43, 0, element_height], [1, 0, 0], [0, 1, 0]) safelevel_picking_frame = Frame( [-0.43, 0, element_height + safelevel_height], [1, 0, 0], [0, 1, 0]) frames = [picking_frame, safelevel_picking_frame] # 4. Convert frames to tool0_frames frames_tool0 = robot.from_tcf_to_t0cf(frames) trajectory = robot.plan_cartesian_motion(frames_tool0, start_configuration, options=dict(max_step=0.01)) print("Computed cartesian path with %d configurations, " %
from compas.geometry import Frame from compas.datastructures import Mesh from compas_fab.robots import Configuration from compas_fab.backends import RosClient HERE = os.path.dirname(__file__) DATA = os.path.join(HERE, '../data') PATH = os.path.join(DATA, 'cylinder.obj') robot.client = RosClient() robot.client.run() goal_frame = Frame([0.20, 0.38, 0.32], [0, 1, 0], [0, 0, 1]) start_configuration = Configuration.from_revolute_values((-0.042, -1.988, 2.174, -3.327, -1.528, -6.283)) group = robot.main_group_name # Create attached collision object cylinder = Mesh.from_obj(PATH) aco = robot.create_collision_mesh_attached_to_end_effector('cylinder', cylinder, group) response = robot.motion_plan_goal_frame(goal_frame, start_configuration, tolerance_position=0.001, tolerance_angle=math.radians(1), group=group, path_constraints=None, planner_id='RRT', num_planning_attempts=20, allowed_planning_time=8.,