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.joint_types == [Joint.REVOLUTE] * 6 config = Configuration.from_revolute_values([math.pi / 2, 0., 0.]) assert [math.degrees(value) for value in config.joint_values] == [90, 0, 0]
def trj(): p1 = JointTrajectoryPoint([1.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6, time_from_start=Duration(2, 1293)) p2 = JointTrajectoryPoint([0.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6, time_from_start=Duration(6, 0)) config = Configuration.from_revolute_values([0.] * 6) return JointTrajectory(trajectory_points=[p1, p2], joint_names=[ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6' ], start_configuration=config)
import math from compas.geometry import Frame from compas.robots import Configuration from compas_fab.backends import RosClient with RosClient() as client: robot = client.load_robot() assert robot.name == 'ur5' 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.]) group = robot.main_group_name # 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='RRTstarkConfigDefault')) print("Computed kinematic path with %d configurations." % len(trajectory.points)) print( "Executing this path at full speed would take approx. %.3f seconds." %
from compas_fab.robots import CollisionMesh, AttachedCollisionMesh with RosClient() as client: robot = client.load_robot() assert robot.name == 'ur5' ee_link_name = robot.get_end_effector_link_name() mesh = Mesh.from_stl(compas_fab.get('planning_scene/cone.stl')) cm = CollisionMesh(mesh, 'tip') acm = AttachedCollisionMesh(cm, link_name=ee_link_name) client.add_attached_collision_mesh(acm) 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]) options = { 'max_step': 0.01, 'avoid_collisions': True, } trajectory = robot.plan_cartesian_motion(frames, start_configuration, options=options) 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) print("Path plan computed with the attached collision meshes: {}".format( [acm.collision_mesh.id for acm in trajectory.attached_collision_meshes]))
from compas.robots import Configuration from compas_fab.robots.ur5 import Robot robot = Robot() 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) assert repr(frame_WCF.point) == 'Point(0.300, 0.100, 0.500)'
def joint_angles_to_configurations(robot, solutions, group=None): joint_names = robot.get_configurable_joint_names(group=group) return [ Configuration.from_revolute_values(q, joint_names=joint_names) if q else None for q in solutions ]